├── .clang-format ├── .clang-tidy ├── .gitignore ├── CMakeLists.txt ├── README.md ├── cmake └── FindCPM.cmake ├── examples ├── CMakeLists.txt ├── bicycle_5d.c ├── bicycle_5d.h ├── bicycle_example.c ├── data │ ├── back_pass_data.h │ ├── forward_pass_data.h │ ├── lqr_lti_track_data.h │ └── lqr_ltv_data.h ├── planar_quad_mpc_example.c ├── planar_quadrotor.c └── planar_quadrotor.h ├── src └── tinympc │ ├── CMakeLists.txt │ ├── constraint_linear.c │ ├── constraint_linear.h │ ├── cost_lqr.c │ ├── cost_lqr.h │ ├── data_struct.c │ ├── data_struct.h │ ├── dynamics_lti.c │ ├── dynamics_lti.h │ ├── dynamics_ltv.c │ ├── dynamics_ltv.h │ ├── lqr_lti.c │ ├── lqr_lti.h │ ├── lqr_ltv.c │ ├── lqr_ltv.h │ ├── mpc_lti.c │ ├── mpc_lti.h │ ├── mpc_ltv.c │ ├── mpc_ltv.h │ ├── tinympc.h │ ├── utils.c │ └── utils.h └── test ├── CMakeLists.txt ├── al_lqr_lti_test.cpp ├── al_lqr_ltv_test.cpp ├── back_pass_test.cpp ├── cost_test.cpp ├── data ├── back_pass_data.h ├── forward_pass_data.h ├── lqr_lti_track_data.h └── lqr_ltv_data.h ├── forward_pass_test.cpp ├── lqr_ineq_utils_test.cpp ├── lqr_lti_test.cpp ├── lqr_lti_track_test.cpp ├── lqr_ltv_test.cpp ├── lqr_ltv_track_test.cpp ├── lqrdata_test.cpp ├── models ├── bicycle_5d.c ├── bicycle_5d.h ├── planar_quadrotor.c └── planar_quadrotor.h ├── test_all.sh ├── test_utils.c └── test_utils.h /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignArrayOfStructures: None 7 | AlignConsecutiveMacros: None 8 | AlignConsecutiveAssignments: None 9 | AlignConsecutiveBitFields: None 10 | AlignConsecutiveDeclarations: None 11 | AlignEscapedNewlines: Left 12 | AlignOperands: Align 13 | AlignTrailingComments: true 14 | AllowAllArgumentsOnNextLine: true 15 | AllowAllParametersOfDeclarationOnNextLine: true 16 | AllowShortEnumsOnASingleLine: true 17 | AllowShortBlocksOnASingleLine: Never 18 | AllowShortCaseLabelsOnASingleLine: false 19 | AllowShortFunctionsOnASingleLine: All 20 | AllowShortLambdasOnASingleLine: All 21 | AllowShortIfStatementsOnASingleLine: WithoutElse 22 | AllowShortLoopsOnASingleLine: true 23 | AlwaysBreakAfterDefinitionReturnType: None 24 | AlwaysBreakAfterReturnType: None 25 | AlwaysBreakBeforeMultilineStrings: true 26 | AlwaysBreakTemplateDeclarations: Yes 27 | AttributeMacros: 28 | - __capability 29 | BinPackArguments: true 30 | BinPackParameters: true 31 | BraceWrapping: 32 | AfterCaseLabel: false 33 | AfterClass: false 34 | AfterControlStatement: Never 35 | AfterEnum: false 36 | AfterFunction: false 37 | AfterNamespace: false 38 | AfterObjCDeclaration: false 39 | AfterStruct: false 40 | AfterUnion: false 41 | AfterExternBlock: false 42 | BeforeCatch: false 43 | BeforeElse: false 44 | BeforeLambdaBody: false 45 | BeforeWhile: false 46 | IndentBraces: false 47 | SplitEmptyFunction: true 48 | SplitEmptyRecord: true 49 | SplitEmptyNamespace: true 50 | BreakBeforeBinaryOperators: None 51 | BreakBeforeConceptDeclarations: true 52 | BreakBeforeBraces: Attach 53 | BreakBeforeInheritanceComma: false 54 | BreakInheritanceList: BeforeColon 55 | BreakBeforeTernaryOperators: true 56 | BreakConstructorInitializersBeforeComma: false 57 | BreakConstructorInitializers: BeforeColon 58 | BreakAfterJavaFieldAnnotations: false 59 | BreakStringLiterals: true 60 | ColumnLimit: 80 61 | CommentPragmas: '^ IWYU pragma:' 62 | QualifierAlignment: Leave 63 | CompactNamespaces: false 64 | ConstructorInitializerIndentWidth: 4 65 | ContinuationIndentWidth: 4 66 | Cpp11BracedListStyle: true 67 | DeriveLineEnding: true 68 | DerivePointerAlignment: true 69 | DisableFormat: false 70 | EmptyLineAfterAccessModifier: Never 71 | EmptyLineBeforeAccessModifier: LogicalBlock 72 | ExperimentalAutoDetectBinPacking: false 73 | PackConstructorInitializers: NextLine 74 | BasedOnStyle: '' 75 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 76 | AllowAllConstructorInitializersOnNextLine: true 77 | FixNamespaceComments: true 78 | ForEachMacros: 79 | - foreach 80 | - Q_FOREACH 81 | - BOOST_FOREACH 82 | IfMacros: 83 | - KJ_IF_MAYBE 84 | IncludeBlocks: Regroup 85 | IncludeCategories: 86 | - Regex: '^' 87 | Priority: 2 88 | SortPriority: 0 89 | CaseSensitive: false 90 | - Regex: '^<.*\.h>' 91 | Priority: 1 92 | SortPriority: 0 93 | CaseSensitive: false 94 | - Regex: '^<.*' 95 | Priority: 2 96 | SortPriority: 0 97 | CaseSensitive: false 98 | - Regex: '.*' 99 | Priority: 3 100 | SortPriority: 0 101 | CaseSensitive: false 102 | IncludeIsMainRegex: '([-_](test|unittest))?$' 103 | IncludeIsMainSourceRegex: '' 104 | IndentAccessModifiers: false 105 | IndentCaseLabels: true 106 | IndentCaseBlocks: false 107 | IndentGotoLabels: true 108 | IndentPPDirectives: None 109 | IndentExternBlock: AfterExternBlock 110 | IndentRequires: false 111 | IndentWidth: 2 112 | IndentWrappedFunctionNames: false 113 | InsertTrailingCommas: None 114 | JavaScriptQuotes: Leave 115 | JavaScriptWrapImports: true 116 | KeepEmptyLinesAtTheStartOfBlocks: false 117 | LambdaBodyIndentation: Signature 118 | MacroBlockBegin: '' 119 | MacroBlockEnd: '' 120 | MaxEmptyLinesToKeep: 1 121 | NamespaceIndentation: None 122 | ObjCBinPackProtocolList: Never 123 | ObjCBlockIndentWidth: 2 124 | ObjCBreakBeforeNestedBlockParam: true 125 | ObjCSpaceAfterProperty: false 126 | ObjCSpaceBeforeProtocolList: true 127 | PenaltyBreakAssignment: 2 128 | PenaltyBreakBeforeFirstCallParameter: 1 129 | PenaltyBreakComment: 300 130 | PenaltyBreakFirstLessLess: 120 131 | PenaltyBreakOpenParenthesis: 0 132 | PenaltyBreakString: 1000 133 | PenaltyBreakTemplateDeclaration: 10 134 | PenaltyExcessCharacter: 1000000 135 | PenaltyReturnTypeOnItsOwnLine: 200 136 | PenaltyIndentedWhitespace: 0 137 | PointerAlignment: Left 138 | PPIndentWidth: -1 139 | RawStringFormats: 140 | - Language: Cpp 141 | Delimiters: 142 | - cc 143 | - CC 144 | - cpp 145 | - Cpp 146 | - CPP 147 | - 'c++' 148 | - 'C++' 149 | CanonicalDelimiter: '' 150 | BasedOnStyle: google 151 | - Language: TextProto 152 | Delimiters: 153 | - pb 154 | - PB 155 | - proto 156 | - PROTO 157 | EnclosingFunctions: 158 | - EqualsProto 159 | - EquivToProto 160 | - PARSE_PARTIAL_TEXT_PROTO 161 | - PARSE_TEST_PROTO 162 | - PARSE_TEXT_PROTO 163 | - ParseTextOrDie 164 | - ParseTextProtoOrDie 165 | - ParseTestProto 166 | - ParsePartialTestProto 167 | CanonicalDelimiter: pb 168 | BasedOnStyle: google 169 | ReferenceAlignment: Pointer 170 | ReflowComments: true 171 | RemoveBracesLLVM: false 172 | SeparateDefinitionBlocks: Leave 173 | ShortNamespaceLines: 1 174 | SortIncludes: CaseSensitive 175 | SortJavaStaticImport: Before 176 | SortUsingDeclarations: true 177 | SpaceAfterCStyleCast: false 178 | SpaceAfterLogicalNot: false 179 | SpaceAfterTemplateKeyword: true 180 | SpaceBeforeAssignmentOperators: true 181 | SpaceBeforeCaseColon: false 182 | SpaceBeforeCpp11BracedList: false 183 | SpaceBeforeCtorInitializerColon: true 184 | SpaceBeforeInheritanceColon: true 185 | SpaceBeforeParens: ControlStatements 186 | SpaceBeforeParensOptions: 187 | AfterControlStatements: true 188 | AfterForeachMacros: true 189 | AfterFunctionDefinitionName: false 190 | AfterFunctionDeclarationName: false 191 | AfterIfMacros: true 192 | AfterOverloadedOperator: false 193 | BeforeNonEmptyParentheses: false 194 | SpaceAroundPointerQualifiers: Default 195 | SpaceBeforeRangeBasedForLoopColon: true 196 | SpaceInEmptyBlock: false 197 | SpaceInEmptyParentheses: false 198 | SpacesBeforeTrailingComments: 2 199 | SpacesInAngles: Never 200 | SpacesInConditionalStatement: false 201 | SpacesInContainerLiterals: true 202 | SpacesInCStyleCastParentheses: false 203 | SpacesInLineCommentPrefix: 204 | Minimum: 1 205 | Maximum: -1 206 | SpacesInParentheses: false 207 | SpacesInSquareBrackets: false 208 | SpaceBeforeSquareBrackets: false 209 | BitFieldColonSpacing: Both 210 | Standard: Auto 211 | StatementAttributeLikeMacros: 212 | - Q_EMIT 213 | StatementMacros: 214 | - Q_UNUSED 215 | - QT_REQUIRE_VERSION 216 | TabWidth: 8 217 | UseCRLF: false 218 | UseTab: Never 219 | WhitespaceSensitiveMacros: 220 | - STRINGIZE 221 | - PP_STRINGIZE 222 | - BOOST_PP_STRINGIZE 223 | - NS_SWIFT_NAME 224 | - CF_SWIFT_NAME 225 | ... 226 | 227 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | Checks: 'clang-diagnostic-*,clang-analyzer-*,bugprone=*,google-*,readability-*,performance*,portability-*' 3 | WarningsAsErrors: '' 4 | HeaderFilterRegex: '' 5 | AnalyzeTemporaryDtors: false 6 | FormatStyle: none 7 | User: brian 8 | CheckOptions: 9 | - key: llvm-else-after-return.WarnOnConditionVariables 10 | value: 'false' 11 | - key: modernize-loop-convert.MinConfidence 12 | value: reasonable 13 | - key: modernize-replace-auto-ptr.IncludeStyle 14 | value: llvm 15 | - key: cert-str34-c.DiagnoseSignedUnsignedCharComparisons 16 | value: 'false' 17 | - key: google-readability-namespace-comments.ShortNamespaceLines 18 | value: '10' 19 | - key: cert-oop54-cpp.WarnOnlyIfThisHasSuspiciousField 20 | value: 'false' 21 | - key: cppcoreguidelines-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic 22 | value: 'true' 23 | - key: cert-dcl16-c.NewSuffixes 24 | value: 'L;LL;LU;LLU' 25 | - key: google-readability-braces-around-statements.ShortStatementLines 26 | value: '1' 27 | - key: modernize-pass-by-value.IncludeStyle 28 | value: llvm 29 | - key: google-readability-namespace-comments.SpacesBeforeComments 30 | value: '2' 31 | - key: modernize-loop-convert.MaxCopySize 32 | value: '16' 33 | - key: cppcoreguidelines-explicit-virtual-functions.IgnoreDestructors 34 | value: 'true' 35 | - key: modernize-use-nullptr.NullMacros 36 | value: 'NULL' 37 | - key: llvm-qualified-auto.AddConstToQualified 38 | value: 'false' 39 | - key: modernize-loop-convert.NamingStyle 40 | value: CamelCase 41 | - key: llvm-else-after-return.WarnOnUnfixable 42 | value: 'false' 43 | - key: google-readability-function-size.StatementThreshold 44 | value: '800' 45 | - key: clang-analyzer-security.insecureAPI.DeprecatedOrUnsafeBufferHandling 46 | value: 'false' 47 | ... 48 | 49 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .cache/ 2 | .idea/ 3 | build/ 4 | cmake-build* 5 | docs/Doxyfile # this gets ignored since it's generated by the build system 6 | venv 7 | .DS_Store 8 | .ipynb_checkpoints/ 9 | .vscode 10 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################## 2 | # Preamble 3 | ############################## 4 | cmake_minimum_required(VERSION 3.2) 5 | list(APPEND CMAKE_MESSAGE_CONTEXT tinympc) 6 | project(tinyMPC VERSION 0.1 LANGUAGES C CXX) 7 | 8 | ############################## 9 | # Project wide 10 | ############################## 11 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 12 | set(CMAKE_C_STANDARD 11) 13 | set(CMAKE_C_STANDARD_REQUIRED ON) 14 | set(CMAKE_C_EXTENSIONS NO) 15 | 16 | # include(CMakePrintHelpers) 17 | # include(GNUInstallDirs) 18 | # include(Functions) 19 | include(FetchContent) 20 | 21 | # Add CPM Dependency Manager 22 | include(FindCPM) 23 | 24 | # Handle default build type 25 | set(TINY_DEFAULT_BUILD_TYPE "Debug") 26 | if (NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 27 | message(STATUS "No build type specified. Setting CMAKE_BUILD_TYPE to ${TINY_DEFAULT_BUILD_TYPE}") 28 | set(CMAKE_BUILD_TYPE ${TINY_DEFAULT_BUILD_TYPE} CACHE STRING "Choose the type of build." FORCE) 29 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 30 | endif () 31 | 32 | # Enable testing 33 | option(TINY_BUILD_TESTS "Build tests for TinyMPC" ON) 34 | 35 | # Enable clang-tidy analysis 36 | option(TINY_CLANG_TIDY "Run clang-tidy analyzer on the source code." OFF) 37 | 38 | # Floating point precision 39 | set(TINY_FLOAT double CACHE STRING "Floating point precision for TinyMPC (float,double).") 40 | 41 | # Build examples 42 | option(TINY_BUILD_EXAMPLES "Build examples for TinyMPC." ON) 43 | 44 | # Build with -march=native 45 | option(TINY_VECTORIZE "Compile with -march=native" OFF) 46 | 47 | ############################## 48 | # Dependencies 49 | ############################## 50 | 51 | # slap 52 | if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) 53 | FetchContent_Declare(slap 54 | GIT_REPOSITORY https://github.com/bjack205/slap 55 | GIT_TAG main 56 | ) 57 | set(SLAP_BUILD_TESTS OFF CACHE BOOL "" FORCE) 58 | FetchContent_MakeAvailable(slap) 59 | endif () 60 | 61 | # Google Test 62 | if (TINY_BUILD_TESTS) 63 | CPMAddPackage( 64 | NAME googletest 65 | GITHUB_REPOSITORY google/googletest 66 | VERSION 1.13.0 67 | DOWNLOAD_EXTRACT_TIMESTAMP 68 | OPTIONS 69 | "INSTALL_GTEST OFF" 70 | "gtest_force_shared_crt ON" 71 | ) 72 | add_library(gtest::gtest ALIAS gtest_main) 73 | enable_testing() 74 | include(GoogleTest) 75 | include(CTest) 76 | endif () 77 | 78 | ############################## 79 | # Main build targets 80 | ############################## 81 | # Compile options 82 | if (NOT WIN32) 83 | add_compile_options(-Wall -Wextra -pedantic -Werror -Wno-error=unknown-pragmas) 84 | # add_compile_options(-Wformat=2 -Wno-unused-parameter -Wshadow 85 | # -Wwrite-strings -Wstrict-prototypes -Wold-style-definition 86 | # -Wredundant-decls -Wnested-externs -Wmissing-include-dirs) 87 | if (TINY_VECTORIZE) 88 | add_compile_options(-march=native) 89 | endif() 90 | endif() 91 | 92 | # if (CMAKE_C_COMPILER_ID MATCHES "GNU") 93 | # add_compile_options(-Wjump-misses-init -Wlogical-op) 94 | # endif() 95 | 96 | # TODO: Future options to optimize library for specific use of constraints 97 | option(SET_GOAL_CONSTRAINT "Set goal constraint" OFF) 98 | if (SET_GOAL_CONSTRAINT) 99 | add_definitions(-DGOAL_CONSTRAINT) 100 | endif() 101 | option(SET_INPUT_CONSTRAINT "Set input constraints" ON) 102 | if (SET_INPUT_CONSTRAINT) 103 | add_definitions(-DINPUT_CONSTRAINT) 104 | endif() 105 | option(SET_STATE_CONSTRAINT "Set state constraints" ON) 106 | if (SET_STATE_CONSTRAINT) 107 | add_definitions(-DSTATE_CONSTRAINT) 108 | endif() 109 | 110 | # Make all includes relative to src/ directory 111 | include_directories(${PROJECT_SOURCE_DIR}/src) 112 | 113 | # Build source files 114 | add_subdirectory(src/tinympc) 115 | 116 | ############################## 117 | # Tests 118 | ############################## 119 | if (TINY_BUILD_TESTS) 120 | add_subdirectory(test) 121 | endif() 122 | 123 | ############################## 124 | # Examples 125 | ############################## 126 | if (TINY_BUILD_EXAMPLES) 127 | add_subdirectory(examples) 128 | endif() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TinyMPC-AL 2 | Under Development 3 | 4 | This is a previous attempt of TinyMPC that uses augmented Lagrange method and solve TVLQR online (applicable to nonlinear dynamics). No tricks are played here so it is slow. 5 | 6 | New developments are at [https://tinympc.org/](https://tinympc.org/) 7 | 8 | ## Descriptions 9 | 10 | - This is a full library, embedded (optimized) version is under development. 11 | However, it aims at highly modular integration. You can just use part of the 12 | sources at your need. Currently, assertion is not present. Users should be 13 | responsible for this during development. 14 | - AL-TVLQR is ready to use. It is able to handle input/state box constraints and 15 | goal constraint within stabilization or tracking problems for LTI and LTV 16 | systems. Check `test/al_lqr_test` for all tests, experiments and examples. Check 17 | `examples` for MPC experiments. 18 | - You can set constraints on/off in `CMakeLists.txt`, more are under development. 19 | - AL-iLQR is under development. It is able to handle input/state box constraints 20 | and goal constraint within stabilization or tracking problems for nonlinear 21 | systems. 22 | 23 | ## How to compile and run (tested on Linux) 24 | 25 | 1. Clone this repo 26 | 27 | ```bash 28 | git clone https://github.com/RoboticExplorationLab/TinyMPC-AL.git 29 | ``` 30 | 31 | 2. Build the source code 32 | 33 | ```bash 34 | cd TinyMPC-AL 35 | cmake -S. -Bbuild 36 | cd build 37 | make 38 | ``` 39 | 40 | If using Windows outside of WSL, run 41 | 42 | ``` 43 | cmake --build . 44 | ``` 45 | 46 | instead of `make`. 47 | 48 | 3. Run the bicycle example 49 | 50 | ```bash 51 | ./examples/bicycle_example 52 | ``` 53 | 54 | ## Notes 55 | 56 | - Use `slap_MatMulAdd(C, A, B, 1, 0)` instead of `slap_MatMulAB(C, A, B)` 57 | because the later one ignores all metadata. 58 | - Can use `slap_MatrixAddition(C, C, A, alp)` to bias `C = C + alp*A`. 59 | - Should use zero-initialization of array in global scope. 60 | - Should pass by reference instead of return type 61 | - Linear term q, qf, r come from reference trajectories, ie. q = -Q*xref 62 | - MPC for LTI systems can handel all provided types of constraints. 63 | - Tracking MPC for LTV systems may not handle all due to the strictness. 64 | 65 | ## Done 66 | 67 | - Augmented Lagrangian LQR/TVLQR and MPC. 68 | - Successful unit and integration testing. 69 | - Experiments on double integrator, planar quadrotor and bicycle model. 70 | -------------------------------------------------------------------------------- /cmake/FindCPM.cmake: -------------------------------------------------------------------------------- 1 | set(CPM_DOWNLOAD_VERSION 0.34.0) 2 | 3 | if(CPM_SOURCE_CACHE) 4 | set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 5 | elseif(DEFINED ENV{CPM_SOURCE_CACHE}) 6 | set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 7 | else() 8 | set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") 9 | endif() 10 | 11 | if(NOT (EXISTS ${CPM_DOWNLOAD_LOCATION})) 12 | message(STATUS "Downloading CPM.cmake to ${CPM_DOWNLOAD_LOCATION}") 13 | file(DOWNLOAD 14 | https://github.com/TheLartians/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake 15 | ${CPM_DOWNLOAD_LOCATION} 16 | ) 17 | endif() 18 | 19 | include(${CPM_DOWNLOAD_LOCATION}) -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(add_example name) 2 | set(EXAMPLE_NAME ${name}_example) 3 | add_executable(${EXAMPLE_NAME} 4 | ${EXAMPLE_NAME}.c 5 | ${ARGN} 6 | ) 7 | target_link_libraries(${EXAMPLE_NAME} 8 | PRIVATE 9 | slap::slap 10 | tinympc::tinympc 11 | gtest::gtest 12 | ) 13 | endfunction() 14 | 15 | add_example(bicycle bicycle_5d.h bicycle_5d.c) 16 | # add_example(planar_quad_mpc planar_quadrotor.h planar_quadrotor.c bicycle_5d.h bicycle_5d.c) 17 | -------------------------------------------------------------------------------- /examples/bicycle_5d.c: -------------------------------------------------------------------------------- 1 | #include "bicycle_5d.h" 2 | 3 | //======================================== 4 | // Bicycle model parameters 5 | //======================================== 6 | // struct tiny_Model_Bicycle { 7 | // double drive_min[2]; 8 | // double drive_max[2]; 9 | // double u_min[2]; 10 | // double u_max[2]; 11 | // } tiny_DefaultModel_Bicycle = {{-2, -0.5}, {2, 0.5}, {-4, -0.7}, {4, 0.7}}; 12 | 13 | //======================================== 14 | // Codes generated from julia/bicycle_tvlqr 15 | // Discrete dynamics of bicycle model with predefined model params 16 | //======================================== 17 | void tiny_Bicycle5dNonlinearDynamics_Raw(double* xn, const double* x, 18 | const double* u) { 19 | xn[0] = 20 | 0.16666666666666666 * 21 | (0.1 * (0.1 * u[0] + x[3]) * 22 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 23 | x[2]) + 24 | 0.1 * cos(x[2]) * x[3] + 25 | 0.2 * (0.05 * u[0] + x[3]) * 26 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 27 | x[2]) + 28 | 0.2 * (0.05 * u[0] + x[3]) * cos(0.05 * tan(x[4]) * x[3] + x[2])) + 29 | x[0]; 30 | xn[1] = 31 | 0.16666666666666666 * 32 | (0.2 * (0.05 * u[0] + x[3]) * 33 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 34 | x[2]) + 35 | 0.2 * (0.05 * u[0] + x[3]) * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 36 | 0.1 * (0.1 * u[0] + x[3]) * 37 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 38 | x[2]) + 39 | 0.1 * sin(x[2]) * x[3]) + 40 | x[1]; 41 | xn[2] = 0.16666666666666666 * 42 | (0.1 * (0.1 * u[0] + x[3]) * tan(0.1 * u[1] + x[4]) + 43 | 0.4 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 44 | 0.1 * tan(x[4]) * x[3]) + 45 | x[2]; 46 | xn[3] = 0.1 * u[0] + x[3]; 47 | xn[4] = 0.1 * u[1] + x[4]; 48 | } 49 | 50 | void tiny_Bicycle5dNonlinearDynamics(Matrix* xn, const Matrix x, 51 | const Matrix u) { 52 | tiny_Bicycle5dNonlinearDynamics_Raw(xn->data, x.data, u.data); 53 | } 54 | 55 | //======================================== 56 | // Codes generated from julia/bicycle_tvlqr 57 | // Jacobians of discrete dynamics of bicycle model with predefined model params 58 | //======================================== 59 | void tiny_Bicycle5dGetJacobianA_Raw(double* A, const double* x, 60 | const double* u) { 61 | A[0] = 1; 62 | A[1] = 0; 63 | A[2] = 0; 64 | A[3] = 0; 65 | A[4] = 0; 66 | A[5] = 0; 67 | A[6] = 1; 68 | A[7] = 0; 69 | A[8] = 0; 70 | A[9] = 0; 71 | A[10] = 72 | 0.16666666666666666 * 73 | (-0.2 * (0.05 * u[0] + x[3]) * 74 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 75 | -0.2 * (0.05 * u[0] + x[3]) * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 76 | -0.1 * (0.1 * u[0] + x[3]) * 77 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 78 | -0.1 * sin(x[2]) * x[3]); 79 | A[11] = 80 | 0.16666666666666666 * 81 | (0.1 * (0.1 * u[0] + x[3]) * 82 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 83 | 0.1 * cos(x[2]) * x[3] + 84 | 0.2 * (0.05 * u[0] + x[3]) * 85 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 86 | 0.2 * (0.05 * u[0] + x[3]) * cos(0.05 * tan(x[4]) * x[3] + x[2])); 87 | A[12] = 1; 88 | A[13] = 0; 89 | A[14] = 0; 90 | A[15] = 91 | 0.16666666666666666 * 92 | (0.2 * cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 93 | 0.2 * cos(0.05 * tan(x[4]) * x[3] + x[2]) + 94 | 0.1 * cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 95 | 0.1 * cos(x[2]) + 96 | -0.01 * (0.05 * u[0] + x[3]) * sin(0.05 * tan(x[4]) * x[3] + x[2]) * 97 | tan(x[4]) + 98 | -0.01 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 99 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 100 | -0.01 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 101 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 102 | A[16] = 103 | 0.16666666666666666 * 104 | (0.2 * sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 105 | 0.2 * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 106 | 0.1 * sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 107 | 0.1 * sin(x[2]) + 108 | 0.01 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 109 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 110 | 0.01 * (0.05 * u[0] + x[3]) * 111 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) * 112 | tan(0.05 * u[1] + x[4]) + 113 | 0.01 * (0.05 * u[0] + x[3]) * cos(0.05 * tan(x[4]) * x[3] + x[2]) * 114 | tan(x[4])); 115 | A[17] = 116 | 0.16666666666666666 * (0.4 * tan(0.05 * u[1] + x[4]) + 117 | 0.1 * tan(0.1 * u[1] + x[4]) + 0.1 * tan(x[4])); 118 | A[18] = 1; 119 | A[19] = 0; 120 | A[20] = 121 | 0.16666666666666666 * 122 | (-0.01 * pow(0.05 * u[0] + x[3], 2) * 123 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 124 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 125 | -0.01 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 126 | (0.1 * u[0] + x[3]) * 127 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 128 | -0.01 * (1 + pow(tan(x[4]), 2)) * (0.05 * u[0] + x[3]) * 129 | sin(0.05 * tan(x[4]) * x[3] + x[2]) * x[3]); 130 | A[21] = 131 | 0.16666666666666666 * 132 | (0.01 * pow(0.05 * u[0] + x[3], 2) * 133 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 134 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 135 | 0.01 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 136 | (0.1 * u[0] + x[3]) * 137 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 138 | 0.01 * (1 + pow(tan(x[4]), 2)) * (0.05 * u[0] + x[3]) * 139 | cos(0.05 * tan(x[4]) * x[3] + x[2]) * x[3]); 140 | A[22] = 0.16666666666666666 * 141 | (0.1 * (1 + pow(tan(0.1 * u[1] + x[4]), 2)) * (0.1 * u[0] + x[3]) + 142 | 0.4 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) + 143 | 0.1 * (1 + pow(tan(x[4]), 2)) * x[3]); 144 | A[23] = 0; 145 | A[24] = 1; 146 | } 147 | 148 | void tiny_Bicycle5dGetJacobianB_Raw(double* B, const double* x, 149 | const double* u) { 150 | B[0] = 151 | 0.16666666666666666 * 152 | (0.01 * 153 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 154 | 0.01 * cos(0.05 * tan(x[4]) * x[3] + x[2]) + 155 | 0.01 * cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 156 | -0.0005 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 157 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 158 | -0.0005 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 159 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 160 | B[1] = 161 | 0.16666666666666666 * 162 | (0.01 * 163 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 164 | 0.01 * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 165 | 0.01 * sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 166 | 0.0005 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 167 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 168 | 0.0005 * (0.05 * u[0] + x[3]) * 169 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) * 170 | tan(0.05 * u[1] + x[4])); 171 | B[2] = 0.16666666666666666 * 172 | (0.02 * tan(0.05 * u[1] + x[4]) + 0.01 * tan(0.1 * u[1] + x[4])); 173 | B[3] = 0.1; 174 | B[4] = 0; 175 | B[5] = 176 | 0.16666666666666666 * 177 | (-0.0005 * pow(0.05 * u[0] + x[3], 2) * 178 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 179 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 180 | -0.0005 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 181 | (0.1 * u[0] + x[3]) * 182 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 183 | B[6] = 184 | 0.16666666666666666 * 185 | (0.0005 * pow(0.05 * u[0] + x[3], 2) * 186 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 187 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 188 | 0.0005 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 189 | (0.1 * u[0] + x[3]) * 190 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 191 | B[7] = 0.16666666666666666 * 192 | (0.01 * (1 + pow(tan(0.1 * u[1] + x[4]), 2)) * (0.1 * u[0] + x[3]) + 193 | 0.02 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3])); 194 | B[8] = 0; 195 | B[9] = 0.1; 196 | } 197 | 198 | void tiny_Bicycle5dGetJacobians(Matrix* A, Matrix* B, const Matrix x, 199 | const Matrix u) { 200 | tiny_Bicycle5dGetJacobianA_Raw(A->data, x.data, u.data); 201 | tiny_Bicycle5dGetJacobianB_Raw(B->data, x.data, u.data); 202 | } -------------------------------------------------------------------------------- /examples/bicycle_5d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "slap/slap.h" 6 | 7 | //======================================== 8 | // Bicycle model parameters 9 | // X = [x; y; theta; v; delta] : x, y, yaw, linear vel, steering angle 10 | // U = [a; delta_dot] : linear accel and steering rate 11 | //======================================== 12 | // struct tiny_Model_Bicycle { 13 | // double drive_min[2]; 14 | // double drive_max[2]; 15 | // double u_min[2]; 16 | // double u_max[2]; 17 | // } tiny_DefaultModel_Bicycle = {{-2, -0.5}, {2, 0.5}, {-4, -0.7}, {4, 0.7}}; 18 | 19 | //======================================== 20 | // Codes generated from julia/bicycle_tvlqr 21 | // Discrete dynamics of bicycle model with predefined model params 22 | //======================================== 23 | void tiny_Bicycle5dNonlinearDynamics_Raw(double* xn, const double* x, 24 | const double* u); 25 | 26 | void tiny_Bicycle5dNonlinearDynamics(Matrix* xn, const Matrix x, 27 | const Matrix u); 28 | 29 | //======================================== 30 | // Codes generated from julia/bicycle_tvlqr 31 | // Jacobians of discrete dynamics of bicycle model with predefined model params 32 | //======================================== 33 | void tiny_Bicycle5dGetJacobianA_Raw(double* A, const double* x, 34 | const double* u); 35 | 36 | void tiny_Bicycle5dGetJacobianB_Raw(double* B, const double* x, 37 | const double* u); 38 | 39 | void tiny_Bicycle5dGetJacobians(Matrix* A, Matrix* B, const Matrix x, 40 | const Matrix u); -------------------------------------------------------------------------------- /examples/bicycle_example.c: -------------------------------------------------------------------------------- 1 | // MPC 2 | // Scenerio: Drive bicycle to track references with constraints. 3 | // Check trajopt at test/al_lqr_test/al_lqr_ltv_test.c 4 | 5 | // === BETTER TURN OFF GOAL_CONSTRAINT IN PROJECT CMAKELISTS.TXT TO PASS === 6 | // IF BOX CONSTRAINTS OFF, CAN HANDLE GOAL CONSTRAINT 7 | // IF BOX CONSTRAINTS ON, UNLIKELY TO HANDLE GOAL CONSTRAINT 8 | // DON"T WORRY ABOUT GRADIENT VANISHING/EXPLOSION SINCE SMALL MPC HORIZON 9 | // GREATER NHORIZON, GREATER ITERATION, GREATER CHANCE OF EXPLOSION 10 | // TODO: Let user choose constraints, compile options with #IFDEF 11 | 12 | #include "bicycle_5d.h" 13 | #include "data/lqr_ltv_data.h" 14 | #include "slap/slap.h" 15 | #include "tinympc/tinympc.h" 16 | 17 | #define H 0.1 18 | #define NSTATES 5 19 | #define NINPUTS 2 20 | #define NHORIZON 21 21 | #define NSIM 101 22 | 23 | int main(void) { 24 | // DEFINE PROBLEM DATA 25 | double x0_data[NSTATES] = {1, -1, 0, 0, 0}; 26 | // double xg_data[NSTATES] = {0}; 27 | // double ug_data[NINPUTS] = {0}; 28 | double Xhrz_data[NSTATES * NHORIZON] = {0}; 29 | double X_data[NSTATES * NSIM] = {0}; 30 | double Uhrz_data[NINPUTS * (NHORIZON - 1)] = {0}; 31 | double K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 32 | double d_data[NINPUTS * (NHORIZON - 1)] = {0}; 33 | double P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 34 | double p_data[NSTATES * NHORIZON] = {0}; 35 | double A_data[NSTATES * NSTATES * (NHORIZON - 1)] = {0}; 36 | double B_data[NSTATES * NINPUTS * (NHORIZON - 1)] = {0}; 37 | double f_data[NSTATES * (NHORIZON - 1)] = {0}; 38 | double input_dual_data[2 * NINPUTS * (NHORIZON - 1)] = {0}; 39 | double state_dual_data[2 * NSTATES * (NHORIZON)] = {0}; 40 | double goal_dual_data[NSTATES] = {0}; 41 | double Q_data[NSTATES * NSTATES] = {0}; 42 | double R_data[NINPUTS * NINPUTS] = {0}; 43 | double Qf_data[NSTATES * NSTATES] = {0}; 44 | 45 | // Put constraints on u, x4, x5 46 | double umin_data[NINPUTS] = {-2.1, -1.1}; 47 | double umax_data[NINPUTS] = {2.1, 1.1}; 48 | double xmin_data[NSTATES] = {-100, -100, -100, -4.0, -0.8}; 49 | double xmax_data[NSTATES] = {100, 100, 100, 4.0, 0.8}; 50 | 51 | // double umin_data[NINPUTS] = {-5, -2}; 52 | // double umax_data[NINPUTS] = {5, 2}; 53 | // double xmin_data[NSTATES] = {-100, -100, -100, -100, -100}; 54 | // double xmax_data[NSTATES] = {100, 100, 100, 100, 100}; 55 | 56 | // DEFINE VARIABLES 57 | Matrix X[NSIM]; 58 | Matrix Xref[NSIM]; 59 | Matrix Uref[NSIM - 1]; 60 | Matrix Xhrz[NHORIZON]; 61 | Matrix Uhrz[NHORIZON - 1]; 62 | Matrix K[NHORIZON - 1]; 63 | Matrix d[NHORIZON - 1]; 64 | Matrix P[NHORIZON]; 65 | Matrix p[NHORIZON]; 66 | Matrix A[NHORIZON - 1]; 67 | Matrix B[NHORIZON - 1]; 68 | Matrix f[NHORIZON - 1]; 69 | Matrix input_duals[NHORIZON - 1]; 70 | Matrix state_duals[NHORIZON]; 71 | 72 | // DEFINE STRUCTS 73 | tiny_LtvModel model; 74 | tiny_InitLtvModel(&model); 75 | tiny_ProblemData prob; 76 | tiny_InitProblemData(&prob); 77 | tiny_Solver solver; 78 | tiny_InitSolver(&solver); 79 | 80 | // DEFINE POINTERS 81 | double* Xhrz_ptr = Xhrz_data; 82 | double* Xptr = X_data; 83 | double* Xref_ptr = Xref_data; 84 | double* Uhrz_ptr = Uhrz_data; 85 | double* Uref_ptr = Uref_data; 86 | double* Kptr = K_data; 87 | double* dptr = d_data; 88 | double* Pptr = P_data; 89 | double* pptr = p_data; 90 | double* Aptr = A_data; 91 | double* Bptr = B_data; 92 | double* fptr = f_data; 93 | double* udual_ptr = input_dual_data; 94 | double* xdual_ptr = state_dual_data; 95 | 96 | // INITIALIZE MATRICES 97 | for (int i = 0; i < NSIM; ++i) { 98 | if (i < NSIM - 1) { 99 | Uref[i] = slap_MatrixFromArray(NINPUTS, 1, Uref_ptr); 100 | Uref_ptr += NINPUTS; 101 | } 102 | X[i] = slap_MatrixFromArray(NSTATES, 1, Xptr); 103 | Xptr += NSTATES; 104 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, Xref_ptr); 105 | Xref_ptr += NSTATES; 106 | // tiny_Print(Xref[i]); 107 | } 108 | for (int i = 0; i < NHORIZON; ++i) { 109 | if (i < NHORIZON - 1) { 110 | A[i] = slap_MatrixFromArray(NSTATES, NSTATES, Aptr); 111 | Aptr += NSTATES * NSTATES; 112 | B[i] = slap_MatrixFromArray(NSTATES, NINPUTS, Bptr); 113 | Bptr += NSTATES * NINPUTS; 114 | f[i] = slap_MatrixFromArray(NSTATES, 1, fptr); 115 | fptr += NSTATES; 116 | Uhrz[i] = slap_MatrixFromArray(NINPUTS, 1, Uhrz_ptr); 117 | slap_Copy(Uhrz[i], Uref[i]); // Initialize U 118 | Uhrz_ptr += NINPUTS; 119 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 120 | Kptr += NINPUTS * NSTATES; 121 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 122 | dptr += NINPUTS; 123 | input_duals[i] = slap_MatrixFromArray(2 * NINPUTS, 1, udual_ptr); 124 | udual_ptr += 2 * NINPUTS; 125 | } 126 | Xhrz[i] = slap_MatrixFromArray(NSTATES, 1, Xhrz_ptr); 127 | slap_Copy(Xhrz[i], Xref[i]); // Initialize U 128 | Xhrz_ptr += NSTATES; 129 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pptr); 130 | Pptr += NSTATES * NSTATES; 131 | p[i] = slap_MatrixFromArray(NSTATES, 1, pptr); 132 | pptr += NSTATES; 133 | state_duals[i] = slap_MatrixFromArray(2 * NSTATES, 1, xdual_ptr); 134 | xdual_ptr += 2 * NSTATES; 135 | } 136 | 137 | // INITIALIZE STRUCTS 138 | model.ninputs = NSTATES; 139 | model.nstates = NINPUTS; 140 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 141 | model.get_jacobians = tiny_Bicycle5dGetJacobians; 142 | model.get_nonlinear_dynamics = tiny_Bicycle5dNonlinearDynamics; 143 | model.A = A; 144 | model.B = B; 145 | model.f = f; 146 | slap_Copy(X[0], model.x0); 147 | 148 | prob.ninputs = NINPUTS; 149 | prob.nstates = NSTATES; 150 | prob.nhorizon = NHORIZON; 151 | prob.ncstr_inputs = 2 * NINPUTS; 152 | prob.ncstr_states = 2 * NSTATES; 153 | prob.ncstr_goal = NSTATES; 154 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 155 | slap_SetIdentity(prob.Q, 10e-1); 156 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 157 | slap_SetIdentity(prob.R, 1e-1); 158 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 159 | slap_SetIdentity(prob.Qf, 10e-1); 160 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, umax_data); 161 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, umin_data); 162 | prob.x_max = slap_MatrixFromArray(NSTATES, 1, xmax_data); 163 | prob.x_min = slap_MatrixFromArray(NSTATES, 1, xmin_data); 164 | prob.X_ref = Xref; 165 | prob.U_ref = Uref; 166 | prob.x0 = model.x0; 167 | prob.K = K; 168 | prob.d = d; 169 | prob.P = P; 170 | prob.p = p; 171 | prob.input_duals = input_duals; 172 | prob.state_duals = state_duals; 173 | prob.goal_dual = slap_MatrixFromArray(NSTATES, 1, goal_dual_data); 174 | 175 | solver.max_primal_iters = 10; // Often takes less than 5 176 | 177 | // Absolute formulation 178 | // Warm-starting since horizon data is reused 179 | // At each time step (stop earlier as horizon exceeds the end) 180 | for (int k = 0; k < NSIM - NHORIZON - 1; ++k) { 181 | // 1. Read from high level controller 182 | // 2. Update problem data with info 183 | // 3. Solve problem again 184 | 185 | printf("\n=> k = %d\n", k); 186 | printf("ex[%d] = %.4f\n", k, slap_NormedDifference(X[k], Xref[k])); 187 | // === 1. Setup and solve MPC === 188 | 189 | slap_Copy(Xhrz[0], X[k]); 190 | // Update A, B within horizon 191 | tiny_UpdateHorizonJacobians(&model, prob); 192 | // Update reference 193 | prob.X_ref = &Xref[k]; 194 | prob.U_ref = &Uref[k]; 195 | 196 | // Solve optimization problem using Augmented Lagrangian TVLQR, benchmark 197 | // this 198 | tiny_MpcLtv(Xhrz, Uhrz, &prob, &solver, model, 0); 199 | 200 | // Test control constraints here (since we didn't save U) 201 | // TEST(slap_NormInf(Uhrz[0]) < slap_NormInf(prob.u_max) + solver.cstr_tol); 202 | 203 | // === 2. Simulate dynamics using the first control solution === 204 | 205 | // Clamping control would not effect since our solution is feasible 206 | tiny_ClampMatrix(&Uhrz[0], prob.u_min, prob.u_max); 207 | tiny_Bicycle5dNonlinearDynamics(&X[k + 1], X[k], Uhrz[0]); 208 | } 209 | 210 | // // ========== Test ========== 211 | // // Test state constraints 212 | // for (int k = 0; k < NSIM - NHORIZON - 1; ++k) { 213 | // for (int i = 0; i < NSTATES; ++i) { 214 | // TEST(X[k].data[i] < xmax_data[i] + solver.cstr_tol); 215 | // TEST(X[k].data[i] > xmin_data[i] - solver.cstr_tol); 216 | // } 217 | // } 218 | // // Test tracking performance 219 | // for (int k = NSIM - NHORIZON - 5; k < NSIM - NHORIZON; ++k) { 220 | // TEST(slap_NormedDifference(X[k], Xref[k]) < 0.1); 221 | // } 222 | // // -------------------------- 223 | return 0; 224 | } 225 | 226 | -------------------------------------------------------------------------------- /examples/data/back_pass_data.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | double X_data[12] = {5.0, 7.0, 2.0, -1.4, 5.0, 7.0, 4 | 2.0, -1.4, 5.0, 7.0, 2.0, -1.4}; 5 | 6 | double U_data[4] = {-7.522015227259879e-5, -0.0019667347526769585, 7 | 0.005620890315977286, -0.01944557581703168}; 8 | 9 | double dsln_data[4] = {10.75088111978507, 1.104847001842924, 11.238761987040757, 10 | -5.253156339850558}; 11 | -------------------------------------------------------------------------------- /examples/data/forward_pass_data.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | double x_data[12] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 4 | 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; 5 | 6 | double d_data[4] = {0.6042411619304313, 0.09811826179886918, 0.5695222989883322, 7 | 0.5114280910753717}; 8 | 9 | double u_data[4] = {2.0, 2.0, 2.0, 2.0}; 10 | 11 | double K_data[16] = { 12 | 0.49686742205256307, 0.13266704729919998, 0.012970804597214558, 13 | 0.4000782856230036, 0.8217734795746234, 0.9601267910608784, 14 | 0.25209686902099326, 0.2370244586454353, 0.31367755244422746, 15 | 0.23728823461435433, 0.21899727930703738, 0.9575915918531737, 16 | 0.8260948299228765, 0.7388757501087568, 0.1600629020559956, 17 | 0.02942262316169697}; 18 | 19 | double xsol_data[12] = {1.0, 20 | 1.0, 21 | 1.0, 22 | 1.0, 23 | 1.089060251314121, 24 | 1.090859925777863, 25 | 0.7812050262824174, 26 | 0.8171985155572613, 27 | 1.1575498339012982, 28 | 1.1605012516232516, 29 | 0.5885866254611287, 30 | 0.5756280013505097}; 31 | -------------------------------------------------------------------------------- /examples/data/lqr_lti_track_data.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | double Xref_data[204] = {5.0, 4 | 7.0, 5 | 2.0, 6 | -1.4, 7 | 5.160652560060507, 8 | 6.839320556675474, 9 | 1.2130512012101469, 10 | -1.8135888664905324, 11 | 5.248301469968943, 12 | 6.641397890900794, 13 | 0.5399269969585725, 14 | -2.144864449003066, 15 | 5.273732403226259, 16 | 6.413960675321315, 17 | -0.031308331812253454, 18 | -2.403879862586514, 19 | 5.246585578869316, 20 | 6.163779035158997, 21 | -0.5116281553266366, 22 | -2.599752940659822, 23 | 5.175449379562446, 24 | 5.896754823933058, 25 | -0.9110958308107685, 26 | -2.7407312838589633, 27 | 5.067948735396972, 28 | 5.618005512612837, 29 | -1.238917052498706, 30 | -2.834254942545475, 31 | 4.930828296188796, 32 | 5.331941938811838, 33 | -1.5034917316648082, 34 | -2.8870165334744953, 35 | 4.77003045942473, 36 | 5.042340179824062, 37 | -1.7124650036165288, 38 | -2.9050186462810266, 39 | 4.590768357721344, 40 | 4.752407825352388, 41 | -1.8727770304511915, 42 | -2.893628443152452, 43 | 4.397593939606499, 44 | 4.464844933391762, 45 | -1.9907113318457161, 46 | -2.8576293960600743, 47 | 4.194461301416077, 48 | 4.181899956543142, 49 | -2.0719414319627116, 50 | -2.8012701409123415, 51 | 3.984785446846749, 52 | 3.9054209266173876, 53 | -2.121575659423859, 54 | -2.7283104576027437, 55 | 3.7714966648816612, 56 | 3.636902183250262, 57 | -2.1441999798779015, 58 | -2.642064409739765, 59 | 3.557090727008504, 60 | 3.3775269278432885, 61 | -2.1439187775852435, 62 | -2.5454406983997098, 63 | 3.3436751114182965, 64 | 3.128205877872067, 65 | -2.1243935342189033, 66 | -2.440980301024724, 67 | 3.1330114656941297, 68 | 2.8896122888186353, 69 | -2.088879380264431, 70 | -2.3308914800439156, 71 | 2.9265545208073664, 72 | 2.6622136019998983, 73 | -2.040259517470838, 74 | -2.217082256330822, 75 | 2.7254876684236526, 76 | 2.4462999666536716, 77 | -1.9810775302034394, 78 | -2.1011904505937213, 79 | 2.5307554109292716, 80 | 2.242009874046007, 81 | -1.9135676196841767, 82 | -1.9846114015595697, 83 | 2.343092889527686, 84 | 2.04935313028511, 85 | -1.8396828083475338, 86 | -1.8685234736583753, 87 | 2.163052690498791, 88 | 1.8682313831465265, 89 | -1.761121172230373, 90 | -1.7539114691132918, 91 | 1.9910291234991955, 92 | 1.6984564066840826, 93 | -1.6793501677615361, 94 | -1.6415880601355841, 95 | 1.827280158820952, 96 | 1.5397663358507838, 97 | -1.5956291258033337, 98 | -1.5322133565303908, 99 | 1.6719472030019182, 100 | 1.3918400318926178, 101 | -1.5110299905773443, 102 | -1.4263127226329275, 103 | 1.5250728842524401, 104 | 1.2543097479965668, 105 | -1.4264563844122196, 106 | -1.3242929552880915, 107 | 1.3866170109673324, 108 | 1.1267722536465432, 109 | -1.342661081289936, 110 | -1.226456931712379, 111 | 1.2564708582464916, 112 | 1.0087985654273817, 113 | -1.260261973126881, 114 | -1.1330168326708547, 115 | 1.134469928951346, 116 | 0.899942421664707, 117 | -1.1797566127760313, 118 | -1.0441060425826407, 119 | 1.0204053274611706, 120 | 0.7997476283335548, 121 | -1.1015354170274778, 122 | -0.959789824040401, 123 | 0.9140338760321995, 124 | 0.7077543941373491, 125 | -1.0258936115519421, 126 | -0.8800748598837127, 127 | 0.8150870955597816, 128 | 0.6235047635690512, 129 | -0.9530419978964161, 130 | -0.8049177514822448, 131 | 0.7232791646444537, 132 | 0.5465472481283972, 133 | -0.8831166204101413, 134 | -0.7342325573308361, 135 | 0.6383139632015288, 136 | 0.47644074768716427, 137 | -0.816187408448358, 138 | -0.6678974514938218, 139 | 0.5598912994563423, 140 | 0.4127578462669362, 141 | -0.7522658664553726, 142 | -0.60576057691074, 143 | 0.4877124120514444, 144 | 0.3550875592147623, 145 | -0.6913118816425852, 146 | -0.5476451641327373, 147 | 0.42148483216843396, 148 | 0.3030376019213867, 149 | -0.6332397160176231, 150 | -0.4933539817347754, 151 | 0.3609266840402354, 152 | 0.25623624381097576, 153 | -0.577923246546348, 154 | -0.44267318047344323, 155 | 0.30577049599831035, 156 | 0.21433380532435387, 157 | -0.5252005142921532, 158 | -0.39537558925899446, 159 | 0.2557665882575768, 160 | 0.1770038500012115, 161 | -0.47487764052251763, 162 | -0.3512235172038531, 163 | 0.21068609797935717, 164 | 0.1439441185202623, 165 | -0.42673216504187494, 166 | -0.3099711124151313, 167 | 0.17032369675533646, 168 | 0.11487724665804554, 169 | -0.3805158594385393, 170 | -0.2713663248292037, 171 | 0.13450005050571714, 172 | 0.08955130455394529, 173 | -0.33595706555384697, 174 | -0.23515251725280112, 175 | 0.10306406686189558, 176 | 0.0677401903970169, 177 | -0.29276260732258425, 178 | -0.20106976588576672, 179 | 0.0758949703847101, 180 | 0.04924390765463322, 181 | -0.25061932222112504, 182 | -0.16885588896190687, 183 | 0.05290424142779979, 184 | 0.03388875121848737, 185 | -0.20919525691708116, 186 | -0.1382472397610101, 187 | 0.03403745006377965, 188 | 0.021527424324411584, 189 | -0.16814057036332172, 190 | -0.10897929812050552, 191 | 0.01927601221861064, 192 | 0.012039104782798255, 193 | -0.12708818654005855, 194 | -0.08078709271176107, 195 | 0.008638890974597507, 196 | 0.005329475909947972, 197 | -0.08565423834020407, 198 | -0.05340548474524458, 199 | 0.002184261870919275, 200 | 0.0013307345511037905, 201 | -0.04343834373336055, 202 | -0.02656934243163905, 203 | 1.1156916756909982e-5, 204 | 1.5857068697935125e-6, 205 | -2.3755349886737942e-5, 206 | -1.3634453040884537e-5}; 207 | 208 | double Uref_data[100] = { 209 | -7.869487987898531, -4.135888664905324, -6.731242042515744, 210 | -3.312755825125335, -5.712353287708259, -2.5901541358344797, 211 | -4.803198235143832, -1.9587307807330805, -3.9946767548413185, 212 | -1.4097834319914113, -3.2782122168793753, -0.9352365868651165, 213 | -2.6457467916610216, -0.5276159092902017, -2.0897327195172055, 214 | -0.18002112806531131, -1.6031202683466281, 0.11390203128574579, 215 | -1.1793430139452465, 0.3599904709237788, -0.8123010011699551, 216 | 0.5635925514773296, -0.4963422746114752, 0.729596833095979, 217 | -0.22624320454042407, 0.8624604786297864, 0.0028120229265806273, 218 | 0.9662371134005528, 0.19525243366340164, 1.044603973749857, 219 | 0.35514153954472294, 1.1008882098080828, 0.48619862793592855, 220 | 1.1380922371309359, 0.5918198726739854, 1.1589180573710058, 221 | 0.6750991051926264, 1.1657904903415173, 0.7388481133664283, 222 | 1.1608792790119447, 0.7856163611716078, 1.1461200454508353, 223 | 0.8177100446883694, 1.1232340897770756, 0.8372104195820234, 224 | 1.0937470360519321, 0.8459913522598942, 1.0590063389746323, 225 | 0.8457360616512467, 1.0201976734483607, 0.8379530312228357, 226 | 0.978360235757125, 0.8239910816305508, 0.9344009904152427, 227 | 0.8050536035084973, 0.8891079008821405, 0.7822119574855338, 228 | 0.8431621854223972, 0.7564180547553572, 0.7971496415668827, 229 | 0.7285161365552604, 0.7515710840146792, 0.6992537748627479, 230 | 0.7068519415140865, 0.6692921196178323, 0.6633510583701431, 231 | 0.6392154199298542, 0.6213687458308186, 0.6095398481278744, 232 | 0.5811541277800264, 0.5807216562496212, 0.5429118239796196, 233 | 0.5531646947127509, 0.5068080126133219, 0.5272273225419483, 234 | 0.4729759121444876, 0.5032287376963556, 0.4415207205514137, 235 | 0.481454754806427, 0.4125240478872175, 0.4621630560333569, 236 | 0.3860478758592761, 0.44558793884692294, 0.36213807576402574, 237 | 0.4319445823126274, 0.340827513670344, 0.4214328510145921, 238 | 0.3221387692385986, 0.4142406530404389, 0.3060864920089675, 239 | 0.41054686553759434, 0.29267941640504586, 0.4105238382326318, 240 | 0.28192205408744453, 0.41433948199854476, 0.2738160796651649, 241 | 0.4221589460684351, 0.26836142313605527, 0.43414588383473807, 242 | 0.26555707978598164}; 243 | -------------------------------------------------------------------------------- /examples/planar_quad_mpc_example.c: -------------------------------------------------------------------------------- 1 | // MPC 2 | // Scenerio: Drive bicycle to track references with constraints. 3 | // Check trajopt at test/al_lqr_test/al_lqr_ltv_test.c 4 | 5 | // === BETTER TURN OFF GOAL_CONSTRAINT IN PROJECT CMAKELISTS.TXT TO PASS === 6 | // IF BOX CONSTRAINTS OFF, CAN HANDLE GOAL CONSTRAINT 7 | // IF BOX CONSTRAINTS ON, UNLIKELY TO HANDLE GOAL CONSTRAINT 8 | // DON"T WORRY ABOUT GRADIENT VANISHING/EXPLOSION SINCE SMALL MPC HORIZON 9 | // GREATER NHORIZON, GREATER ITERATION, GREATER CHANCE OF EXPLOSION 10 | // TODO: Let user choose constraints, compile options with #IFDEF 11 | 12 | #include "planar_quadrotor.h" 13 | // #include "bicycle_5d.h" 14 | #include "data/lqr_ltv_data.h" 15 | #include "simpletest.h" 16 | #include "slap/slap.h" 17 | #include "tinympc/tinympc.h" 18 | 19 | 20 | #define H 0.05 21 | #define NSTATES 6 22 | #define NINPUTS 2 23 | #define NHORIZON 2 24 | #define NSIM 51 25 | 26 | int main() { 27 | 28 | // sfloat g = 9.81; // m/s^2 29 | // sfloat m = 1.0; // kg 30 | 31 | tiny_LtvModel model; 32 | tiny_InitLtvModel(&model); 33 | model.nstates = NSTATES; 34 | model.ninputs = NINPUTS; 35 | model.dt = H; 36 | 37 | sfloat x0_data[NSTATES] = {1, 3, .5, 0, 0, 0}; 38 | // sfloat xg_data[NSTATES] = {0}; 39 | // sfloat ug_data[NINPUTS] = {0}; 40 | sfloat Xhrz_data[NSTATES * NHORIZON] = {0}; 41 | sfloat X_data[NSTATES * NSIM] = {0}; 42 | sfloat Uhrz_data[NINPUTS * (NHORIZON - 1)] = {0}; 43 | sfloat K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 44 | sfloat d_data[NINPUTS * (NHORIZON - 1)] = {0}; 45 | sfloat P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 46 | sfloat p_data[NSTATES * NHORIZON] = {0}; 47 | sfloat A_data[NSTATES * NSTATES * (NHORIZON - 1)] = {0}; 48 | sfloat B_data[NSTATES * NINPUTS * (NHORIZON - 1)] = {0}; 49 | sfloat f_data[NSTATES * (NHORIZON - 1)] = {0}; 50 | sfloat input_dual_data[2 * NINPUTS * (NHORIZON - 1)] = {0}; 51 | sfloat state_dual_data[2 * NSTATES * (NHORIZON)] = {0}; 52 | sfloat goal_dual_data[NSTATES] = {0}; 53 | 54 | sfloat Q_data[NSTATES * NSTATES] = {0}; 55 | sfloat R_data[NINPUTS * NINPUTS] = {0}; 56 | sfloat Qf_data[NSTATES * NSTATES] = {0}; 57 | 58 | // Put constraints on u, x4, x5 59 | // sfloat umin_data[NINPUTS] = {0.2*m*g, 0.2*m*g}; 60 | // sfloat umax_data[NINPUTS] = {0.6*m*g, 0.6*m*g}; 61 | sfloat umin_data[NINPUTS] = {-99999, -99999}; 62 | sfloat umax_data[NINPUTS] = {99999, 99999}; 63 | sfloat xmin_data[NSTATES] = {-99999, 0, -99999, -99999, -99999, -99999}; 64 | sfloat xmax_data[NSTATES] = {99999, 99999, 99999, 99999, 99999, 99999}; 65 | 66 | Matrix X[NSIM]; 67 | 68 | Matrix Xref[NSIM]; 69 | // Matrix Uref[NSIM - 1]; 70 | Matrix Xhrz[NHORIZON]; 71 | Matrix Uhrz[NHORIZON - 1]; 72 | Matrix K[NHORIZON - 1]; 73 | Matrix d[NHORIZON - 1]; 74 | Matrix P[NHORIZON]; 75 | Matrix p[NHORIZON]; 76 | Matrix A[NHORIZON - 1]; 77 | Matrix B[NHORIZON - 1]; 78 | Matrix f[NHORIZON - 1]; 79 | Matrix input_duals[NHORIZON - 1]; 80 | Matrix state_duals[NHORIZON]; 81 | 82 | tiny_ProblemData prob; 83 | tiny_InitProblemData(&prob); 84 | tiny_Solver solver; 85 | tiny_InitSolver(&solver); 86 | 87 | X[0] = slap_MatrixFromArray(NSTATES, 1, x0_data); 88 | 89 | for (int i = 0; i < NSIM; ++i) { 90 | // if (i < NSIM - 1) { 91 | // Uref[i] = slap_MatrixFromArray(NINPUTS, 1, &Uref_data[i*NINPUTS]); 92 | // } 93 | X[i] = slap_MatrixFromArray(NSTATES, 1, &X_data[i*NSTATES]); 94 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, &Xref_data[i*NSTATES]); 95 | } 96 | for (int i = 0; i < NHORIZON; ++i) { 97 | if (i < NHORIZON - 1) { 98 | A[i] = slap_MatrixFromArray(NSTATES, NSTATES, &A_data[i*NSTATES*NINPUTS]); 99 | B[i] = slap_MatrixFromArray(NSTATES, NINPUTS, &B_data[i*NSTATES*NINPUTS]); 100 | f[i] = slap_MatrixFromArray(NSTATES, 1, &f_data[i*NSTATES]); 101 | Uhrz[i] = slap_MatrixFromArray(NINPUTS, 1, &Uhrz_data[i*NINPUTS]); 102 | // slap_Copy(Uhrz[i], Uref[i]); // Initialize U 103 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, &K_data[i*NINPUTS*NSTATES]); 104 | d[i] = slap_MatrixFromArray(NINPUTS, 1, &d_data[i*NINPUTS]); 105 | input_duals[i] = slap_MatrixFromArray(2 * NINPUTS, 1, &input_dual_data[i*2*NINPUTS]); 106 | } 107 | Xhrz[i] = slap_MatrixFromArray(NSTATES, 1, &Xhrz_data[i*NSTATES]); 108 | slap_Copy(Xhrz[i], Xref[i]); // Initialize U 109 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, &P_data[i*NSTATES*NSTATES]); 110 | p[i] = slap_MatrixFromArray(NSTATES, 1, &p_data[i*NSTATES]); 111 | state_duals[i] = slap_MatrixFromArray(2 * NSTATES, 1, &state_dual_data[i*NSTATES]); 112 | } 113 | 114 | model.ninputs = NSTATES; 115 | model.nstates = NINPUTS; 116 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 117 | model.get_jacobians = tiny_PQuadGetJacobians; 118 | model.get_nonlinear_dynamics = tiny_PQuadNonlinearDynamics; 119 | // model.get_jacobians = tiny_Bicycle5dGetJacobians; 120 | // model.get_nonlinear_dynamics = tiny_Bicycle5dNonlinearDynamics; 121 | model.A = A; 122 | model.B = B; 123 | model.f = f; 124 | 125 | prob.ninputs = NINPUTS; 126 | prob.nstates = NSTATES; 127 | prob.nhorizon = NHORIZON; 128 | prob.ncstr_inputs = 2 * NINPUTS; 129 | prob.ncstr_states = 2 * NSTATES; 130 | prob.ncstr_goal = 0; 131 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 132 | slap_SetIdentity(prob.Q, 1e-1); 133 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 134 | slap_SetIdentity(prob.R, 1e-1); 135 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 136 | slap_SetIdentity(prob.Qf, 10e-1); 137 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, umin_data); 138 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, umax_data); 139 | prob.x_min = slap_MatrixFromArray(NSTATES, 1, xmin_data); 140 | prob.x_max = slap_MatrixFromArray(NSTATES, 1, xmax_data); 141 | prob.X_ref = Xref; 142 | // prob.U_ref = Uref; 143 | prob.x0 = model.x0; 144 | prob.K = K; 145 | prob.d = d; 146 | prob.P = P; 147 | prob.p = p; 148 | prob.input_duals = input_duals; 149 | prob.state_duals = state_duals; 150 | prob.goal_dual = slap_MatrixFromArray(NSTATES, 1, goal_dual_data); 151 | 152 | solver.max_primal_iters = 10; // Often takes less than 5 153 | 154 | printf("uhrz and xhrz init\n"); 155 | for (int i=0; i k = %d\n", k); 169 | // printf("ex[%d] = %.4f\n", k, slap_NormedDifference(X[k], Xref[k])); 170 | // === 1. Setup and solve MPC === 171 | 172 | slap_Copy(Xhrz[0], X[k]); 173 | // Update A, B within horizon 174 | tiny_UpdateHorizonJacobians(&model, prob); 175 | // Update reference 176 | prob.X_ref = &Xref[k]; 177 | // prob.U_ref = &Uref[k]; 178 | 179 | // Solve optimization problem using Augmented Lagrangian TVLQR, benchmark 180 | // this 181 | tiny_MpcLtv(Xhrz, Uhrz, &prob, &solver, model, 0); 182 | 183 | // Test control constraints here (since we didn't save U) 184 | printf("%.4f < %.4f\n", slap_NormInf(Uhrz[0]), slap_NormInf(prob.u_max) + solver.cstr_tol); 185 | TEST(slap_NormInf(Uhrz[0]) < slap_NormInf(prob.u_max) + solver.cstr_tol); 186 | 187 | for (int i=0; i xmin_data[i] - solver.cstr_tol); 209 | } 210 | } 211 | // Test tracking performance 212 | for (int k = NSIM - NHORIZON - 5; k < NSIM - NHORIZON; ++k) { 213 | TEST(slap_NormedDifference(X[k], Xref[k]) < 0.1); 214 | } 215 | // -------------------------- 216 | 217 | // PrintTestResult(); 218 | // return TestResult(); 219 | return 0; 220 | } 221 | -------------------------------------------------------------------------------- /examples/planar_quadrotor.c: -------------------------------------------------------------------------------- 1 | #include "planar_quadrotor.h" 2 | 3 | //======================================== 4 | // Planar quadrotor model parameters 5 | //======================================== 6 | // struct tiny_Model_PlanarQuadrotor { 7 | // double g; 8 | // double m; 9 | // double l; 10 | // double J; 11 | // double umin[2]; 12 | // double umax[2]; 13 | // } tiny_DefaultModel_PlanarQuadrotor = {9.81, 1, 0.018, 14 | // 0.3, {0, 0}, {19.62, 19.62}}; 15 | 16 | //======================================== 17 | // Codes generated from julia/planar_quad_gen 18 | // Discrete dynamics of planar quadrotor 19 | //======================================== 20 | void tiny_PQuadNonlinearDynamics_Raw(double* xn, const double* x, 21 | const double* u) { 22 | xn[0] = 0.16666666666666666 * 23 | (0.2 * (0.05 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + x[3]) + 24 | 0.1 * (0.1 * (u[0] + u[1]) * 25 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + 26 | x[5]) + 27 | x[2]) + 28 | x[3]) + 29 | 0.2 * (0.05 * (u[0] + u[1]) * sin(x[2]) + x[3]) + 0.1 * x[3]) + 30 | x[0]; 31 | xn[1] = 0.16666666666666666 * 32 | (0.2 * (0.05 * (-9.81 + (u[0] + u[1]) * cos(0.05 * x[5] + x[2])) + 33 | x[4]) + 34 | 0.2 * (0.05 * (-9.81 + (u[0] + u[1]) * cos(x[2])) + x[4]) + 35 | 0.1 * (0.1 * (-9.81 + (u[0] + u[1]) * 36 | cos(0.05 * (0.41666666666666674 * 37 | (-1 * u[0] + u[1]) + 38 | x[5]) + 39 | x[2])) + 40 | x[4]) + 41 | 0.1 * x[4]) + 42 | x[1]; 43 | xn[2] = 0.16666666666666666 * 44 | (0.4 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 45 | 0.1 * (0.8333333333333335 * (-1 * u[0] + u[1]) + x[5]) + 46 | 0.1 * x[5]) + 47 | x[2]; 48 | xn[3] = 49 | 0.16666666666666666 * 50 | (0.1 * (u[0] + u[1]) * 51 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 52 | x[2]) + 53 | 0.2 * (u[0] + u[1]) * 54 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 55 | x[2]) + 56 | 0.2 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + 57 | 0.1 * (u[0] + u[1]) * sin(x[2])) + 58 | x[3]; 59 | xn[4] = 60 | 0.16666666666666666 * 61 | (0.1 * (-9.81 + (u[0] + u[1]) * cos(0.1 * (0.41666666666666674 * 62 | (-1 * u[0] + u[1]) + 63 | x[5]) + 64 | x[2])) + 65 | 0.2 * (-9.81 + (u[0] + u[1]) * cos(0.05 * (0.41666666666666674 * 66 | (-1 * u[0] + u[1]) + 67 | x[5]) + 68 | x[2])) + 69 | 0.2 * (-9.81 + (u[0] + u[1]) * cos(0.05 * x[5] + x[2])) + 70 | 0.1 * (-9.81 + (u[0] + u[1]) * cos(x[2]))) + 71 | x[4]; 72 | xn[5] = 0.8333333333333336 * (-1 * u[0] + u[1]) + x[5]; 73 | } 74 | 75 | void tiny_PQuadNonlinearDynamics(Matrix* xn, const Matrix x, const Matrix u) { 76 | tiny_PQuadNonlinearDynamics_Raw(xn->data, x.data, u.data); 77 | } 78 | 79 | //======================================== 80 | // Codes generated from julia/planar_quad_gen 81 | // Jacobians of discrete dynamics of planar quadrotor 82 | //======================================== 83 | void tiny_PQuadGetJacobianA_Raw(double* A, const double* x, const double* u) { 84 | A[0] = 1; 85 | A[1] = 0; 86 | A[2] = 0; 87 | A[3] = 0; 88 | A[4] = 0; 89 | A[5] = 0; 90 | A[6] = 0; 91 | A[7] = 1; 92 | A[8] = 0; 93 | A[9] = 0; 94 | A[10] = 0; 95 | A[11] = 0; 96 | A[12] = 0.16666666666666666 * 97 | (0.01 * (u[0] + u[1]) * 98 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 99 | x[2]) + 100 | 0.01 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2]) + 101 | 0.01 * (u[0] + u[1]) * cos(x[2])); 102 | A[13] = 0.16666666666666666 * 103 | (-0.01 * (u[0] + u[1]) * 104 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 105 | x[2]) + 106 | -0.01 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + 107 | -0.01 * (u[0] + u[1]) * sin(x[2])); 108 | A[14] = 1; 109 | A[15] = 110 | 0.16666666666666666 * 111 | (0.1 * (u[0] + u[1]) * 112 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 113 | 0.1 * (u[0] + u[1]) * cos(x[2]) + 114 | 0.2 * (u[0] + u[1]) * 115 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 116 | x[2]) + 117 | 0.2 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2])); 118 | A[16] = 119 | 0.16666666666666666 * 120 | (-0.1 * (u[0] + u[1]) * 121 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 122 | -0.2 * (u[0] + u[1]) * 123 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 124 | x[2]) + 125 | -0.2 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + 126 | -0.1 * (u[0] + u[1]) * sin(x[2])); 127 | A[17] = 0; 128 | A[18] = 0.1; 129 | A[19] = 0; 130 | A[20] = 0; 131 | A[21] = 1; 132 | A[22] = 0; 133 | A[23] = 0; 134 | A[24] = 0; 135 | A[25] = 0.1; 136 | A[26] = 0; 137 | A[27] = 0; 138 | A[28] = 1; 139 | A[29] = 0; 140 | A[30] = 0.16666666666666666 * 141 | (0.0005 * (u[0] + u[1]) * 142 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 143 | x[2]) + 144 | 0.0005 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2])); 145 | A[31] = 0.16666666666666666 * 146 | (-0.0005 * (u[0] + u[1]) * 147 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 148 | x[2]) + 149 | -0.0005 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2])); 150 | A[32] = 0.1; 151 | A[33] = 152 | 0.16666666666666666 * 153 | (0.01 * (u[0] + u[1]) * 154 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 155 | x[2]) + 156 | 0.01 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2]) + 157 | 0.01 * (u[0] + u[1]) * 158 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2])); 159 | A[34] = 160 | 0.16666666666666666 * 161 | (-0.01 * (u[0] + u[1]) * 162 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 163 | x[2]) + 164 | -0.01 * (u[0] + u[1]) * 165 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 166 | -0.01 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2])); 167 | A[35] = 1; 168 | } 169 | 170 | void tiny_PQuadGetJacobianB_Raw(double* B, const double* x, const double* u) { 171 | B[0] = 172 | 0.16666666666666666 * 173 | (0.01 * sin(0.05 * x[5] + x[2]) + 0.01 * sin(x[2]) + 174 | 0.1 * 175 | (0.1 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 176 | x[2]) + 177 | -0.002083333333333334 * (u[0] + u[1]) * 178 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 179 | x[2]))); 180 | B[1] = 181 | 0.16666666666666666 * 182 | (0.01 * 183 | (0.02083333333333334 * (u[0] + u[1]) * 184 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 185 | x[2]) + 186 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 187 | x[2])) + 188 | 0.01 * cos(0.05 * x[5] + x[2]) + 0.01 * cos(x[2])); 189 | B[2] = -0.04166666666666667; 190 | B[3] = 191 | 0.16666666666666666 * 192 | (0.2 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 193 | x[2]) + 194 | 0.1 * 195 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 196 | 0.1 * sin(x[2]) + 0.2 * sin(0.05 * x[5] + x[2]) + 197 | -0.004166666666666668 * (u[0] + u[1]) * 198 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 199 | x[2]) + 200 | -0.004166666666666668 * (u[0] + u[1]) * 201 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2])); 202 | B[4] = 203 | 0.16666666666666666 * 204 | (0.2 * (0.02083333333333334 * (u[0] + u[1]) * 205 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 206 | x[2]) + 207 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 208 | x[2])) + 209 | 0.1 * (0.04166666666666668 * (u[0] + u[1]) * 210 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 211 | x[2]) + 212 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 213 | x[2])) + 214 | 0.2 * cos(0.05 * x[5] + x[2]) + 0.1 * cos(x[2])); 215 | B[5] = -0.8333333333333336; 216 | B[6] = 217 | 0.16666666666666666 * 218 | (0.01 * sin(0.05 * x[5] + x[2]) + 0.01 * sin(x[2]) + 219 | 0.1 * 220 | (0.1 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 221 | x[2]) + 222 | 0.002083333333333334 * (u[0] + u[1]) * 223 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 224 | x[2]))); 225 | B[7] = 226 | 0.16666666666666666 * 227 | (0.01 * 228 | (-0.02083333333333334 * (u[0] + u[1]) * 229 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 230 | x[2]) + 231 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 232 | x[2])) + 233 | 0.01 * cos(0.05 * x[5] + x[2]) + 0.01 * cos(x[2])); 234 | B[8] = 0.04166666666666667; 235 | B[9] = 236 | 0.16666666666666666 * 237 | (0.2 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 238 | x[2]) + 239 | 0.1 * 240 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 241 | 0.1 * sin(x[2]) + 0.2 * sin(0.05 * x[5] + x[2]) + 242 | 0.004166666666666668 * (u[0] + u[1]) * 243 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 244 | x[2]) + 245 | 0.004166666666666668 * (u[0] + u[1]) * 246 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2])); 247 | B[10] = 248 | 0.16666666666666666 * 249 | (0.1 * (-0.04166666666666668 * (u[0] + u[1]) * 250 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 251 | x[2]) + 252 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 253 | x[2])) + 254 | 0.1 * cos(x[2]) + 255 | 0.2 * (-0.02083333333333334 * (u[0] + u[1]) * 256 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 257 | x[2]) + 258 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 259 | x[2])) + 260 | 0.2 * cos(0.05 * x[5] + x[2])); 261 | B[11] = 0.8333333333333336; 262 | } 263 | 264 | void tiny_PQuadGetJacobians(Matrix* A, Matrix* B, const Matrix x, 265 | const Matrix u) { 266 | tiny_PQuadGetJacobianA_Raw(A->data, x.data, u.data); 267 | tiny_PQuadGetJacobianB_Raw(B->data, x.data, u.data); 268 | } -------------------------------------------------------------------------------- /examples/planar_quadrotor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "slap/slap.h" 6 | 7 | //======================================== 8 | // Planar quadrotor model parameters 9 | //======================================== 10 | // struct tiny_Model_PlanarQuadrotor { 11 | // double g; 12 | // double m; 13 | // double l; 14 | // double J; 15 | // double umin[2]; 16 | // double umax[2]; 17 | // } tiny_DefaultModel_PlanarQuadrotor = {9.81, 1, 0.018, 18 | // 0.3, {0, 0}, {19.62, 19.62}}; 19 | 20 | //======================================== 21 | // Codes generated from julia/planar_quad_gen 22 | // Discrete dynamics of planar quadrotor 23 | //======================================== 24 | void tiny_PQuadNonlinearDynamics_Raw(double* xn, const double* x, 25 | const double* u); 26 | 27 | void tiny_PQuadNonlinearDynamics(Matrix* xn, const Matrix x, const Matrix u); 28 | 29 | //======================================== 30 | // Codes generated from julia/planar_quad_gen 31 | // Jacobians of discrete dynamics of planar quadrotor 32 | //======================================== 33 | void tiny_PQuadGetJacobianA_Raw(double* A, const double* x, const double* u); 34 | 35 | void tiny_PQuadGetJacobianB_Raw(double* B, const double* x, const double* u); 36 | 37 | void tiny_PQuadGetJacobians(Matrix* A, Matrix* B, const Matrix x, 38 | const Matrix u); -------------------------------------------------------------------------------- /src/tinympc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # TODO (sschoedel): add support for GNU build 2 | if (TINY_CLANG_TIDY AND (CMAKE_C_COMPILER_ID STREQUAL "Clang")) 3 | message(STATUS "Running clang-tidy on source directory.") 4 | set(CMAKE_C_CLANG_TIDY 5 | clang-tidy; 6 | -export-fixes=tidy-fixes.yaml; 7 | -header-filter=.; 8 | --config-file=${PROJECT_SOURCE_DIR}/.clang-tidy; 9 | ) 10 | endif () 11 | 12 | add_library(tinympc 13 | lqr_lti.h 14 | lqr_lti.c 15 | 16 | lqr_ltv.h 17 | lqr_ltv.c 18 | 19 | mpc_lti.h 20 | mpc_lti.c 21 | 22 | mpc_ltv.h 23 | mpc_ltv.c 24 | 25 | utils.h 26 | utils.c 27 | 28 | data_struct.h 29 | data_struct.c 30 | 31 | cost_lqr.h 32 | cost_lqr.c 33 | 34 | dynamics_lti.h 35 | dynamics_lti.c 36 | 37 | dynamics_ltv.h 38 | dynamics_ltv.c 39 | 40 | constraint_linear.h 41 | constraint_linear.c 42 | ) 43 | 44 | target_include_directories(tinympc 45 | INTERFACE 46 | $ 47 | $ 48 | ) 49 | 50 | target_link_libraries(tinympc 51 | PRIVATE 52 | slap::slap 53 | ) 54 | 55 | add_library(tinympc::tinympc ALIAS tinympc) -------------------------------------------------------------------------------- /src/tinympc/constraint_linear.c: -------------------------------------------------------------------------------- 1 | #include "constraint_linear.h" 2 | 3 | double tiny_RiccatiConvergence(const tiny_ProblemData prob) { 4 | double norm_d_max = 0.0; 5 | for (int k = 0; k < prob.nhorizon - 1; ++k) { 6 | double norm_d = slap_NormTwo(prob.d[k]); 7 | if (norm_d > norm_d_max) { 8 | norm_d_max = norm_d; 9 | } 10 | } 11 | return norm_d_max; 12 | } 13 | 14 | // [u-p.u_max;-u + p.u_min] 15 | void tiny_IneqInputs(Matrix* ineq_input, const tiny_ProblemData prob, 16 | const Matrix u) { 17 | slap_SetConst(*ineq_input, 0); // clear before processing 18 | Matrix upper_half = slap_CreateSubMatrix(*ineq_input, 0, 0, prob.ninputs, 1); 19 | Matrix lower_half = 20 | slap_CreateSubMatrix(*ineq_input, prob.ninputs, 0, prob.ninputs, 1); 21 | slap_MatrixAddition(upper_half, u, prob.u_max, -1); 22 | slap_MatrixAddition(lower_half, prob.u_min, u, -1); 23 | } 24 | 25 | // [u_max, -u_min] 26 | void tiny_IneqInputsOffset(Matrix* ineq_input, const tiny_ProblemData prob) { 27 | Matrix upper_half = slap_CreateSubMatrix(*ineq_input, 0, 0, prob.ninputs, 1); 28 | Matrix lower_half = 29 | slap_CreateSubMatrix(*ineq_input, prob.ninputs, 0, prob.ninputs, 1); 30 | slap_Copy(upper_half, prob.u_max); 31 | slap_Copy(lower_half, prob.u_min); 32 | slap_ScaleByConst(lower_half, -1); 33 | } 34 | 35 | void tiny_IneqInputsJacobian(Matrix* ineq_jac, const tiny_ProblemData prob) { 36 | slap_SetConst(*ineq_jac, 0); // clear before processing 37 | Matrix upper_half = 38 | slap_CreateSubMatrix(*ineq_jac, 0, 0, prob.ninputs, prob.ninputs); 39 | Matrix lower_half = slap_CreateSubMatrix(*ineq_jac, prob.ninputs, 0, 40 | prob.ninputs, prob.ninputs); 41 | slap_SetIdentity(upper_half, 1); 42 | slap_SetIdentity(lower_half, -1); 43 | } 44 | 45 | // [x-p.x_max;-x + p.x_min] 46 | void tiny_IneqStates(Matrix* ineq_state, const tiny_ProblemData prob, 47 | const Matrix x) { 48 | slap_SetConst(*ineq_state, 0); // clear before processing 49 | Matrix upper_half = slap_CreateSubMatrix(*ineq_state, 0, 0, prob.nstates, 1); 50 | Matrix lower_half = 51 | slap_CreateSubMatrix(*ineq_state, prob.nstates, 0, prob.nstates, 1); 52 | slap_MatrixAddition(upper_half, x, prob.x_max, -1); 53 | slap_MatrixAddition(lower_half, prob.x_min, x, -1); 54 | } 55 | 56 | // [x_max, -x_min] 57 | void tiny_IneqStatesOffset(Matrix* ineq_state, const tiny_ProblemData prob) { 58 | Matrix upper_half = slap_CreateSubMatrix(*ineq_state, 0, 0, prob.nstates, 1); 59 | Matrix lower_half = 60 | slap_CreateSubMatrix(*ineq_state, prob.nstates, 0, prob.nstates, 1); 61 | slap_Copy(upper_half, prob.x_max); 62 | slap_Copy(lower_half, prob.x_min); 63 | slap_ScaleByConst(lower_half, -1); 64 | } 65 | 66 | void tiny_IneqStatesJacobian(Matrix* ineq_jac, const tiny_ProblemData prob) { 67 | slap_SetConst(*ineq_jac, 0); // clear before processing 68 | Matrix upper_half = 69 | slap_CreateSubMatrix(*ineq_jac, 0, 0, prob.nstates, prob.nstates); 70 | Matrix lower_half = slap_CreateSubMatrix(*ineq_jac, prob.nstates, 0, 71 | prob.nstates, prob.nstates); 72 | slap_SetIdentity(upper_half, 1); 73 | slap_SetIdentity(lower_half, -1); 74 | } 75 | 76 | void tiny_ActiveIneqMask(Matrix* mask, const Matrix dual, const Matrix ineq) { 77 | slap_SetConst(*mask, 0); // clear before processing 78 | for (int i = 0; i < ineq.rows; ++i) { 79 | // When variables are on the boundary or violating constraints 80 | bool active = dual.data[i] > 0 || ineq.data[i] > 0; 81 | slap_SetElement(*mask, i, i, active); 82 | } 83 | } 84 | 85 | void tiny_ClampIneqDuals(Matrix* dual, const Matrix new_dual) { 86 | for (int i = 0; i < dual->rows; ++i) { 87 | if (new_dual.data[i] > 0) { 88 | dual->data[i] = new_dual.data[i]; 89 | } else 90 | dual->data[i] = 0.0; 91 | } 92 | } -------------------------------------------------------------------------------- /src/tinympc/constraint_linear.h: -------------------------------------------------------------------------------- 1 | #include "data_struct.h" 2 | 3 | double tiny_RiccatiConvergence(const tiny_ProblemData prob); 4 | 5 | void tiny_IneqInputs(Matrix* ineq, const tiny_ProblemData prob, const Matrix u); 6 | 7 | void tiny_IneqInputsOffset(Matrix* ineq_input, const tiny_ProblemData prob); 8 | 9 | void tiny_IneqInputsJacobian(Matrix* ineq_jac, const tiny_ProblemData prob); 10 | 11 | void tiny_IneqStates(Matrix* ineq_state, const tiny_ProblemData prob, 12 | const Matrix x); 13 | 14 | void tiny_IneqStatesOffset(Matrix* ineq_state, const tiny_ProblemData prob); 15 | 16 | void tiny_IneqStatesJacobian(Matrix* ineq_jac, const tiny_ProblemData prob); 17 | 18 | void tiny_ActiveIneqMask(Matrix* mask, const Matrix input_dual, 19 | const Matrix ineq); 20 | 21 | void tiny_ClampIneqDuals(Matrix* dual, const Matrix new_dual); -------------------------------------------------------------------------------- /src/tinympc/cost_lqr.c: -------------------------------------------------------------------------------- 1 | #include "cost_lqr.h" 2 | 3 | void tiny_AddStageCost(double* cost, const tiny_ProblemData prob, 4 | const Matrix x, const Matrix u, const int k) { 5 | double dx_data[prob.nstates]; 6 | Matrix dx = slap_MatrixFromArray(prob.nstates, 1, dx_data); 7 | slap_MatrixAddition(dx, x, prob.X_ref[k], -1); 8 | *cost += 0.5 * slap_QuadraticForm(dx, prob.Q, dx); 9 | Matrix du = slap_MatrixFromArray(prob.ninputs, 1, dx_data); 10 | slap_MatrixAddition(du, u, prob.U_ref[k], -1); 11 | *cost += 0.5 * slap_QuadraticForm(du, prob.R, du); 12 | } 13 | 14 | void tiny_AddTerminalCost(double* cost, const tiny_ProblemData prob, 15 | const Matrix x) { 16 | double dx_data[prob.nstates]; 17 | Matrix dx = slap_MatrixFromArray(prob.nstates, 1, dx_data); 18 | slap_MatrixAddition(dx, x, prob.X_ref[prob.nhorizon - 1], -1); 19 | *cost += 0.5 * slap_QuadraticForm(dx, prob.Qf, dx); 20 | } 21 | 22 | void tiny_ExpandStageCost(Matrix* hes_el_xx, Matrix* grad_el_x, 23 | Matrix* hes_el_uu, Matrix* grad_el_u, 24 | const tiny_ProblemData prob, const int k) { 25 | slap_Copy(*hes_el_xx, prob.Q); 26 | slap_MatMulAdd(*grad_el_x, prob.Q, prob.X_ref[k], -1, 0); 27 | slap_Copy(*hes_el_uu, prob.R); 28 | slap_MatMulAdd(*grad_el_u, prob.R, prob.U_ref[k], -1, 0); 29 | } 30 | 31 | void tiny_ExpandTerminalCost(Matrix* hes_el_xx, Matrix* grad_el_x, 32 | const tiny_ProblemData prob) { 33 | slap_Copy(*hes_el_xx, prob.Qf); 34 | slap_MatMulAdd(*grad_el_x, prob.Qf, prob.X_ref[prob.nhorizon - 1], -1, 0); 35 | } -------------------------------------------------------------------------------- /src/tinympc/cost_lqr.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "slap/slap.h" 4 | #include "data_struct.h" 5 | 6 | void tiny_AddStageCost(double* cost, const tiny_ProblemData prob, 7 | const Matrix x, const Matrix u, const int k); 8 | 9 | void tiny_AddTerminalCost(double* cost, const tiny_ProblemData prob, 10 | const Matrix x); 11 | 12 | void tiny_ExpandStageCost(Matrix* hes_el_xx, Matrix* grad_el_x, 13 | Matrix* hes_el_uu, Matrix* grad_el_u, 14 | const tiny_ProblemData prob, const int k); 15 | 16 | void tiny_ExpandTerminalCost(Matrix* hes_el_xx, Matrix* grad_el_x, 17 | const tiny_ProblemData prob); -------------------------------------------------------------------------------- /src/tinympc/data_struct.c: -------------------------------------------------------------------------------- 1 | #include "data_struct.h" 2 | 3 | void tiny_InitLtiModel(tiny_LtiModel* model) { 4 | *model = (tiny_LtiModel){ 5 | .nstates = 0, 6 | .ninputs = 0, 7 | .dt = 0.0, 8 | .A = kNullMat, 9 | .B = kNullMat, 10 | .f = kNullMat, 11 | .x0 = kNullMat, 12 | }; 13 | } 14 | 15 | void tiny_InitLtvModel(tiny_LtvModel* model) { 16 | *model = (tiny_LtvModel){ 17 | .nstates = 0, 18 | .ninputs = 0, 19 | .dt = 0.0, 20 | .A = NULL, 21 | .B = NULL, 22 | .f = NULL, 23 | .x0 = kNullMat, 24 | .get_jacobians = NULL, 25 | .get_nonlinear_dynamics = NULL, 26 | }; 27 | } 28 | 29 | void tiny_InitKnotPoint(tiny_KnotPoint* z) { 30 | *z = (tiny_KnotPoint){ 31 | .x = kNullMat, 32 | .u = kNullMat, 33 | .t = 0.0, 34 | .dt = 0.0, 35 | }; 36 | } 37 | 38 | void tiny_InitSolver(tiny_Solver* solver) { 39 | *solver = (tiny_Solver){ 40 | .reg = 1e-8, 41 | .reg_min = 1e-8, 42 | .reg_max = 1e2, 43 | .penalty = 1, 44 | .penalty_max = 1e8, 45 | .penalty_mul = 10, 46 | .max_primal_iters = 50, 47 | .max_search_iters = 5, 48 | .riccati_tol = 1e-1, 49 | .cstr_tol = 1e-4, 50 | }; 51 | } 52 | 53 | void tiny_InitProblemData(tiny_ProblemData* prob) { 54 | *prob = (tiny_ProblemData){ 55 | .nstates = 0, 56 | .ninputs = 0, 57 | .nhorizon = 0, 58 | .ncstr_states = 0, 59 | .ncstr_inputs = 0, 60 | .ncstr_goal = 0, 61 | .Q = kNullMat, 62 | .R = kNullMat, 63 | .q = kNullMat, 64 | .r = kNullMat, 65 | .Qf = kNullMat, 66 | .qf = kNullMat, 67 | .u_max = kNullMat, 68 | .u_min = kNullMat, 69 | .x_max = kNullMat, 70 | .x_min = kNullMat, 71 | .X_ref = NULL, 72 | .U_ref = NULL, 73 | .dt = 0.0, 74 | .x0 = kNullMat, 75 | .K = NULL, 76 | .d = NULL, 77 | .P = NULL, 78 | .p = NULL, 79 | .input_duals = NULL, 80 | .state_duals = NULL, 81 | .goal_dual = kNullMat, 82 | }; 83 | } 84 | -------------------------------------------------------------------------------- /src/tinympc/data_struct.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "slap/slap.h" 4 | 5 | #define kNullMat \ 6 | ((Matrix){ \ 7 | 0, \ 8 | 0, \ 9 | 0, \ 10 | 0, \ 11 | NULL, \ 12 | slap_DENSE, \ 13 | }) 14 | 15 | 16 | // TODO (sschoedel): replace tiny_LtiModel and tiny_LtvModel with tiny_Model 17 | typedef struct { 18 | int nstates; 19 | int ninputs; 20 | double dt; 21 | Matrix A; 22 | Matrix B; 23 | Matrix f; 24 | Matrix x0; 25 | // int data_size; ///< number of doubles need to store the data 26 | } tiny_LtiModel; 27 | 28 | typedef struct { 29 | int nstates; 30 | int ninputs; 31 | double dt; 32 | Matrix* A; 33 | Matrix* B; 34 | Matrix* f; 35 | Matrix x0; 36 | void (*get_jacobians)(Matrix*, Matrix*, const Matrix, const Matrix); 37 | void (*get_nonlinear_dynamics)(Matrix*, const Matrix, const Matrix); 38 | // int data_size; 39 | } tiny_LtvModel; 40 | 41 | enum modelType {kTimeInvariant = 0, kTimeVariant = 1}; 42 | 43 | typedef struct { 44 | enum modelType type; 45 | int nstates; 46 | int ninputs; 47 | double dt; 48 | Matrix* A; 49 | Matrix* B; 50 | Matrix* f; 51 | Matrix x0; 52 | void (*get_jacobians)(Matrix*, Matrix*, const Matrix, const Matrix); 53 | void (*get_nonlinear_dynamics)(Matrix*, const Matrix, const Matrix); 54 | // int data_size; 55 | } tiny_Model; 56 | 57 | void tiny_InitLtiModel(tiny_LtiModel* model); 58 | void tiny_InitLtvModel(tiny_LtvModel* model); 59 | 60 | typedef struct { 61 | Matrix x; ///< state vector 62 | Matrix u; ///< control input vector 63 | double t; ///< time 64 | double dt; ///< time step 65 | } tiny_KnotPoint; 66 | 67 | void tiny_InitKnotPoint(tiny_KnotPoint* z); 68 | 69 | typedef struct { 70 | double reg; 71 | double reg_min; 72 | double reg_max; 73 | double penalty; 74 | double penalty_max; 75 | double penalty_mul; 76 | int max_primal_iters; 77 | int max_search_iters; 78 | double riccati_tol; 79 | double cstr_tol; 80 | } tiny_Solver; 81 | 82 | void tiny_InitSolver(tiny_Solver* solver); 83 | 84 | typedef struct tiny_ProblemData { 85 | int nstates; 86 | int ninputs; 87 | int nhorizon; 88 | int ncstr_states; 89 | int ncstr_inputs; 90 | int ncstr_goal; 91 | Matrix Q; 92 | Matrix R; 93 | Matrix q; 94 | Matrix r; 95 | Matrix Qf; 96 | Matrix qf; 97 | Matrix u_max; 98 | Matrix u_min; 99 | Matrix x_max; 100 | Matrix x_min; 101 | Matrix* X_ref; 102 | Matrix* U_ref; 103 | double dt; 104 | Matrix x0; 105 | Matrix* K; 106 | Matrix* d; 107 | Matrix* P; 108 | Matrix* p; 109 | Matrix* input_duals; 110 | Matrix* state_duals; 111 | Matrix goal_dual; 112 | // int data_size; 113 | } tiny_ProblemData; 114 | 115 | void tiny_InitProblemData(tiny_ProblemData* prob); -------------------------------------------------------------------------------- /src/tinympc/dynamics_lti.c: -------------------------------------------------------------------------------- 1 | #include "dynamics_lti.h" 2 | 3 | void tiny_DynamicsLti(Matrix* xn, const Matrix x, const Matrix u, 4 | const tiny_LtiModel model) { 5 | slap_Copy(*xn, model.f); 6 | slap_MatMulAdd(*xn, model.A, x, 1, 1); // x[k+1] += A * x[k] 7 | slap_MatMulAdd(*xn, model.B, u, 1, 1); // x[k+1] += B * u[k] 8 | } 9 | 10 | // Roll out the closed-loop dynamics with K and d from backward pass, 11 | // calculate new X, U in place 12 | enum slap_ErrorCode tiny_ForwardPassLti(Matrix* X, Matrix* U, 13 | const tiny_ProblemData prob, 14 | const tiny_LtiModel model) { 15 | int N = prob.nhorizon; 16 | for (int k = 0; k < N - 1; ++k) { 17 | // delta_x and delta_u over previous X, U 18 | // Control input: u = uf - d - K*(x - xf) 19 | slap_Copy(U[k], prob.d[k]); // u[k] = d[k] 20 | slap_MatMulAdd(U[k], prob.K[k], X[k], -1, -1); // u[k] += K[k] * x[k] 21 | // Next state: x = A*x + B*u + f 22 | tiny_DynamicsLti(&X[k + 1], X[k], U[k], model); 23 | } 24 | return SLAP_NO_ERROR; 25 | } -------------------------------------------------------------------------------- /src/tinympc/dynamics_lti.h: -------------------------------------------------------------------------------- 1 | #include "slap/slap.h" 2 | #include "data_struct.h" 3 | 4 | void tiny_DynamicsLti(Matrix* xn, const Matrix x, const Matrix u, 5 | const tiny_LtiModel model); 6 | 7 | enum slap_ErrorCode tiny_ForwardPassLti(Matrix* X, Matrix* U, 8 | const tiny_ProblemData prob, 9 | const tiny_LtiModel model); -------------------------------------------------------------------------------- /src/tinympc/dynamics_ltv.c: -------------------------------------------------------------------------------- 1 | #include "dynamics_ltv.h" 2 | 3 | void tiny_DynamicsLtv(Matrix* xn, const Matrix x, const Matrix u, 4 | const tiny_LtvModel model, const int k) { 5 | slap_Copy(*xn, model.f[k]); 6 | slap_MatMulAdd(*xn, model.A[k], x, 1, 1); // x[k+1] += A * x[k] 7 | slap_MatMulAdd(*xn, model.B[k], u, 1, 1); // x[k+1] += B * u[k] 8 | } 9 | 10 | // Roll out the closed-loop dynamics with K and d from backward pass, 11 | // provided function to calculate Ak, Bk. Calculate new X, U in place 12 | enum slap_ErrorCode tiny_ForwardPassLtv(Matrix* X, Matrix* U, 13 | const tiny_ProblemData prob, 14 | const tiny_LtvModel model) { 15 | int N = prob.nhorizon; 16 | for (int k = 0; k < N - 1; ++k) { 17 | // Control input: u = - d - K*x 18 | slap_Copy(U[k], prob.d[k]); // u[k] = -d[k] 19 | slap_MatMulAdd(U[k], prob.K[k], X[k], -1, -1); // u[k] -= K[k] * x[k] 20 | // Next state: x = A*x + B*u + f 21 | tiny_DynamicsLtv(&X[k + 1], X[k], U[k], model, k); 22 | } 23 | return SLAP_NO_ERROR; 24 | } 25 | 26 | void tiny_UpdateHorizonJacobians(tiny_LtvModel* model, tiny_ProblemData prob) { 27 | for (int i = 0; i < prob.nhorizon - 1; ++i) { 28 | model->get_jacobians(&(model->A[i]), &(model->B[i]), prob.X_ref[i], 29 | prob.U_ref[i]); 30 | if (model->get_nonlinear_dynamics != NULL) { 31 | model->get_nonlinear_dynamics(&(model->f[i]), prob.X_ref[i], 32 | prob.U_ref[i]); 33 | slap_MatMulAdd(model->f[i], model->A[i], prob.X_ref[i], -1, 1); 34 | slap_MatMulAdd(model->f[i], model->B[i], prob.U_ref[i], -1, 1); 35 | } 36 | } 37 | } -------------------------------------------------------------------------------- /src/tinympc/dynamics_ltv.h: -------------------------------------------------------------------------------- 1 | #include "data_struct.h" 2 | 3 | void tiny_DynamicsLtv(Matrix* xn, const Matrix x, const Matrix u, 4 | const tiny_LtvModel model, const int k); 5 | 6 | enum slap_ErrorCode tiny_ForwardPassLtv(Matrix* X, Matrix* U, 7 | const tiny_ProblemData prob, 8 | const tiny_LtvModel model); 9 | 10 | void tiny_UpdateHorizonJacobians(tiny_LtvModel* model, tiny_ProblemData prob); -------------------------------------------------------------------------------- /src/tinympc/lqr_lti.c: -------------------------------------------------------------------------------- 1 | #include "lqr_lti.h" 2 | 3 | // Riccati recursion for LTI without constraints 4 | enum slap_ErrorCode tiny_BackwardPassLti(tiny_ProblemData* prob, 5 | const tiny_Solver solver, 6 | const tiny_LtiModel model, 7 | Matrix* Q_temp) { 8 | int N = prob->nhorizon; 9 | int n = prob->nstates; 10 | int m = prob->ninputs; 11 | tiny_ExpandTerminalCost(&(prob->P[N - 1]), &(prob->p[N - 1]), *prob); 12 | 13 | Matrix Qxx = slap_CreateSubMatrix(*Q_temp, 0, 0, n, n); 14 | Matrix Qxu = slap_CreateSubMatrix(*Q_temp, 0, n, n, m); 15 | Matrix Qux = slap_CreateSubMatrix(*Q_temp, n, 0, m, n); 16 | Matrix Quu = slap_CreateSubMatrix(*Q_temp, n, n, m, m); 17 | Matrix Qx = slap_CreateSubMatrix(*Q_temp, 0, n + m, n, 1); 18 | Matrix Qu = slap_CreateSubMatrix(*Q_temp, n, n + m, m, 1); 19 | 20 | // Hijack the first part of P[N] to use for Cholesky decomposition 21 | // NOTE: Assumes m <= n 22 | Matrix Quu_temp = slap_Reshape(prob->P[N - 1], m, m); 23 | for (int k = N - 2; k >= 0; --k) { 24 | // Stage cost expansion 25 | tiny_ExpandStageCost(&Qxx, &Qx, &Quu, &Qu, *prob, k); 26 | // State Gradient: Qx = q + A'(P*f + p) 27 | slap_Copy(prob->p[k], prob->p[k + 1]); 28 | slap_MatMulAdd(prob->p[k], prob->P[k + 1], model.f, 1, 29 | 1); // p[k] = P[k+1]*f + p[k+1] 30 | slap_MatMulAdd(Qx, slap_Transpose(model.A), prob->p[k], 1, 1); 31 | 32 | // Control Gradient: Qu = r + B'(P*f + p) 33 | slap_MatMulAdd(Qu, slap_Transpose(model.B), prob->p[k], 1, 1); 34 | 35 | // State Hessian: Qxx = Q + A'P*A 36 | slap_MatMulAdd(prob->P[k], prob->P[k + 1], model.A, 1, 37 | 0); // P[k] = P[k+1]*A 38 | slap_MatMulAdd(Qxx, slap_Transpose(model.A), prob->P[k], 1, 39 | 1); // Qxx = Q + A'P*A 40 | 41 | // Control Hessian Quu = R + B'P*B 42 | slap_MatMulAdd(Qxu, prob->P[k + 1], model.B, 1, 0); // Qxu = P * B 43 | slap_MatMulAdd(Quu, slap_Transpose(model.B), Qxu, 1, 1); // Quu = R + B'P*B 44 | slap_MatMulAdd(Quu, slap_Transpose(model.B), model.B, solver.reg, 1); 45 | // Hessian Cross-Term 46 | slap_MatMulAdd(Qux, slap_Transpose(model.B), prob->P[k], 1, 47 | 0); // Qux = B'P*A 48 | 49 | // Calculate Gains 50 | slap_Copy(Quu_temp, Quu); 51 | slap_Cholesky(Quu_temp); 52 | slap_Copy(prob->K[k], Qux); 53 | slap_Copy(prob->d[k], Qu); 54 | slap_CholeskySolve(Quu_temp, prob->d[k]); // d = Quu\Qu 55 | slap_CholeskySolve(Quu_temp, prob->K[k]); // K = Quu\Qux 56 | 57 | // Cost-to-Go Hessian: P = Qxx + K'Quu*K - K'Qux - Qux'K 58 | slap_Copy(prob->P[k], Qxx); // P = Qxx 59 | slap_MatMulAdd(Qxu, slap_Transpose(prob->K[k]), Quu, 1, 0); // Qxu = K'Quu 60 | slap_MatMulAdd(prob->P[k], Qxu, prob->K[k], 1, 1); // P += K'Quu*K 61 | slap_MatMulAdd(prob->P[k], slap_Transpose(prob->K[k]), Qux, -2, 62 | 1); // P -= K'Qux 63 | // slap_MatMulAdd(prob->P[k], slap_Transpose(Qux), prob->K[k], -1, 64 | // 1); // P -= Qux'K 65 | 66 | // Cost-to-Go Gradient: p = Qx + K'Quu*d - K'Qu - Qux'd 67 | slap_Copy(prob->p[k], Qx); // p = Qx 68 | slap_MatMulAdd(prob->p[k], Qxu, prob->d[k], 1, 1); // p += K'Quu*d 69 | slap_MatMulAdd(prob->p[k], slap_Transpose(prob->K[k]), Qu, -1, 70 | 1); // p -= K'Qu 71 | slap_MatMulAdd(prob->p[k], slap_Transpose(Qux), prob->d[k], -1, 72 | 1); // p -= Qux'd 73 | } 74 | tiny_ExpandTerminalCost(&(prob->P[N - 1]), &(prob->p[N - 1]), *prob); 75 | // Replace P[N] since we used it for Quu_temp (need improving later) 76 | return SLAP_NO_ERROR; 77 | } -------------------------------------------------------------------------------- /src/tinympc/lqr_lti.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "cost_lqr.h" 3 | #include "dynamics_lti.h" 4 | #include "data_struct.h" 5 | 6 | // A and B are computed before LQR 7 | enum slap_ErrorCode tiny_BackwardPassLti(tiny_ProblemData* prob, 8 | const tiny_Solver solver, 9 | const tiny_LtiModel model, 10 | Matrix* Q_temp); 11 | -------------------------------------------------------------------------------- /src/tinympc/lqr_ltv.c: -------------------------------------------------------------------------------- 1 | #include "lqr_ltv.h" 2 | 3 | enum slap_ErrorCode tiny_BackwardPassLtv(tiny_ProblemData* prob, 4 | const tiny_Solver solver, 5 | const tiny_LtvModel model, 6 | Matrix* Q_temp) { 7 | int N = prob->nhorizon; 8 | int n = prob->nstates; 9 | int m = prob->ninputs; 10 | tiny_ExpandTerminalCost(&(prob->P[N - 1]), &(prob->p[N - 1]), *prob); 11 | 12 | Matrix Qxx = slap_CreateSubMatrix(*Q_temp, 0, 0, n, n); 13 | Matrix Qxu = slap_CreateSubMatrix(*Q_temp, 0, n, n, m); 14 | Matrix Qux = slap_CreateSubMatrix(*Q_temp, n, 0, m, n); 15 | Matrix Quu = slap_CreateSubMatrix(*Q_temp, n, n, m, m); 16 | Matrix Qx = slap_CreateSubMatrix(*Q_temp, 0, n + m, n, 1); 17 | Matrix Qu = slap_CreateSubMatrix(*Q_temp, n, n + m, m, 1); 18 | 19 | // Hijack the first part of P[N] to use for Cholesky decomposition 20 | // NOTE: Assumes m <= n 21 | Matrix Quu_temp = slap_Reshape(prob->P[N - 1], m, m); 22 | for (int k = N - 2; k >= 0; --k) { 23 | // Stage cost expansion 24 | tiny_ExpandStageCost(&Qxx, &Qx, &Quu, &Qu, *prob, k); 25 | // State Gradient: Qx = q + A'(P*f + p) 26 | slap_Copy(prob->p[k], prob->p[k + 1]); 27 | slap_MatMulAdd(prob->p[k], prob->P[k + 1], model.f[k], 1, 28 | 1); // p[k] = P[k+1]*f + p[k+1] 29 | slap_MatMulAdd(Qx, slap_Transpose(model.A[k]), prob->p[k], 1, 1); 30 | // slap_PrintMatrix(model.f[k]); 31 | // Control Gradient: Qu = r + B'(P*f + p) 32 | slap_MatMulAdd(Qu, slap_Transpose(model.B[k]), prob->p[k], 1, 1); 33 | 34 | // State Hessian: Qxx = Q + A'P*A 35 | slap_MatMulAdd(prob->P[k], prob->P[k + 1], model.A[k], 1, 36 | 0); // P[k] = P[k+1]*A 37 | slap_MatMulAdd(Qxx, slap_Transpose(model.A[k]), prob->P[k], 1, 38 | 1); // Qxx = Q + A'P*A 39 | 40 | // Control Hessian Quu = R + B'P*B 41 | slap_MatMulAdd(Qxu, prob->P[k + 1], model.B[k], 1, 0); // Qxu = P * B 42 | slap_MatMulAdd(Quu, slap_Transpose(model.B[k]), Qxu, 1, 43 | 1); // Quu = R + B'P*B 44 | slap_MatMulAdd(Quu, slap_Transpose(model.B[k]), model.B[k], solver.reg, 1); 45 | // Hessian Cross-Term 46 | slap_MatMulAdd(Qux, slap_Transpose(model.B[k]), prob->P[k], 1, 47 | 0); // Qux = B'P*A 48 | 49 | // Calculate Gains 50 | slap_Copy(Quu_temp, Quu); 51 | slap_Cholesky(Quu_temp); 52 | slap_Copy(prob->K[k], Qux); 53 | slap_Copy(prob->d[k], Qu); 54 | slap_CholeskySolve(Quu_temp, prob->d[k]); // d = Quu\Qu 55 | slap_CholeskySolve(Quu_temp, prob->K[k]); // K = Quu\Qux 56 | 57 | // Cost-to-Go Hessian: P = Qxx + K'Quu*K - K'Qux - Qux'K 58 | slap_Copy(prob->P[k], Qxx); // P = Qxx 59 | slap_MatMulAdd(Qxu, slap_Transpose(prob->K[k]), Quu, 1, 0); // Qxu = K'Quu 60 | slap_MatMulAdd(prob->P[k], Qxu, prob->K[k], 1, 1); // P += K'Quu*K 61 | slap_MatMulAdd(prob->P[k], slap_Transpose(prob->K[k]), Qux, -2, 62 | 1); // P -= K'Qux 63 | // slap_MatMulAdd(prob->P[k], slap_Transpose(Qux), prob->K[k], -1, 64 | // 1); // P -= Qux'K 65 | 66 | // Cost-to-Go Gradient: p = Qx + K'Quu*d - K'Qu - Qux'd 67 | slap_Copy(prob->p[k], Qx); // p = Qx 68 | slap_MatMulAdd(prob->p[k], Qxu, prob->d[k], 1, 1); // p += K'Quu*d 69 | slap_MatMulAdd(prob->p[k], slap_Transpose(prob->K[k]), Qu, -1, 70 | 1); // p -= K'Qu 71 | slap_MatMulAdd(prob->p[k], slap_Transpose(Qux), prob->d[k], -1, 72 | 1); // p -= Qux'd 73 | } 74 | tiny_ExpandTerminalCost(&(prob->P[N - 1]), &(prob->p[N - 1]), *prob); 75 | // Replace P[N] since we used it for Quu_temp (need improving later) 76 | return SLAP_NO_ERROR; 77 | } -------------------------------------------------------------------------------- /src/tinympc/lqr_ltv.h: -------------------------------------------------------------------------------- 1 | #include "cost_lqr.h" 2 | #include "dynamics_ltv.h" 3 | #include "data_struct.h" 4 | 5 | enum slap_ErrorCode tiny_BackwardPassLtv(tiny_ProblemData* prob, 6 | const tiny_Solver solver, 7 | const tiny_LtvModel model, 8 | Matrix* Q_temp); 9 | 10 | // TODO: Allow online A, B computation? (require less memory but more time) 11 | // enum slap_ErrorCode tiny_BackwardPassLtvf( 12 | // tiny_ProblemData* prob, const tiny_Solver solver, 13 | // const tiny_LtvModel model, Matrix* Q_temp); -------------------------------------------------------------------------------- /src/tinympc/mpc_lti.c: -------------------------------------------------------------------------------- 1 | #include "mpc_lti.h" 2 | 3 | // Riccati recursion for LTI with constraints 4 | enum slap_ErrorCode tiny_ConstrainedBackwardPassLti( 5 | tiny_ProblemData* prob, const tiny_Solver solver, const tiny_LtiModel model, 6 | const Matrix* X, const Matrix* U, Matrix* Q_temp, Matrix* ineq_temp) { 7 | // Copy terminal cost-to-go 8 | int N = prob->nhorizon; 9 | int n = prob->nstates; 10 | int m = prob->ninputs; 11 | tiny_ExpandTerminalCost(&(prob->P[N - 1]), &(prob->p[N - 1]), *prob); 12 | 13 | Matrix Qxx = slap_CreateSubMatrix(*Q_temp, 0, 0, n, n); 14 | Matrix Qxu = slap_CreateSubMatrix(*Q_temp, 0, n, n, m); 15 | Matrix Qux = slap_CreateSubMatrix(*Q_temp, n, 0, m, n); 16 | Matrix Quu = slap_CreateSubMatrix(*Q_temp, n, n, m, m); 17 | Matrix Qx = slap_CreateSubMatrix(*Q_temp, 0, n + m, n, 1); 18 | Matrix Qu = slap_CreateSubMatrix(*Q_temp, n, n + m, m, 1); 19 | 20 | // Hijack the first part of P[N] to use for Cholesky decomposition 21 | // NOTE: Assumes m <= n 22 | Matrix Quu_temp = slap_Reshape(prob->P[N - 1], m, m); 23 | 24 | Matrix ineq_input = 25 | slap_CreateSubMatrix(*ineq_temp, 0, 0, prob->ncstr_inputs, 1); 26 | Matrix ineq_input2 = 27 | slap_CreateSubMatrix(*ineq_temp, 0, 1, prob->ncstr_inputs, 1); 28 | Matrix mask_input = slap_CreateSubMatrix(*ineq_temp, 0, 2, prob->ncstr_inputs, 29 | prob->ncstr_inputs); 30 | Matrix ineq_input_jac = slap_CreateSubMatrix( 31 | *ineq_temp, 0, prob->ncstr_inputs + 2, prob->ncstr_inputs, prob->ninputs); 32 | Matrix ineq_input_jac2 = slap_CreateSubMatrix( 33 | *ineq_temp, 0, prob->ncstr_inputs + 2 + prob->ninputs, prob->ncstr_inputs, 34 | prob->ninputs); 35 | 36 | Matrix ineq_state = 37 | slap_CreateSubMatrix(*ineq_temp, 0, 0, prob->ncstr_states, 1); 38 | Matrix ineq_state2 = 39 | slap_CreateSubMatrix(*ineq_temp, 0, 1, prob->ncstr_states, 1); 40 | Matrix mask_state = slap_CreateSubMatrix(*ineq_temp, 0, 2, prob->ncstr_states, 41 | prob->ncstr_states); 42 | Matrix ineq_state_jac = slap_CreateSubMatrix( 43 | *ineq_temp, 0, prob->ncstr_states + 2, prob->ncstr_states, prob->nstates); 44 | Matrix ineq_state_jac2 = slap_CreateSubMatrix( 45 | *ineq_temp, 0, prob->ncstr_states + 2 + prob->nstates, prob->ncstr_states, 46 | prob->nstates); 47 | 48 | //========= Goal constraints ========== 49 | slap_Copy(Qx, prob->X_ref[N - 1]); // h = xg 50 | slap_SetIdentity(Qxx, 1); // H constant 51 | slap_MatrixAddition(prob->p[N - 2], prob->goal_dual, Qx, 52 | -solver.penalty); // (λ - ρ*h) 53 | slap_MatMulAdd(prob->p[N - 1], slap_Transpose(Qxx), prob->p[N - 2], 1, 54 | 1); // p[N] += H'*(λ - ρ*h) 55 | slap_MatMulAdd(prob->P[N - 1], slap_Transpose(Qxx), Qxx, solver.penalty, 56 | 1); // P[N] += ρ*H'H 57 | 58 | //========= State constraints at end ========== 59 | tiny_IneqStates(&ineq_state, *prob, X[N - 1]); // ineq_state size = 2*NINPUTS 60 | tiny_ActiveIneqMask(&mask_state, prob->state_duals[N - 1], ineq_state); 61 | slap_ScaleByConst(mask_state, solver.penalty); // mask = ρ*mask 62 | tiny_IneqStatesJacobian(&ineq_state_jac, *prob); 63 | // Qx += G'*(μx[k] - ρ*mask * g) 64 | tiny_IneqStatesOffset(&ineq_state, *prob); // g 65 | slap_Copy(ineq_state2, prob->state_duals[N - 1]); 66 | slap_MatMulAdd(ineq_state2, mask_state, ineq_state, -1, 67 | 1); //μx[k] - ρ*mask*g 68 | slap_MatMulAdd(prob->p[N - 1], slap_Transpose(ineq_state_jac), ineq_state2, 1, 69 | 1); 70 | // Qxx += G'*ρmask*G 71 | slap_MatMulAdd(ineq_state_jac2, mask_state, ineq_state_jac, 1, 0); 72 | slap_MatMulAdd(prob->P[N - 1], slap_Transpose(ineq_state_jac), 73 | ineq_state_jac2, 1, 1); 74 | 75 | for (int k = N - 2; k >= 0; --k) { 76 | // Stage cost expansion 77 | tiny_ExpandStageCost(&Qxx, &Qx, &Quu, &Qu, *prob, k); 78 | // State Gradient: Qx = q + A'(P*f + p) 79 | slap_Copy(prob->p[k], prob->p[k + 1]); 80 | slap_MatMulAdd(prob->p[k], prob->P[k + 1], model.f, 1, 81 | 1); // p[k] = P[k+1]*f + p[k+1] 82 | slap_MatMulAdd(Qx, slap_Transpose(model.A), prob->p[k], 1, 1); 83 | 84 | // Control Gradient: Qu = r + B'(P*f + p) 85 | slap_MatMulAdd(Qu, slap_Transpose(model.B), prob->p[k], 1, 1); 86 | 87 | // State Hessian: Qxx = Q + A'P*A 88 | slap_MatMulAdd(prob->P[k], prob->P[k + 1], model.A, 1, 89 | 0); // P[k] = P[k+1]*A 90 | slap_MatMulAdd(Qxx, slap_Transpose(model.A), prob->P[k], 1, 91 | 1); // Qxx = Q + A'P*A 92 | 93 | // Control Hessian Quu = R + B'P*B 94 | slap_MatMulAdd(Qxu, prob->P[k + 1], model.B, 1, 0); // Qxu = P * B 95 | slap_MatMulAdd(Quu, slap_Transpose(model.B), Qxu, 1, 1); // Quu = R + B'P*B 96 | slap_MatMulAdd(Quu, slap_Transpose(model.B), model.B, solver.reg, 1); 97 | // Hessian Cross-Term 98 | slap_MatMulAdd(Qux, slap_Transpose(model.B), prob->P[k], 1, 99 | 0); // Qux = B'P*A 100 | 101 | //========= Control constraints ========== 102 | tiny_IneqInputs(&ineq_input, *prob, U[k]); // ineq_input size = 2*NINPUTS 103 | tiny_ActiveIneqMask(&mask_input, prob->input_duals[k], ineq_input); 104 | slap_ScaleByConst(mask_input, solver.penalty); // mask = ρ*mask 105 | tiny_IneqInputsJacobian(&ineq_input_jac, *prob); 106 | // Qu += G'*(μ[k] + (mask * g) 107 | tiny_IneqInputsOffset(&ineq_input, *prob); // g 108 | slap_Copy(ineq_input2, prob->input_duals[k]); 109 | slap_MatMulAdd(ineq_input2, mask_input, ineq_input, -1, 1); 110 | slap_MatMulAdd(Qu, slap_Transpose(ineq_input_jac), ineq_input2, 1, 1); 111 | // Quu += ∇hu'*mask*∇hu 112 | slap_MatMulAdd(ineq_input_jac2, mask_input, ineq_input_jac, 1, 0); 113 | slap_MatMulAdd(Quu, slap_Transpose(ineq_input_jac), ineq_input_jac2, 1, 1); 114 | 115 | //========= State constraints ========== 116 | tiny_IneqStates(&ineq_state, *prob, X[k]); // ineq_state size = 2*NINPUTS 117 | tiny_ActiveIneqMask(&mask_state, prob->state_duals[k], ineq_state); 118 | slap_ScaleByConst(mask_state, solver.penalty); // mask = ρ*mask 119 | tiny_IneqStatesJacobian(&ineq_state_jac, *prob); 120 | // Qx += G'*(μx[k] - ρ*mask * g) 121 | tiny_IneqStatesOffset(&ineq_state, *prob); // g 122 | slap_Copy(ineq_state2, prob->state_duals[k]); 123 | slap_MatMulAdd(ineq_state2, mask_state, ineq_state, -1, 1); 124 | slap_MatMulAdd(Qx, slap_Transpose(ineq_state_jac), ineq_state2, 1, 1); 125 | // Qxx += ρ*∇hx'*mask*∇hx 126 | slap_MatMulAdd(ineq_state_jac2, mask_state, ineq_state_jac, 1, 0); 127 | slap_MatMulAdd(Qxx, slap_Transpose(ineq_state_jac), ineq_state_jac2, 1, 1); 128 | 129 | // Calculate Gains 130 | slap_Copy(Quu_temp, Quu); 131 | slap_Cholesky(Quu_temp); 132 | slap_Copy(prob->K[k], Qux); 133 | slap_Copy(prob->d[k], Qu); 134 | slap_CholeskySolve(Quu_temp, prob->d[k]); // d = Quu\Qu 135 | slap_CholeskySolve(Quu_temp, prob->K[k]); // K = Quu\Qux 136 | 137 | // Cost-to-Go Hessian: P = Qxx + K'Quu*K - K'Qux - Qux'K 138 | slap_Copy(prob->P[k], Qxx); // P = Qxx 139 | slap_MatMulAdd(Qxu, slap_Transpose(prob->K[k]), Quu, 1, 0); // Qxu = K'Quu 140 | slap_MatMulAdd(prob->P[k], Qxu, prob->K[k], 1, 1); // P += K'Quu*K 141 | slap_MatMulAdd(prob->P[k], slap_Transpose(prob->K[k]), Qux, -2, 142 | 1); // P -= 2*K'Qux 143 | // slap_MatMulAdd(prob->P[k], slap_Transpose(Qux), prob->K[k], -1, 144 | // 1); // P -= Qux'K 145 | 146 | // Cost-to-Go Gradient: p = Qx + K'Quu*d - K'Qu - Qux'd 147 | slap_Copy(prob->p[k], Qx); // p = Qx 148 | slap_MatMulAdd(prob->p[k], Qxu, prob->d[k], 1, 1); // p += K'Quu*d 149 | slap_MatMulAdd(prob->p[k], slap_Transpose(prob->K[k]), Qu, -1, 150 | 1); // p -= K'Qu 151 | slap_MatMulAdd(prob->p[k], slap_Transpose(Qux), prob->d[k], -1, 152 | 1); // p -= Qux'd 153 | } 154 | 155 | tiny_ExpandTerminalCost(&(prob->P[N - 1]), &(prob->p[N - 1]), *prob); 156 | // Replace P[N] since we used it for Quu_temp (need improving later) 157 | return SLAP_NO_ERROR; 158 | } 159 | 160 | enum slap_ErrorCode tiny_MpcLti(Matrix* X, Matrix* U, tiny_ProblemData* prob, 161 | tiny_Solver* solver, const tiny_LtiModel model, 162 | const int verbose) { 163 | int N = prob->nhorizon; 164 | int n = prob->nstates; 165 | int m = prob->ninputs; 166 | for (int k = 0; k < N - 1; ++k) { 167 | tiny_DynamicsLti(&(X[k + 1]), X[k], U[k], model); 168 | } 169 | double G_temp_data[(n + m) * (n + m + 1)]; 170 | Matrix Q_temp = slap_MatrixFromArray(n + m, n + m + 1, G_temp_data); 171 | 172 | double ineq_temp_data[prob->ncstr_states * 173 | (prob->ncstr_states + prob->ncstr_states + 2)]; 174 | Matrix ineq_temp = slap_MatrixFromArray( 175 | prob->ncstr_states, prob->ncstr_states + 2 * prob->nstates + 2, 176 | ineq_temp_data); 177 | 178 | Matrix ineq_input = 179 | slap_CreateSubMatrix(ineq_temp, 0, 0, prob->ncstr_inputs, 1); 180 | Matrix new_input_duals = 181 | slap_CreateSubMatrix(ineq_temp, 0, 1, prob->ncstr_inputs, 1); 182 | Matrix mask_input = slap_CreateSubMatrix(ineq_temp, 0, 2, prob->ncstr_inputs, 183 | prob->ncstr_inputs); 184 | 185 | Matrix ineq_state = 186 | slap_CreateSubMatrix(ineq_temp, 0, 0, prob->ncstr_states, 1); 187 | Matrix new_state_duals = 188 | slap_CreateSubMatrix(ineq_temp, 0, 1, prob->ncstr_states, 1); 189 | Matrix mask_state = slap_CreateSubMatrix(ineq_temp, 0, 2, prob->ncstr_states, 190 | prob->ncstr_states); 191 | 192 | Matrix eq_goal = slap_MatrixFromArray(prob->ncstr_goal, 1, ineq_temp_data); 193 | 194 | double cstr_violation = 0.0; 195 | for (int iter = 0; iter < solver->max_primal_iters; ++iter) { 196 | if (verbose > 1) printf("backward pass\n"); 197 | tiny_ConstrainedBackwardPassLti(prob, *solver, model, X, U, &Q_temp, 198 | &ineq_temp); 199 | if (verbose > 1) printf("forward pass\n"); 200 | tiny_ForwardPassLti(X, U, *prob, model); 201 | 202 | if (verbose > 0) { 203 | printf("iter J ΔJ reg ρ\n"); 204 | printf("--------------------------------------------------\n"); 205 | printf("%3d %10.3e %9.2e %9.2e %9.2e\n", iter, 0.0, 0.0, 206 | solver->reg, solver->penalty); 207 | } 208 | 209 | if (verbose > 1) printf("update duals and penalty\n"); 210 | 211 | // For linear systems, only 1 iteration 212 | cstr_violation = 0.0; 213 | double norm_inf = 0.0; 214 | 215 | for (int k = 0; k < N - 1; ++k) { 216 | //========= Control constraints ========== 217 | tiny_IneqInputs(&ineq_input, *prob, 218 | U[k]); // ineq_input size = 2*NINPUTS 219 | tiny_ActiveIneqMask(&mask_input, prob->input_duals[k], ineq_input); 220 | slap_ScaleByConst(mask_input, solver->penalty); // mask = ρ*mask 221 | // Constraint violation 222 | slap_ArgMax(ineq_input, &norm_inf); 223 | norm_inf = norm_inf > 0.0 ? norm_inf : 0.0; 224 | norm_inf = norm_inf * 2; 225 | // convio = max(convio,norm(hxv + abs.(hxv),Inf)) 226 | cstr_violation = cstr_violation < norm_inf ? norm_inf : cstr_violation; 227 | // Update duals 228 | tiny_IneqInputsOffset(&ineq_input, *prob); // g 229 | slap_Copy(new_input_duals, prob->input_duals[k]); 230 | slap_MatMulAdd(new_input_duals, mask_input, ineq_input, -1, 231 | 1); //μ[k] - ρ*mask * g 232 | tiny_ClampIneqDuals(&(prob->input_duals)[k], new_input_duals); 233 | } 234 | 235 | for (int k = 0; k < N; ++k) { 236 | //========= State constraints ========== 237 | tiny_IneqStates(&ineq_state, *prob, 238 | X[k]); // ineq_input size = 2*NINPUTS 239 | tiny_ActiveIneqMask(&mask_state, prob->state_duals[k], ineq_state); 240 | slap_ScaleByConst(mask_state, solver->penalty); // mask = ρ*mask 241 | // Constraint violation 242 | slap_ArgMax(ineq_state, &norm_inf); 243 | norm_inf = norm_inf > 0.0 ? norm_inf : 0.0; 244 | norm_inf = norm_inf * 2; 245 | // convio = max(convio,norm(hxv + abs.(hxv),Inf)) 246 | cstr_violation = cstr_violation < norm_inf ? norm_inf : cstr_violation; 247 | // Update duals 248 | tiny_IneqStatesOffset(&ineq_state, *prob); // g 249 | slap_Copy(new_state_duals, prob->state_duals[k]); 250 | slap_MatMulAdd(new_state_duals, mask_state, ineq_state, -1, 251 | 1); //μ[k] - ρ*mask*g 252 | tiny_ClampIneqDuals(&(prob->state_duals)[k], new_state_duals); 253 | } 254 | 255 | //========= Goal constraints ========== 256 | slap_MatrixAddition(eq_goal, X[N - 1], prob->X_ref[N - 1], -1); 257 | norm_inf = slap_NormInf(eq_goal); 258 | cstr_violation = cstr_violation < norm_inf ? norm_inf : cstr_violation; 259 | // λ -= ρ*h 260 | slap_Copy(eq_goal, prob->X_ref[N - 1]); // h = xg 261 | slap_MatrixAddition(prob->goal_dual, prob->goal_dual, eq_goal, 262 | -solver->penalty); 263 | 264 | if (verbose > 0) printf("convio: %.6f \n\n", cstr_violation); 265 | if (cstr_violation < solver->cstr_tol) { 266 | if (verbose > 0) printf("SUCCESS!\n"); 267 | solver->penalty = 1; // reset penalty for next MPC 268 | return SLAP_NO_ERROR; 269 | } 270 | solver->penalty = solver->penalty * solver->penalty_mul; 271 | } 272 | return SLAP_NO_ERROR; 273 | } -------------------------------------------------------------------------------- /src/tinympc/mpc_lti.h: -------------------------------------------------------------------------------- 1 | #include "constraint_linear.h" 2 | #include "cost_lqr.h" 3 | #include "dynamics_lti.h" 4 | #include "data_struct.h" 5 | 6 | enum slap_ErrorCode tiny_ConstrainedBackwardPassLti( 7 | tiny_ProblemData* prob, const tiny_Solver solver, const tiny_LtiModel model, 8 | const Matrix* X, const Matrix* U, Matrix* Q_temp, Matrix* ineq_temp); 9 | 10 | enum slap_ErrorCode tiny_MpcLti(Matrix* X, Matrix* U, tiny_ProblemData* prob, 11 | tiny_Solver* solver, const tiny_LtiModel model, 12 | const int verbose); 13 | -------------------------------------------------------------------------------- /src/tinympc/mpc_ltv.h: -------------------------------------------------------------------------------- 1 | #include "constraint_linear.h" 2 | #include "cost_lqr.h" 3 | #include "dynamics_ltv.h" 4 | #include "data_struct.h" 5 | 6 | enum slap_ErrorCode tiny_ConstrainedBackwardPassLtv( 7 | tiny_ProblemData* prob, const tiny_Solver solver, const tiny_LtvModel model, 8 | const Matrix* X, const Matrix* U, Matrix* Q_temp, Matrix* ineq_temp); 9 | 10 | enum slap_ErrorCode tiny_MpcLtv(Matrix* X, Matrix* U, tiny_ProblemData* prob, 11 | tiny_Solver* solver, const tiny_LtvModel model, 12 | const int verbose); -------------------------------------------------------------------------------- /src/tinympc/tinympc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifdef __cplusplus 3 | extern "C" { 4 | #endif 5 | 6 | #include "data_struct.h" 7 | #include "utils.h" 8 | #include "cost_lqr.h" 9 | #include "constraint_linear.h" 10 | #include "dynamics_lti.h" 11 | #include "mpc_lti.h" 12 | #include "lqr_lti.h" 13 | #include "dynamics_ltv.h" 14 | #include "mpc_ltv.h" 15 | #include "lqr_ltv.h" 16 | 17 | // #if (MODEL == LTI_MODEL) 18 | // #include "dynamics_lti.h" 19 | // #if (CONSTRAINT == CONIC_CONSTRAINT) 20 | // // #include "conic_mpc_lti.h" 21 | // // #include "conic_constraint.h" 22 | // #endif 23 | // #if (CONSTRAINT == LINEAR_CONSTRAINT) 24 | // #include "mpc_lti.h" 25 | // #include "constraint.h" 26 | // #endif 27 | // #if (CONSTRAINT == NO_CONSTRAINT) 28 | // #include "lqr_lti.h" 29 | // #endif 30 | // #endif 31 | 32 | 33 | // #if (MODEL == LTV_MODEL) 34 | // #include "dynamics_ltv.h" 35 | // #if (CONSTRAINT == CONIC_CONSTRAINT) 36 | // // #include "conic_mpc_ltv.h" 37 | // // #include "conic_constraint.h" 38 | // #endif 39 | // #if (CONSTRAINT == LINEAR_CONSTRAINT) 40 | // #include "mpc_ltv.h" 41 | // #include "constraint.h" 42 | // #endif 43 | // #if (CONSTRAINT == NO_CONSTRAINT) 44 | // #include "lqr_ltv.h" 45 | // #endif 46 | // #endif 47 | 48 | #ifdef __cplusplus 49 | } 50 | #endif -------------------------------------------------------------------------------- /src/tinympc/utils.c: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | //======================================== 4 | // Read data from file 5 | //======================================== 6 | int tiny_ReadData(const char* filename, double* des, const int size, 7 | bool verbose) { 8 | FILE* input; 9 | int i; 10 | 11 | input = fopen(filename, "r"); 12 | if (!input) { 13 | if (verbose == true) 14 | fprintf(stderr, "Cannot open %s: %s.\n", filename, strerror(errno)); 15 | return EXIT_FAILURE; 16 | } 17 | 18 | for (i = 0; i < size; ++i) { 19 | if (fscanf(input, "%lf ", &(des[i])) != 1) { 20 | if (verbose == true) fprintf(stderr, "Invalid data in %s.\n", filename); 21 | fclose(input); 22 | return EXIT_FAILURE; 23 | } 24 | 25 | if (verbose == true) printf("Read %lf from %s.\n", des[i], filename); 26 | } 27 | 28 | if (ferror(input)) { 29 | fclose(input); 30 | if (verbose == true) fprintf(stderr, "Error reading %s.\n", filename); 31 | return EXIT_FAILURE; 32 | } 33 | if (fclose(input)) { 34 | if (verbose == true) fprintf(stderr, "Error closing %s.\n", filename); 35 | return EXIT_FAILURE; 36 | } 37 | 38 | if (verbose == true) printf("All doubles read successfully.\n"); 39 | 40 | return EXIT_SUCCESS; 41 | } 42 | 43 | //======================================== 44 | // Read data from file and copy the last knot point into 45 | // remaining space of the array. Useful for extend horizon at the end. 46 | //======================================== 47 | int tiny_ReadData_Extend(const char* filename, double* des, const int stride, 48 | const int size, bool verbose) { 49 | FILE* input; 50 | int i; 51 | int k = 0; 52 | input = fopen(filename, "r"); 53 | if (!input) { 54 | if (verbose == true) 55 | fprintf(stderr, "Cannot open %s: %s.\n", filename, strerror(errno)); 56 | return EXIT_FAILURE; 57 | } 58 | 59 | for (i = 0; i < size; ++i) { 60 | if (fscanf(input, "%lf ", &(des[i])) != 1) { 61 | if (verbose == true) fprintf(stderr, "Invalid data in %s.\n", filename); 62 | fclose(input); 63 | break; 64 | } 65 | 66 | if (verbose == true) printf("Read %lf from %s.\n", des[i], filename); 67 | 68 | k += 1; 69 | } 70 | 71 | if (verbose == true) 72 | printf("All doubles read successfully and now extend.\n"); 73 | 74 | int remain_cnt = (size - k) / stride; // # of remaining chunks 75 | for (i = 0; i < remain_cnt; i += 1) { 76 | for (int j = 0; j < stride; j += 1) { 77 | des[k + j + i * stride] = des[k + j - stride]; // copy 78 | } 79 | } 80 | 81 | return EXIT_SUCCESS; 82 | } 83 | 84 | //======================================== 85 | // Read data from file and copy the goal state into 86 | // remaining space of the array. Useful for extend horizon at the end. 87 | //======================================== 88 | int tiny_ReadData_ExtendGoal(const char* filename, double* des, 89 | const double* xf, const int stride, const int size, 90 | bool verbose) { 91 | FILE* input; 92 | int i; 93 | int k = 0; 94 | input = fopen(filename, "r"); 95 | if (!input) { 96 | if (verbose == true) 97 | fprintf(stderr, "Cannot open %s: %s.\n", filename, strerror(errno)); 98 | return EXIT_FAILURE; 99 | } 100 | 101 | for (i = 0; i < size; ++i) { 102 | if (fscanf(input, "%lf ", &(des[i])) != 1) { 103 | if (verbose == true) fprintf(stderr, "Invalid data in %s.\n", filename); 104 | fclose(input); 105 | break; 106 | } 107 | 108 | if (verbose == true) printf("Read %lf from %s.\n", des[i], filename); 109 | 110 | k += 1; 111 | } 112 | 113 | if (verbose == true) 114 | printf("All doubles read successfully and now extend.\n"); 115 | 116 | int remain_cnt = (size - k) / stride; // # of remaining chunks 117 | for (i = 0; i < remain_cnt; i += 1) { 118 | for (int j = 0; j < stride; j += 1) { 119 | des[k + j + i * stride] = xf[j]; // copy 120 | } 121 | } 122 | 123 | return EXIT_SUCCESS; 124 | } 125 | 126 | //======================================== 127 | // Clamp the inputs to within min max value 128 | //======================================== 129 | void tiny_Clamps(double* arr, const double* min, const double* max, 130 | const int N) { 131 | for (int k = 0; k < N; ++k) { 132 | arr[k] = (arr[k] > max[k]) ? max[k] : ((arr[k] < min[k]) ? min[k] : arr[k]); 133 | } 134 | } 135 | 136 | void tiny_Clamp(double* arr, const double min, const double max, const int N) { 137 | for (int k = 0; k < N; ++k) { 138 | arr[k] = (arr[k] > max) ? max : ((arr[k] < min) ? min : arr[k]); 139 | } 140 | } 141 | 142 | // Clamp all data for matrix or vector 143 | void tiny_ClampMatrix(Matrix* mat, const Matrix min, const Matrix max) { 144 | tiny_Clamps(mat->data, min.data, max.data, (mat->rows) * (mat->cols)); 145 | } -------------------------------------------------------------------------------- /src/tinympc/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "slap/slap.h" 10 | 11 | //======================================== 12 | // Print matrix with its name (dummy) 13 | //======================================== 14 | #define tiny_Print(mat) \ 15 | { \ 16 | printf("\n%s = \n", #mat); \ 17 | slap_PrintMatrix(mat); \ 18 | } 19 | 20 | //======================================== 21 | // Read data from file 22 | //======================================== 23 | int tiny_ReadData(const char* filename, double* des, const int size, 24 | bool verbose); 25 | 26 | //======================================== 27 | // Read data from file and copy the last knot point into 28 | // remaining space of the array. Useful for extend horizon at the end. 29 | //======================================== 30 | int tiny_ReadData_Extend(const char* filename, double* des, const int stride, 31 | const int size, bool verbose); 32 | 33 | //======================================== 34 | // Read data from file and copy the goal state into 35 | // remaining space of the array. Useful for extend horizon at the end. 36 | //======================================== 37 | int tiny_ReadData_ExtendGoal(const char* filename, double* des, 38 | const double* xf, const int stride, const int size, 39 | bool verbose); 40 | 41 | //======================================== 42 | // Clamp the inputs to within min max value, 43 | // will modify the provided array 44 | //======================================== 45 | void tiny_Clamps(double* arr, const double* min, const double* max, 46 | const int N); 47 | 48 | void tiny_Clamp(double* arr, const double min, const double max, const int N); 49 | 50 | void tiny_ClampMatrix(Matrix* mat, const Matrix min, const Matrix max); -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # function add_al_lqr_test(name) 2 | # 3 | # Adds a new executable test called _test. 4 | # Assumes thes source code is in a file called _test. 5 | function(add_my_test name) 6 | set(TEST_NAME ${name}_test) 7 | add_executable(${TEST_NAME} 8 | ${TEST_NAME}.cpp 9 | 10 | test_utils.h 11 | test_utils.c 12 | 13 | models/bicycle_5d.h # we don't want to include for all 14 | models/bicycle_5d.c 15 | 16 | models/planar_quadrotor.h # we don't want to include for all 17 | models/planar_quadrotor.c 18 | ) 19 | target_link_libraries(${TEST_NAME} 20 | PRIVATE 21 | tinympc::tinympc 22 | slap::slap 23 | gtest::gtest 24 | ) 25 | add_test(NAME ${TEST_NAME} 26 | COMMAND ${TEST_NAME} 27 | ) 28 | endfunction() 29 | 30 | # Add tests 31 | add_my_test(al_lqr_lti) 32 | add_my_test(al_lqr_ltv) 33 | # add_my_test(back_pass) 34 | add_my_test(cost) 35 | add_my_test(forward_pass) 36 | add_my_test(lqr_ineq_utils) 37 | add_my_test(lqr_lti) 38 | add_my_test(lqr_lti_track) 39 | # add_my_test(lqr_ltv) 40 | # add_my_test(lqr_ltv_track) 41 | add_my_test(lqrdata) -------------------------------------------------------------------------------- /test/al_lqr_lti_test.cpp: -------------------------------------------------------------------------------- 1 | // Task: Test AL-LQR on double integrator with input/state box constraints and 2 | // goal constraint. Scenerio: drive from initial state to goal state. 3 | 4 | #include 5 | #include 6 | 7 | #include "test_utils.h" 8 | 9 | #define NSTATES 4 10 | #define NINPUTS 2 11 | #define NHORIZON 51 12 | // U, X, Psln 13 | 14 | class MpcLtiTest : public testing::Test { 15 | public: 16 | double A_data[NSTATES * NSTATES] = {1, 0, 0, 0, 0, 1, 0, 0, 17 | 0.1, 0, 1, 0, 0, 0.1, 0, 1}; 18 | double B_data[NSTATES * NINPUTS] = {0.005, 0, 0.1, 0, 0, 0.005, 0, 0.1}; 19 | double f_data[NSTATES] = {0}; 20 | double x0_data[NSTATES] = {5, 7, 2, -1.4}; 21 | double xg_data[NSTATES] = {0}; 22 | double Xref_data[NSTATES * NHORIZON] = {0}; 23 | double Uref_data[NINPUTS * (NHORIZON - 1)] = {0}; 24 | double X_data[NSTATES * NHORIZON] = {0}; 25 | double U_data[NINPUTS * (NHORIZON - 1)] = {0}; 26 | double K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 27 | double d_data[NINPUTS * (NHORIZON - 1)] = {0}; 28 | double P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 29 | double p_data[NSTATES * NHORIZON] = {0}; 30 | double Q_data[NSTATES * NSTATES] = {0}; 31 | double R_data[NINPUTS * NINPUTS] = {0}; 32 | double Qf_data[NSTATES * NSTATES] = {0}; 33 | double umin_data[NINPUTS] = {-6, -6}; 34 | double umax_data[NINPUTS] = {6, 6}; 35 | double xmin_data[NSTATES] = {-2, -2, -2, -2}; 36 | double xmax_data[NSTATES] = {6, 8, 3, 2}; 37 | double input_dual_data[2 * NINPUTS * (NHORIZON - 1)] = {0}; 38 | double state_dual_data[2 * NSTATES * (NHORIZON)] = {0}; 39 | double goal_dual_data[NSTATES] = {0}; 40 | 41 | tiny_LtiModel model; 42 | tiny_ProblemData prob; 43 | tiny_Solver solver; 44 | 45 | Matrix X[NHORIZON]; 46 | Matrix U[NHORIZON - 1]; 47 | Matrix Xref[NHORIZON]; 48 | Matrix Uref[NHORIZON - 1]; 49 | Matrix K[NHORIZON - 1]; 50 | Matrix d[NHORIZON - 1]; 51 | Matrix P[NHORIZON]; 52 | Matrix p[NHORIZON]; 53 | Matrix input_duals[NHORIZON - 1]; 54 | Matrix state_duals[NHORIZON]; 55 | 56 | Matrix xg; 57 | 58 | protected: 59 | void SetUp() override { 60 | tiny_InitLtiModel(&model); 61 | tiny_InitProblemData(&prob); 62 | tiny_InitSolver(&solver); 63 | 64 | model.ninputs = NSTATES; 65 | model.nstates = NINPUTS; 66 | model.A = slap_MatrixFromArray(NSTATES, NSTATES, A_data); 67 | model.B = slap_MatrixFromArray(NSTATES, NINPUTS, B_data); 68 | model.f = slap_MatrixFromArray(NSTATES, 1, f_data); 69 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 70 | xg = slap_MatrixFromArray(NSTATES, 1, xg_data); 71 | } 72 | }; 73 | 74 | TEST_F(MpcLtiTest, DoubleIntegrator) { 75 | double* Xptr = X_data; 76 | double* Xref_ptr = Xref_data; 77 | double* Uptr = U_data; 78 | double* Uref_ptr = Uref_data; 79 | double* Kptr = K_data; 80 | double* dptr = d_data; 81 | double* Pptr = P_data; 82 | double* pptr = p_data; 83 | double* udual_ptr = input_dual_data; 84 | double* xdual_ptr = state_dual_data; 85 | 86 | for (int i = 0; i < NHORIZON; ++i) { 87 | if (i < NHORIZON - 1) { 88 | U[i] = slap_MatrixFromArray(NINPUTS, 1, Uptr); 89 | slap_SetConst(U[i], 0.01); 90 | Uptr += NINPUTS; 91 | Uref[i] = slap_MatrixFromArray(NINPUTS, 1, Uref_ptr); 92 | Uref_ptr += NINPUTS; 93 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 94 | Kptr += NINPUTS * NSTATES; 95 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 96 | dptr += NINPUTS; 97 | input_duals[i] = slap_MatrixFromArray(2 * NINPUTS, 1, udual_ptr); 98 | udual_ptr += 2 * NINPUTS; 99 | } 100 | X[i] = slap_MatrixFromArray(NSTATES, 1, Xptr); 101 | Xptr += NSTATES; 102 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, Xref_ptr); 103 | slap_Copy(Xref[i], xg); 104 | Xref_ptr += NSTATES; 105 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pptr); 106 | Pptr += NSTATES * NSTATES; 107 | p[i] = slap_MatrixFromArray(NSTATES, 1, pptr); 108 | pptr += NSTATES; 109 | state_duals[i] = slap_MatrixFromArray(2 * NSTATES, 1, xdual_ptr); 110 | xdual_ptr += 2 * NSTATES; 111 | } 112 | 113 | slap_Copy(X[0], model.x0); 114 | prob.ninputs = NINPUTS; 115 | prob.nstates = NSTATES; 116 | prob.nhorizon = NHORIZON; 117 | prob.ncstr_inputs = 2 * NINPUTS; 118 | prob.ncstr_states = 2 * NSTATES; 119 | prob.ncstr_goal = NSTATES; 120 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 121 | slap_SetIdentity(prob.Q, 1e-1); 122 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 123 | slap_SetIdentity(prob.R, 1e-1); 124 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 125 | slap_SetIdentity(prob.Qf, 100 * 1e-1); 126 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, umax_data); 127 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, umin_data); 128 | prob.x_max = slap_MatrixFromArray(NSTATES, 1, xmax_data); 129 | prob.x_min = slap_MatrixFromArray(NSTATES, 1, xmin_data); 130 | prob.X_ref = Xref; 131 | prob.U_ref = Uref; 132 | prob.x0 = model.x0; 133 | prob.K = K; 134 | prob.d = d; 135 | prob.P = P; 136 | prob.p = p; 137 | prob.input_duals = input_duals; 138 | prob.state_duals = state_duals; 139 | prob.goal_dual = slap_MatrixFromArray(NSTATES, 1, goal_dual_data); 140 | 141 | solver.max_primal_iters = 10; 142 | tiny_MpcLti(X, U, &prob, &solver, model, 0); 143 | 144 | for (int k = 0; k < NHORIZON - 1; ++k) { 145 | // tiny_Print(X[k]); 146 | EXPECT_LT(slap_NormInf(U[k]), slap_NormInf(prob.u_max) + solver.cstr_tol); 147 | for (int i = 0; i < NSTATES; ++i) { 148 | EXPECT_LT(X[k].data[i], xmax_data[i] + solver.cstr_tol); 149 | EXPECT_GT(X[k].data[i], xmin_data[i] - solver.cstr_tol); 150 | } 151 | } 152 | 153 | EXPECT_LT(SumOfSquaredError(X[NHORIZON - 1].data, xg_data, NSTATES), solver.cstr_tol); 154 | } 155 | -------------------------------------------------------------------------------- /test/al_lqr_ltv_test.cpp: -------------------------------------------------------------------------------- 1 | // Test AL-TVLQR 2 | // Scenerio: Drive bicycle to track references with constraints. 3 | 4 | // === BETTER TURN OFF GOAL_CONSTRAINT IN PROJECT CMAKELISTS.TXT TO PASS === 5 | // IF BOX CONSTRAINTS OFF, CAN HANDLE GOAL CONSTRAINT 6 | // IF BOX CONSTRAINTS ON, UNLIKELY TO HANDLE GOAL CONSTRAINT 7 | // NO GRADIENT VANISHING/EXPLOSION WHEN NHORIZON = 71 (MORE MAY FAIL) 8 | // GREATER NHORIZON, GREATER ITERATION, GREATER CHANCE OF EXPLOSION 9 | // TODO: Let user choose constraints, compile options with #IFDEF 10 | 11 | #include 12 | #include 13 | 14 | #include "test_utils.h" 15 | #include "models/bicycle_5d.h" 16 | #include "data/lqr_ltv_data.h" 17 | 18 | #define NSTATES 5 19 | #define NINPUTS 2 20 | #define NHORIZON 71 21 | 22 | class MpcLtvTest : public testing::Test { 23 | public: 24 | double x0_data[NSTATES] = {1, -1, 0, 0, 0}; 25 | double Q_data[NSTATES * NSTATES] = {0}; 26 | double R_data[NINPUTS * NINPUTS] = {0}; 27 | double Qf_data[NSTATES * NSTATES] = {0}; 28 | double umin_data[NINPUTS] = {-2.1, -1.1}; 29 | double umax_data[NINPUTS] = {2.1, 1.1}; 30 | double xmin_data[NSTATES] = {-100, -100, -100, -4.0, -0.8}; 31 | double xmax_data[NSTATES] = {100, 100, 100, 4.0, 0.8}; 32 | 33 | Matrix X[NHORIZON]; 34 | Matrix U[NHORIZON - 1]; 35 | Matrix Xref[NHORIZON]; 36 | Matrix Uref[NHORIZON - 1]; 37 | Matrix K[NHORIZON - 1]; 38 | Matrix d[NHORIZON - 1]; 39 | Matrix P[NHORIZON]; 40 | Matrix p[NHORIZON]; 41 | Matrix A[NHORIZON - 1]; 42 | Matrix B[NHORIZON - 1]; 43 | Matrix f[NHORIZON - 1]; 44 | Matrix input_duals[NHORIZON - 1]; 45 | Matrix state_duals[NHORIZON]; 46 | 47 | double X_data[NSTATES * NHORIZON] = {0}; 48 | double U_data[NINPUTS * (NHORIZON - 1)] = {0}; 49 | double K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 50 | double d_data[NINPUTS * (NHORIZON - 1)] = {0}; 51 | double P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 52 | double p_data[NSTATES * NHORIZON] = {0}; 53 | double A_data[NSTATES * NSTATES * (NHORIZON - 1)] = {0}; 54 | double B_data[NSTATES * NINPUTS * (NHORIZON - 1)] = {0}; 55 | double f_data[NSTATES * (NHORIZON - 1)] = {0}; 56 | double input_dual_data[2 * NINPUTS * (NHORIZON - 1)] = {0}; 57 | double state_dual_data[2 * NSTATES * (NHORIZON)] = {0}; 58 | double goal_dual_data[NSTATES] = {0}; 59 | 60 | tiny_LtvModel model; 61 | tiny_ProblemData prob; 62 | tiny_Solver solver; 63 | 64 | private: 65 | void SetUp() override { 66 | tiny_InitLtvModel(&model); 67 | tiny_InitProblemData(&prob); 68 | tiny_InitSolver(&solver); 69 | 70 | model.ninputs = NSTATES; 71 | model.nstates = NINPUTS; 72 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 73 | model.A = A; 74 | model.B = B; 75 | model.f = f; 76 | slap_Copy(X[0], model.x0); 77 | 78 | } 79 | }; 80 | 81 | TEST_F(MpcLtvTest, BicycleTest) { 82 | 83 | model.get_jacobians = tiny_Bicycle5dGetJacobians; // from bicycle_5d.h 84 | model.get_nonlinear_dynamics = tiny_Bicycle5dNonlinearDynamics; 85 | 86 | double* Xptr = X_data; 87 | double* Xref_ptr = Xref_data; 88 | double* Uptr = U_data; 89 | double* Uref_ptr = Uref_data; 90 | double* Kptr = K_data; 91 | double* dptr = d_data; 92 | double* Pptr = P_data; 93 | double* pptr = p_data; 94 | double* Aptr = A_data; 95 | double* Bptr = B_data; 96 | double* fptr = f_data; 97 | double* udual_ptr = input_dual_data; 98 | double* xdual_ptr = state_dual_data; 99 | 100 | for (int i = 0; i < NHORIZON; ++i) { 101 | if (i < NHORIZON - 1) { 102 | A[i] = slap_MatrixFromArray(NSTATES, NSTATES, Aptr); 103 | Aptr += NSTATES * NSTATES; 104 | B[i] = slap_MatrixFromArray(NSTATES, NINPUTS, Bptr); 105 | Bptr += NSTATES * NINPUTS; 106 | f[i] = slap_MatrixFromArray(NSTATES, 1, fptr); 107 | fptr += NSTATES; 108 | U[i] = slap_MatrixFromArray(NINPUTS, 1, Uptr); 109 | // slap_SetConst(U[i], 0.01); 110 | Uptr += NINPUTS; 111 | Uref[i] = slap_MatrixFromArray(NINPUTS, 1, Uref_ptr); 112 | Uref_ptr += NINPUTS; 113 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 114 | Kptr += NINPUTS * NSTATES; 115 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 116 | dptr += NINPUTS; 117 | input_duals[i] = slap_MatrixFromArray(2 * NINPUTS, 1, udual_ptr); 118 | udual_ptr += 2 * NINPUTS; 119 | } 120 | X[i] = slap_MatrixFromArray(NSTATES, 1, Xptr); 121 | Xptr += NSTATES; 122 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, Xref_ptr); 123 | Xref_ptr += NSTATES; 124 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pptr); 125 | Pptr += NSTATES * NSTATES; 126 | p[i] = slap_MatrixFromArray(NSTATES, 1, pptr); 127 | pptr += NSTATES; 128 | state_duals[i] = slap_MatrixFromArray(2 * NSTATES, 1, xdual_ptr); 129 | xdual_ptr += 2 * NSTATES; 130 | } 131 | 132 | prob.ninputs = NINPUTS; 133 | prob.nstates = NSTATES; 134 | prob.nhorizon = NHORIZON; 135 | prob.ncstr_inputs = 2 * NINPUTS; 136 | prob.ncstr_states = 2 * NSTATES; 137 | prob.ncstr_goal = NSTATES; 138 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 139 | slap_SetIdentity(prob.Q, 10e-1); 140 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 141 | slap_SetIdentity(prob.R, 1e-1); 142 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 143 | slap_SetIdentity(prob.Qf, 10e-1); 144 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, umax_data); 145 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, umin_data); 146 | prob.x_max = slap_MatrixFromArray(NSTATES, 1, xmax_data); 147 | prob.x_min = slap_MatrixFromArray(NSTATES, 1, xmin_data); 148 | prob.X_ref = Xref; 149 | prob.U_ref = Uref; 150 | prob.x0 = model.x0; 151 | prob.K = K; 152 | prob.d = d; 153 | prob.P = P; 154 | prob.p = p; 155 | prob.input_duals = input_duals; 156 | prob.state_duals = state_duals; 157 | prob.goal_dual = slap_MatrixFromArray(NSTATES, 1, goal_dual_data); 158 | 159 | // Absolute formulation 160 | // Compute and store A, B before solving 161 | tiny_UpdateHorizonJacobians(&model, prob); 162 | 163 | solver.max_primal_iters = 50; 164 | tiny_MpcLtv(X, U, &prob, &solver, model, 0); 165 | 166 | for (int k = 0; k < NHORIZON - 1; ++k) { 167 | // printf("ex[%d] = %.4f\n", k, slap_MatrixNormedDifference(X[k], Xref[k])); 168 | // tiny_NonlinearDynamics(&X[k+1], X[k], Uref[k]); 169 | // tiny_Print(slap_Transpose(U[k])); 170 | // tiny_Print(model.B[k]); 171 | } 172 | // ========== Test ========== 173 | for (int k = 0; k < NHORIZON - 1; ++k) { 174 | // tiny_Print(X[k]); 175 | EXPECT_LT(slap_NormInf(U[k]), slap_NormInf(prob.u_max) + solver.cstr_tol); 176 | for (int i = 0; i < NSTATES; ++i) { 177 | EXPECT_LT(X[k].data[i], xmax_data[i] + solver.cstr_tol); 178 | EXPECT_GT(X[k].data[i], xmin_data[i] - solver.cstr_tol); 179 | } 180 | } 181 | for (int k = NHORIZON - 5; k < NHORIZON; ++k) { 182 | EXPECT_LT(SumOfSquaredError(X[k].data, Xref[k].data, NSTATES), 0.2); 183 | } 184 | // -------------------------- 185 | } -------------------------------------------------------------------------------- /test/back_pass_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "test_utils.h" 10 | #include "data/back_pass_data.h" 11 | 12 | #define NSTATES 4 13 | #define NINPUTS 2 14 | #define NHORIZON 3 15 | 16 | class BackPassTest : public testing::Test { 17 | public: 18 | double A_data[NSTATES * NSTATES] = {1, 0, 0, 0, 0, 1, 0, 0, 19 | 0.1, 0, 1, 0, 0, 0.1, 0, 1}; 20 | double B_data[NSTATES * NINPUTS] = {0.005, 0, 0.1, 0, 0, 0.005, 0, 0.1}; 21 | double f_data[NSTATES] = {0}; 22 | double x0_data[NSTATES] = {5, 7, 2, -1.4}; 23 | double Xref_data[NSTATES * NHORIZON] = {0}; 24 | double Uref_data[NINPUTS * (NHORIZON - 1)] = {0}; 25 | // double X_data[NSTATES*NHORIZON] = {0}; 26 | // double U_data[NINPUTS*(NHORIZON-1)] = {0}; 27 | double K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 28 | double d_data[NINPUTS * (NHORIZON - 1)] = {0}; 29 | double P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 30 | double p_data[NSTATES * NHORIZON] = {0}; 31 | double Q_data[NSTATES * NSTATES] = {0}; 32 | double R_data[NINPUTS * NINPUTS] = {0}; 33 | double Qf_data[NSTATES * NSTATES] = {0}; 34 | double umin_data[NINPUTS] = {-2, -2}; 35 | double umax_data[NINPUTS] = {2, 2}; 36 | const double tol = 1e-8; 37 | 38 | tiny_LtiModel model; 39 | tiny_ProblemData prob; 40 | tiny_Solver solver; 41 | 42 | Matrix Xref[NHORIZON]; 43 | Matrix Uref[NHORIZON - 1]; 44 | Matrix K[NHORIZON - 1]; 45 | Matrix d[NHORIZON - 1]; 46 | Matrix P[NHORIZON]; 47 | Matrix p[NHORIZON]; 48 | 49 | private: 50 | void SetUp() override { 51 | tiny_InitLtiModel(&model); 52 | tiny_InitProblemData(&prob); 53 | tiny_InitSolver(&solver); 54 | 55 | model.ninputs = NSTATES; 56 | model.nstates = NINPUTS; 57 | model.A = slap_MatrixFromArray(NSTATES, NSTATES, A_data); 58 | model.B = slap_MatrixFromArray(NSTATES, NINPUTS, B_data); 59 | model.f = slap_MatrixFromArray(NSTATES, 1, f_data); 60 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 61 | } 62 | }; 63 | 64 | // U, X, Psln 65 | TEST_F(BackPassTest, BackPass) { 66 | double* Xref_ptr = Xref_data; 67 | double* Uref_ptr = Uref_data; 68 | double* Kptr = K_data; 69 | double* dptr = d_data; 70 | double* Pptr = P_data; 71 | double* pptr = p_data; 72 | 73 | 74 | for (int i = 0; i < NHORIZON; ++i) { 75 | if (i < NHORIZON - 1) { 76 | Uref[i] = slap_MatrixFromArray(NINPUTS, 1, Uref_ptr); 77 | Uref_ptr += NINPUTS; 78 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 79 | Kptr += NINPUTS * NSTATES; 80 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 81 | dptr += NINPUTS; 82 | } 83 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, Xref_ptr); 84 | Xref_ptr += NSTATES; 85 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pptr); 86 | Pptr += NSTATES * NSTATES; 87 | p[i] = slap_MatrixFromArray(NSTATES, 1, pptr); 88 | pptr += NSTATES; 89 | } 90 | 91 | prob.ninputs = NINPUTS; 92 | prob.nstates = NSTATES; 93 | prob.nhorizon = NHORIZON; 94 | prob.ncstr_inputs = 2 * NINPUTS * (NHORIZON - 1); 95 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 96 | slap_SetIdentity(prob.Q, 1e-1); 97 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 98 | slap_SetIdentity(prob.R, 1e-1); 99 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 100 | slap_SetIdentity(prob.Qf, 100 * 1e-1); 101 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, umax_data); 102 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, umin_data); 103 | prob.X_ref = Xref; 104 | prob.U_ref = Uref; 105 | prob.x0 = model.x0; 106 | prob.K = K; 107 | prob.d = d; 108 | prob.P = P; 109 | prob.p = p; 110 | 111 | solver.reg = 1e-8; 112 | solver.penalty_mul = 10; 113 | 114 | double G_temp_data[(NSTATES + NINPUTS) * (NSTATES + NINPUTS + 1)] = {0}; 115 | Matrix G_temp = slap_MatrixFromArray(NSTATES + NINPUTS, NSTATES + NINPUTS + 1, 116 | G_temp_data); 117 | tiny_BackwardPassLti(&prob, solver, model, &G_temp); 118 | EXPECT_NEAR(SumOfSquaredError(d_data, dsln_data, (NHORIZON - 1) * NINPUTS), tol); 119 | } -------------------------------------------------------------------------------- /test/cost_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "test_utils.h" 10 | 11 | #define NSTATES 4 12 | #define NINPUTS 2 13 | #define NHORIZON 2 14 | 15 | class CostTest : public testing::Test { 16 | public: 17 | double x_data[NSTATES] = {1.1, 1.2, 1.3, -4.3}; 18 | double u_data[NSTATES] = {-2.1, 1.1}; 19 | double x_ref_data[NSTATES * NHORIZON] = {1.1, 1.2, 1.3, -4.2, 20 | 1.2, 1.3, 1.3, -4.3}; 21 | double u_ref_data[NINPUTS * NHORIZON] = {-2.1, 1.4, -2.2, 1.5}; 22 | double Q_data[NSTATES * NSTATES] = {0}; // NOLINT 23 | double R_data[NINPUTS * NINPUTS] = {0}; // NOLINT 24 | double q_data[NSTATES] = {0}; // NOLINT 25 | double r_data[NINPUTS] = {0}; // NOLINT 26 | double Qf_data[NSTATES * NSTATES] = {0}; 27 | double ans_stage[2] = {0.04549999999999994, 0.1314999999999999}; 28 | double ans_term = 0.0049999999999999975; 29 | double ans_gradx[NSTATES] = {-0.11, -0.12, -0.13, 0.42}; 30 | double ans_gradu[NINPUTS] = {0.21, -0.14}; 31 | double ans_gradxf[NSTATES] = {-0.6, -0.65, -0.65, 2.15}; 32 | 33 | const double tol = 1e-8; 34 | double cost = 0; 35 | Matrix U_ref[NHORIZON]; 36 | Matrix X_ref[NHORIZON]; 37 | double* uptr = u_ref_data; 38 | double* xptr = x_ref_data; 39 | 40 | tiny_ProblemData prob; 41 | 42 | private: 43 | void SetUp() override { 44 | for (int i = 0; i < NHORIZON; ++i) { 45 | U_ref[i] = slap_MatrixFromArray(NINPUTS, 1, uptr); 46 | uptr += NINPUTS; 47 | X_ref[i] = slap_MatrixFromArray(NSTATES, 1, xptr); 48 | xptr += NSTATES; 49 | } 50 | 51 | tiny_InitProblemData(&prob); 52 | 53 | } 54 | }; 55 | 56 | 57 | TEST_F(CostTest, AddCost) { 58 | 59 | Matrix x = slap_MatrixFromArray(NSTATES, 1, x_data); 60 | Matrix u = slap_MatrixFromArray(NINPUTS, 1, u_data); 61 | 62 | prob.nstates = NSTATES; 63 | prob.ninputs = NINPUTS; 64 | prob.nhorizon = NHORIZON; 65 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 66 | slap_SetIdentity(prob.Q, 0.1); 67 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 68 | slap_SetIdentity(prob.R, 1); 69 | prob.q = slap_MatrixFromArray(NSTATES, 1, q_data); 70 | slap_SetConst(prob.q, 1); 71 | prob.r = slap_MatrixFromArray(NINPUTS, 1, r_data); 72 | slap_SetConst(prob.r, 2); 73 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 74 | slap_SetIdentity(prob.Qf, 0.5); 75 | prob.X_ref = X_ref; 76 | prob.U_ref = U_ref; 77 | 78 | for (int k = 0; k < 2; ++k) { 79 | tiny_AddStageCost(&cost, prob, x, u, k); 80 | EXPECT_NEAR(cost, ans_stage[k], tol); 81 | } 82 | cost = 0; 83 | tiny_AddTerminalCost(&cost, prob, x); 84 | EXPECT_NEAR(cost, ans_term, tol); 85 | } 86 | 87 | TEST_F(CostTest, ExpandCost) { 88 | prob.nstates = NSTATES; 89 | prob.ninputs = NINPUTS; 90 | prob.nhorizon = NHORIZON; 91 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 92 | slap_SetIdentity(prob.Q, 0.1); 93 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 94 | slap_SetIdentity(prob.R, 0.1); 95 | prob.q = slap_MatrixFromArray(NSTATES, 1, q_data); 96 | slap_SetConst(prob.q, 1); 97 | prob.r = slap_MatrixFromArray(NINPUTS, 1, r_data); 98 | slap_SetConst(prob.r, 2); 99 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 100 | slap_SetIdentity(prob.Qf, 0.5); 101 | prob.X_ref = X_ref; 102 | prob.U_ref = U_ref; 103 | 104 | double hessx_data[NSTATES * NSTATES]; 105 | double gradx_data[NSTATES]; 106 | double hessu_data[NSTATES * NSTATES]; 107 | double gradu_data[NSTATES]; 108 | Matrix hessx = slap_MatrixFromArray(NSTATES, NSTATES, hessx_data); 109 | Matrix gradx = slap_MatrixFromArray(NSTATES, 1, gradx_data); 110 | Matrix hessu = slap_MatrixFromArray(NINPUTS, NINPUTS, hessu_data); 111 | Matrix gradu = slap_MatrixFromArray(NINPUTS, 1, gradu_data); 112 | 113 | tiny_ExpandStageCost(&hessx, &gradx, &hessu, &gradu, prob, 0); 114 | EXPECT_NEAR(SumOfSquaredError(hessx.data, Q_data, NSTATES * NSTATES), 0, tol); 115 | EXPECT_NEAR(SumOfSquaredError(hessu.data, R_data, NSTATES), 0, tol); 116 | EXPECT_NEAR(SumOfSquaredError(gradx.data, ans_gradx, NINPUTS * NINPUTS), 0, tol); 117 | EXPECT_NEAR(SumOfSquaredError(gradu.data, ans_gradu, NINPUTS), 0, tol); 118 | 119 | tiny_ExpandTerminalCost(&hessx, &gradx, prob); 120 | EXPECT_NEAR(SumOfSquaredError(hessx.data, Qf_data, NSTATES * NSTATES), 0, tol); 121 | EXPECT_NEAR(SumOfSquaredError(gradx.data, ans_gradxf, NSTATES), 0, tol); 122 | } -------------------------------------------------------------------------------- /test/data/back_pass_data.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | double X_data[12] = {5.0, 7.0, 2.0, -1.4, 5.0, 7.0, 4 | 2.0, -1.4, 5.0, 7.0, 2.0, -1.4}; 5 | 6 | double U_data[4] = {-7.522015227259879e-5, -0.0019667347526769585, 7 | 0.005620890315977286, -0.01944557581703168}; 8 | 9 | double dsln_data[4] = {10.75088111978507, 1.104847001842924, 11.238761987040757, 10 | -5.253156339850558}; 11 | -------------------------------------------------------------------------------- /test/data/forward_pass_data.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | double x_data[12] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 4 | 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; 5 | 6 | double d_data[4] = {0.6042411619304313, 0.09811826179886918, 0.5695222989883322, 7 | 0.5114280910753717}; 8 | 9 | double u_data[4] = {2.0, 2.0, 2.0, 2.0}; 10 | 11 | double K_data[16] = { 12 | 0.49686742205256307, 0.13266704729919998, 0.012970804597214558, 13 | 0.4000782856230036, 0.8217734795746234, 0.9601267910608784, 14 | 0.25209686902099326, 0.2370244586454353, 0.31367755244422746, 15 | 0.23728823461435433, 0.21899727930703738, 0.9575915918531737, 16 | 0.8260948299228765, 0.7388757501087568, 0.1600629020559956, 17 | 0.02942262316169697}; 18 | 19 | double xsol_data[12] = {1.0, 20 | 1.0, 21 | 1.0, 22 | 1.0, 23 | 1.089060251314121, 24 | 1.090859925777863, 25 | 0.7812050262824174, 26 | 0.8171985155572613, 27 | 1.1575498339012982, 28 | 1.1605012516232516, 29 | 0.5885866254611287, 30 | 0.5756280013505097}; 31 | -------------------------------------------------------------------------------- /test/data/lqr_lti_track_data.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | double Xref_data[204] = {5.0, 4 | 7.0, 5 | 2.0, 6 | -1.4, 7 | 5.160652560060507, 8 | 6.839320556675474, 9 | 1.2130512012101469, 10 | -1.8135888664905324, 11 | 5.248301469968943, 12 | 6.641397890900794, 13 | 0.5399269969585725, 14 | -2.144864449003066, 15 | 5.273732403226259, 16 | 6.413960675321315, 17 | -0.031308331812253454, 18 | -2.403879862586514, 19 | 5.246585578869316, 20 | 6.163779035158997, 21 | -0.5116281553266366, 22 | -2.599752940659822, 23 | 5.175449379562446, 24 | 5.896754823933058, 25 | -0.9110958308107685, 26 | -2.7407312838589633, 27 | 5.067948735396972, 28 | 5.618005512612837, 29 | -1.238917052498706, 30 | -2.834254942545475, 31 | 4.930828296188796, 32 | 5.331941938811838, 33 | -1.5034917316648082, 34 | -2.8870165334744953, 35 | 4.77003045942473, 36 | 5.042340179824062, 37 | -1.7124650036165288, 38 | -2.9050186462810266, 39 | 4.590768357721344, 40 | 4.752407825352388, 41 | -1.8727770304511915, 42 | -2.893628443152452, 43 | 4.397593939606499, 44 | 4.464844933391762, 45 | -1.9907113318457161, 46 | -2.8576293960600743, 47 | 4.194461301416077, 48 | 4.181899956543142, 49 | -2.0719414319627116, 50 | -2.8012701409123415, 51 | 3.984785446846749, 52 | 3.9054209266173876, 53 | -2.121575659423859, 54 | -2.7283104576027437, 55 | 3.7714966648816612, 56 | 3.636902183250262, 57 | -2.1441999798779015, 58 | -2.642064409739765, 59 | 3.557090727008504, 60 | 3.3775269278432885, 61 | -2.1439187775852435, 62 | -2.5454406983997098, 63 | 3.3436751114182965, 64 | 3.128205877872067, 65 | -2.1243935342189033, 66 | -2.440980301024724, 67 | 3.1330114656941297, 68 | 2.8896122888186353, 69 | -2.088879380264431, 70 | -2.3308914800439156, 71 | 2.9265545208073664, 72 | 2.6622136019998983, 73 | -2.040259517470838, 74 | -2.217082256330822, 75 | 2.7254876684236526, 76 | 2.4462999666536716, 77 | -1.9810775302034394, 78 | -2.1011904505937213, 79 | 2.5307554109292716, 80 | 2.242009874046007, 81 | -1.9135676196841767, 82 | -1.9846114015595697, 83 | 2.343092889527686, 84 | 2.04935313028511, 85 | -1.8396828083475338, 86 | -1.8685234736583753, 87 | 2.163052690498791, 88 | 1.8682313831465265, 89 | -1.761121172230373, 90 | -1.7539114691132918, 91 | 1.9910291234991955, 92 | 1.6984564066840826, 93 | -1.6793501677615361, 94 | -1.6415880601355841, 95 | 1.827280158820952, 96 | 1.5397663358507838, 97 | -1.5956291258033337, 98 | -1.5322133565303908, 99 | 1.6719472030019182, 100 | 1.3918400318926178, 101 | -1.5110299905773443, 102 | -1.4263127226329275, 103 | 1.5250728842524401, 104 | 1.2543097479965668, 105 | -1.4264563844122196, 106 | -1.3242929552880915, 107 | 1.3866170109673324, 108 | 1.1267722536465432, 109 | -1.342661081289936, 110 | -1.226456931712379, 111 | 1.2564708582464916, 112 | 1.0087985654273817, 113 | -1.260261973126881, 114 | -1.1330168326708547, 115 | 1.134469928951346, 116 | 0.899942421664707, 117 | -1.1797566127760313, 118 | -1.0441060425826407, 119 | 1.0204053274611706, 120 | 0.7997476283335548, 121 | -1.1015354170274778, 122 | -0.959789824040401, 123 | 0.9140338760321995, 124 | 0.7077543941373491, 125 | -1.0258936115519421, 126 | -0.8800748598837127, 127 | 0.8150870955597816, 128 | 0.6235047635690512, 129 | -0.9530419978964161, 130 | -0.8049177514822448, 131 | 0.7232791646444537, 132 | 0.5465472481283972, 133 | -0.8831166204101413, 134 | -0.7342325573308361, 135 | 0.6383139632015288, 136 | 0.47644074768716427, 137 | -0.816187408448358, 138 | -0.6678974514938218, 139 | 0.5598912994563423, 140 | 0.4127578462669362, 141 | -0.7522658664553726, 142 | -0.60576057691074, 143 | 0.4877124120514444, 144 | 0.3550875592147623, 145 | -0.6913118816425852, 146 | -0.5476451641327373, 147 | 0.42148483216843396, 148 | 0.3030376019213867, 149 | -0.6332397160176231, 150 | -0.4933539817347754, 151 | 0.3609266840402354, 152 | 0.25623624381097576, 153 | -0.577923246546348, 154 | -0.44267318047344323, 155 | 0.30577049599831035, 156 | 0.21433380532435387, 157 | -0.5252005142921532, 158 | -0.39537558925899446, 159 | 0.2557665882575768, 160 | 0.1770038500012115, 161 | -0.47487764052251763, 162 | -0.3512235172038531, 163 | 0.21068609797935717, 164 | 0.1439441185202623, 165 | -0.42673216504187494, 166 | -0.3099711124151313, 167 | 0.17032369675533646, 168 | 0.11487724665804554, 169 | -0.3805158594385393, 170 | -0.2713663248292037, 171 | 0.13450005050571714, 172 | 0.08955130455394529, 173 | -0.33595706555384697, 174 | -0.23515251725280112, 175 | 0.10306406686189558, 176 | 0.0677401903970169, 177 | -0.29276260732258425, 178 | -0.20106976588576672, 179 | 0.0758949703847101, 180 | 0.04924390765463322, 181 | -0.25061932222112504, 182 | -0.16885588896190687, 183 | 0.05290424142779979, 184 | 0.03388875121848737, 185 | -0.20919525691708116, 186 | -0.1382472397610101, 187 | 0.03403745006377965, 188 | 0.021527424324411584, 189 | -0.16814057036332172, 190 | -0.10897929812050552, 191 | 0.01927601221861064, 192 | 0.012039104782798255, 193 | -0.12708818654005855, 194 | -0.08078709271176107, 195 | 0.008638890974597507, 196 | 0.005329475909947972, 197 | -0.08565423834020407, 198 | -0.05340548474524458, 199 | 0.002184261870919275, 200 | 0.0013307345511037905, 201 | -0.04343834373336055, 202 | -0.02656934243163905, 203 | 1.1156916756909982e-5, 204 | 1.5857068697935125e-6, 205 | -2.3755349886737942e-5, 206 | -1.3634453040884537e-5}; 207 | 208 | double Uref_data[100] = { 209 | -7.869487987898531, -4.135888664905324, -6.731242042515744, 210 | -3.312755825125335, -5.712353287708259, -2.5901541358344797, 211 | -4.803198235143832, -1.9587307807330805, -3.9946767548413185, 212 | -1.4097834319914113, -3.2782122168793753, -0.9352365868651165, 213 | -2.6457467916610216, -0.5276159092902017, -2.0897327195172055, 214 | -0.18002112806531131, -1.6031202683466281, 0.11390203128574579, 215 | -1.1793430139452465, 0.3599904709237788, -0.8123010011699551, 216 | 0.5635925514773296, -0.4963422746114752, 0.729596833095979, 217 | -0.22624320454042407, 0.8624604786297864, 0.0028120229265806273, 218 | 0.9662371134005528, 0.19525243366340164, 1.044603973749857, 219 | 0.35514153954472294, 1.1008882098080828, 0.48619862793592855, 220 | 1.1380922371309359, 0.5918198726739854, 1.1589180573710058, 221 | 0.6750991051926264, 1.1657904903415173, 0.7388481133664283, 222 | 1.1608792790119447, 0.7856163611716078, 1.1461200454508353, 223 | 0.8177100446883694, 1.1232340897770756, 0.8372104195820234, 224 | 1.0937470360519321, 0.8459913522598942, 1.0590063389746323, 225 | 0.8457360616512467, 1.0201976734483607, 0.8379530312228357, 226 | 0.978360235757125, 0.8239910816305508, 0.9344009904152427, 227 | 0.8050536035084973, 0.8891079008821405, 0.7822119574855338, 228 | 0.8431621854223972, 0.7564180547553572, 0.7971496415668827, 229 | 0.7285161365552604, 0.7515710840146792, 0.6992537748627479, 230 | 0.7068519415140865, 0.6692921196178323, 0.6633510583701431, 231 | 0.6392154199298542, 0.6213687458308186, 0.6095398481278744, 232 | 0.5811541277800264, 0.5807216562496212, 0.5429118239796196, 233 | 0.5531646947127509, 0.5068080126133219, 0.5272273225419483, 234 | 0.4729759121444876, 0.5032287376963556, 0.4415207205514137, 235 | 0.481454754806427, 0.4125240478872175, 0.4621630560333569, 236 | 0.3860478758592761, 0.44558793884692294, 0.36213807576402574, 237 | 0.4319445823126274, 0.340827513670344, 0.4214328510145921, 238 | 0.3221387692385986, 0.4142406530404389, 0.3060864920089675, 239 | 0.41054686553759434, 0.29267941640504586, 0.4105238382326318, 240 | 0.28192205408744453, 0.41433948199854476, 0.2738160796651649, 241 | 0.4221589460684351, 0.26836142313605527, 0.43414588383473807, 242 | 0.26555707978598164}; 243 | -------------------------------------------------------------------------------- /test/forward_pass_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "test_utils.h" 10 | #include "data/forward_pass_data.h" 11 | 12 | #define NSTATES 4 13 | #define NINPUTS 2 14 | #define NHORIZON 3 15 | 16 | class ForwardPassTest : public testing::Test { 17 | public: 18 | double A_data[NSTATES * NSTATES] = {1, 0, 0, 0, 0, 1, 0, 0, 19 | 0.1, 0, 1, 0, 0, 0.1, 0, 1}; 20 | double B_data[NSTATES * NINPUTS] = {0.005, 0, 0.1, 0, 0, 0.005, 0, 0.1}; 21 | double f_data[NSTATES] = {0, 0, 0, 0}; 22 | // double x0_data[NSTATES] = {5,7,2,-1.4}; 23 | 24 | const double tol = 1e-8; 25 | 26 | Matrix X[NHORIZON]; 27 | Matrix Xsln[NHORIZON]; 28 | Matrix U[NHORIZON - 1]; 29 | Matrix K[NHORIZON - 1]; 30 | Matrix d[NHORIZON - 1]; 31 | 32 | tiny_LtiModel model; 33 | tiny_ProblemData prob; 34 | 35 | double* xptr = x_data; 36 | double* xsol_ptr = xsol_data; 37 | double* uptr = u_data; 38 | double* Kptr = K_data; 39 | double* dptr = d_data; 40 | 41 | private: 42 | void SetUp() override { 43 | tiny_InitLtiModel(&model); 44 | tiny_InitProblemData(&prob); 45 | 46 | model.dt = 0.1; 47 | model.ninputs = NSTATES; 48 | model.nstates = NINPUTS; 49 | model.A = slap_MatrixFromArray(NSTATES, NSTATES, A_data); 50 | model.B = slap_MatrixFromArray(NSTATES, NINPUTS, B_data); 51 | model.f = slap_MatrixFromArray(NSTATES, 1, f_data); 52 | // model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 53 | 54 | for (int i = 0; i < NHORIZON; ++i) { 55 | if (i < NHORIZON - 1) { 56 | U[i] = slap_MatrixFromArray(NINPUTS, 1, uptr); 57 | uptr += NINPUTS; 58 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 59 | Kptr += NINPUTS * NSTATES; 60 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 61 | dptr += NINPUTS; 62 | } 63 | X[i] = slap_MatrixFromArray(NSTATES, 1, xptr); 64 | xptr += NSTATES; 65 | Xsln[i] = slap_MatrixFromArray(NSTATES, 1, xsol_ptr); 66 | xsol_ptr += NSTATES; 67 | } 68 | 69 | prob.ninputs = NINPUTS; 70 | prob.nstates = NSTATES; 71 | prob.nhorizon = NHORIZON; 72 | prob.x0 = X[0]; // check if possible 73 | prob.K = K; 74 | prob.d = d; 75 | 76 | uptr = u_data; 77 | xsol_ptr = xsol_data; 78 | xptr = x_data; 79 | Kptr = K_data; 80 | dptr = d_data; 81 | } 82 | }; 83 | 84 | 85 | TEST_F(ForwardPassTest, SetUpTest) { 86 | for (int i = 0; i < NHORIZON; ++i) { 87 | if (i < NHORIZON - 1) { 88 | EXPECT_EQ(U[i].rows, NINPUTS); 89 | EXPECT_EQ(U[i].cols, 1); 90 | EXPECT_NEAR(SumOfSquaredError(U[i].data, uptr, NINPUTS), 0, tol); 91 | uptr += NINPUTS; 92 | EXPECT_EQ(prob.K[i].rows, NINPUTS); 93 | EXPECT_EQ(prob.K[i].cols, NSTATES); 94 | EXPECT_NEAR(SumOfSquaredError(prob.K[i].data, Kptr, NINPUTS * NSTATES), 0, tol); 95 | Kptr += NINPUTS * NSTATES; 96 | EXPECT_EQ(prob.d[i].rows, NINPUTS); 97 | EXPECT_EQ(prob.d[i].cols, 1); 98 | EXPECT_NEAR(SumOfSquaredError(prob.d[i].data, dptr, NINPUTS), 0, tol); 99 | dptr += NINPUTS; 100 | } 101 | EXPECT_EQ(X[i].rows, NSTATES); 102 | EXPECT_EQ(X[i].cols, 1); 103 | EXPECT_NEAR(SumOfSquaredError(X[i].data, xptr, NSTATES), 0, tol); 104 | xptr += NSTATES; 105 | EXPECT_EQ(Xsln[i].rows, NSTATES); 106 | EXPECT_EQ(Xsln[i].cols, 1); 107 | EXPECT_NEAR(SumOfSquaredError(Xsln[i].data, xsol_ptr, NSTATES), 0, tol); 108 | xsol_ptr += NSTATES; 109 | } 110 | } 111 | 112 | TEST_F(ForwardPassTest, DiscreteDynamics) { 113 | // Include discrete dynamics test 114 | tiny_ForwardPassLti(X, U, prob, model); 115 | for (int i = 0; i < NHORIZON; ++i) { 116 | EXPECT_NEAR(SumOfSquaredError(X[i].data, Xsln[i].data, NSTATES), 0, tol); 117 | } 118 | } -------------------------------------------------------------------------------- /test/lqr_ineq_utils_test.cpp: -------------------------------------------------------------------------------- 1 | // Test all inequality-related functions and tiny_RiccatiConvergence 2 | 3 | #include 4 | #include 5 | 6 | #include "test_utils.h" 7 | 8 | #define NSTATES 4 9 | #define NINPUTS 2 10 | #define NHORIZON 3 11 | 12 | class IneqLqrTest : public testing::Test { 13 | public: 14 | const double tol = 1e-8; 15 | double u_max_data[NINPUTS] = {1, 1}; 16 | double u_min_data[NINPUTS] = {-1, -1}; 17 | double u_data[NINPUTS] = {1.1, 0.8}; 18 | double ans[NINPUTS * 2] = { 19 | u_data[0] - u_max_data[0], u_data[1] - u_max_data[1], 20 | -u_data[0] + u_min_data[0], -u_data[1] + u_min_data[1]}; 21 | double ineq_data[NINPUTS * 2]; 22 | 23 | tiny_ProblemData prob; 24 | 25 | Matrix u; 26 | Matrix mat; 27 | 28 | private: 29 | void SetUp() override { 30 | tiny_InitProblemData(&prob); 31 | 32 | prob.nstates = NSTATES; 33 | prob.ninputs = NINPUTS; 34 | prob.nhorizon = NHORIZON; 35 | prob.ncstr_states = 2 * NSTATES; 36 | prob.ncstr_inputs = 2 * NINPUTS; 37 | prob.ncstr_goal = NSTATES; 38 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, u_max_data); 39 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, u_min_data); 40 | u = slap_MatrixFromArray(NINPUTS, 1, u_data); 41 | mat = slap_MatrixFromArray(NINPUTS * 2, 1, ineq_data); 42 | }; 43 | }; 44 | 45 | TEST_F(IneqLqrTest, IneqInputs) { 46 | tiny_IneqInputs(&mat, prob, u); 47 | // slap_PrintMatrix(mat); 48 | EXPECT_NEAR(SumOfSquaredError(mat.data, ans, NINPUTS * 2), 0, tol); 49 | } 50 | 51 | TEST_F(IneqLqrTest, IneqInputsOffset) { 52 | 53 | double ans1[NINPUTS * 2] = {u_max_data[0], u_max_data[1], -u_min_data[0], 54 | -u_min_data[1]}; 55 | tiny_IneqInputsOffset(&mat, prob); 56 | // slap_PrintMatrix(mat); 57 | EXPECT_NEAR(SumOfSquaredError(mat.data, ans1, NINPUTS * 2), 0, tol); 58 | } 59 | 60 | TEST_F(IneqLqrTest, IneqInputsJacobian) { 61 | double ans_jac[NINPUTS * 2 * NINPUTS] = {1, 0, -1, 0, 0, 1, 0, -1}; 62 | double jac_data[NINPUTS * 2 * NINPUTS]; 63 | Matrix jac_mat = slap_MatrixFromArray(NINPUTS * 2, NINPUTS, jac_data); 64 | tiny_IneqInputsJacobian(&jac_mat, prob); 65 | // slap_PrintMatrix(mat); 66 | EXPECT_NEAR(SumOfSquaredError(jac_mat.data, ans_jac, NINPUTS * 2 * NINPUTS), 0, tol); 67 | } 68 | 69 | TEST_F(IneqLqrTest, ActiveIneqMask) { 70 | double u_data1[NINPUTS] = {-2, 2}; 71 | double ans1[NINPUTS * 2] = {u_data1[0] - u_max_data[0], //-3 72 | u_data1[1] - u_max_data[1], // 1 73 | -u_data1[0] + u_min_data[0], // 1 74 | -u_data1[1] + u_min_data[1]}; // -3 75 | double dual_data[NINPUTS * 2] = {1, 0, 2, 0}; 76 | double ans2[NINPUTS * 2 * NINPUTS * 2] = {1, 0, 0, 0, 0, 1, 0, 0, 77 | 0, 0, 1, 0, 0, 0, 0, 0}; 78 | double ineq_data[NINPUTS * 2]; 79 | double mask_data[NINPUTS * 2 * NINPUTS * 2]; 80 | 81 | u = slap_MatrixFromArray(NINPUTS, 1, u_data1); 82 | Matrix ineq = slap_MatrixFromArray(NINPUTS * 2, 1, ineq_data); 83 | tiny_IneqInputs(&ineq, prob, u); 84 | Matrix input_dual = slap_MatrixFromArray(NINPUTS * 2, 1, dual_data); 85 | Matrix mask = slap_MatrixFromArray(NINPUTS * 2, NINPUTS * 2, mask_data); 86 | tiny_ActiveIneqMask(&mask, input_dual, ineq); 87 | // slap_PrintMatrix(mask); 88 | EXPECT_NEAR(SumOfSquaredError(ineq.data, ans1, NINPUTS * 2), 0, tol); 89 | EXPECT_NEAR(SumOfSquaredError(mask.data, ans2, NINPUTS * 2 * NINPUTS * 2), 0, tol); 90 | } 91 | 92 | TEST_F(IneqLqrTest, RiccatiConvergence) { 93 | tiny_ProblemData prob; 94 | tiny_InitProblemData(&prob); 95 | 96 | prob.nhorizon = 3; 97 | prob.ninputs = 2; 98 | double d_data[4] = {1.2, -0.3, -2.1, 3.1}; 99 | double ans = 3.744329045369811; 100 | Matrix d[2]; 101 | double* dptr = d_data; 102 | for (int k = 0; k < prob.nhorizon - 1; ++k) { 103 | d[k] = slap_MatrixFromArray(prob.ninputs, 1, dptr); 104 | dptr += prob.ninputs; 105 | } 106 | prob.d = d; 107 | double norm_d_max = tiny_RiccatiConvergence(prob); 108 | EXPECT_NEAR(norm_d_max, ans, 1e-6); 109 | } 110 | 111 | TEST_F(IneqLqrTest, ClampIneqDuals) { 112 | double dual_data[2] = {-2, -1}; 113 | double new_dual_data[2] = {10, -10}; 114 | double ans[2] = {10, 0}; 115 | Matrix dual = slap_MatrixFromArray(2, 1, dual_data); 116 | Matrix new_dual = slap_MatrixFromArray(2, 1, new_dual_data); 117 | tiny_ClampIneqDuals(&dual, new_dual); 118 | EXPECT_NEAR(SumOfSquaredError(dual.data, ans, 2), 0, 1e-8); 119 | } 120 | -------------------------------------------------------------------------------- /test/lqr_lti_test.cpp: -------------------------------------------------------------------------------- 1 | // Test LQR 2 | // Scenerio: Drive double integrator to arbitrary goal state. 3 | 4 | #include 5 | #include 6 | 7 | #include "test_utils.h" 8 | 9 | #define NSTATES 4 10 | #define NINPUTS 2 11 | #define NHORIZON 51 12 | 13 | class LqrLtiTest : public testing::Test { 14 | public: 15 | double A_data[NSTATES * NSTATES] = {1, 0, 0, 0, 0, 1, 0, 0, 16 | 0.1, 0, 1, 0, 0, 0.1, 0, 1}; 17 | double B_data[NSTATES * NINPUTS] = {0.005, 0, 0.1, 0, 0, 0.005, 0, 0.1}; 18 | double f_data[NSTATES] = {0}; 19 | double x0_data[NSTATES] = {5, 7, 2, -1.4}; 20 | double xg_data[NSTATES] = {2, 5, 3, -1}; 21 | double Xref_data[NSTATES * NHORIZON] = {0}; 22 | double Uref_data[NINPUTS * (NHORIZON - 1)] = {0}; 23 | double X_data[NSTATES * NHORIZON] = {0}; 24 | double U_data[NINPUTS * (NHORIZON - 1)] = {0}; 25 | double K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 26 | double d_data[NINPUTS * (NHORIZON - 1)] = {0}; 27 | double P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 28 | double p_data[NSTATES * NHORIZON] = {0}; 29 | double Q_data[NSTATES * NSTATES] = {0}; 30 | double R_data[NINPUTS * NINPUTS] = {0}; 31 | double Qf_data[NSTATES * NSTATES] = {0}; 32 | double umin_data[NINPUTS] = {-2, -2}; 33 | double umax_data[NINPUTS] = {2, 2}; 34 | 35 | tiny_LtiModel model; 36 | tiny_ProblemData prob; 37 | tiny_Solver solver; 38 | 39 | Matrix X[NHORIZON]; 40 | Matrix U[NHORIZON - 1]; 41 | Matrix Xref[NHORIZON]; 42 | Matrix Uref[NHORIZON - 1]; 43 | Matrix K[NHORIZON - 1]; 44 | Matrix d[NHORIZON - 1]; 45 | Matrix P[NHORIZON]; 46 | Matrix p[NHORIZON]; 47 | 48 | double* Xptr = X_data; 49 | double* Xref_ptr = Xref_data; 50 | double* Uptr = U_data; 51 | double* Uref_ptr = Uref_data; 52 | double* Kptr = K_data; 53 | double* dptr = d_data; 54 | double* Pptr = P_data; 55 | double* pptr = p_data; 56 | 57 | Matrix Q_temp; 58 | 59 | private: 60 | void SetUp() override { 61 | tiny_InitLtiModel(&model); 62 | tiny_InitProblemData(&prob); 63 | tiny_InitSolver(&solver); 64 | 65 | model.ninputs = NSTATES; 66 | model.nstates = NINPUTS; 67 | model.A = slap_MatrixFromArray(NSTATES, NSTATES, A_data); 68 | model.B = slap_MatrixFromArray(NSTATES, NINPUTS, B_data); 69 | model.f = slap_MatrixFromArray(NSTATES, 1, f_data); 70 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 71 | 72 | } 73 | }; 74 | 75 | // U, X, Psln 76 | TEST_F(LqrLtiTest, LqrLti) { 77 | Matrix xg = slap_MatrixFromArray(NSTATES, 1, xg_data); 78 | for (int i = 0; i < NHORIZON; ++i) { 79 | if (i < NHORIZON - 1) { 80 | U[i] = slap_MatrixFromArray(NINPUTS, 1, Uptr); 81 | // slap_SetConst(U[i], 0.01); 82 | Uptr += NINPUTS; 83 | Uref[i] = slap_MatrixFromArray(NINPUTS, 1, Uref_ptr); 84 | Uref_ptr += NINPUTS; 85 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 86 | Kptr += NINPUTS * NSTATES; 87 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 88 | dptr += NINPUTS; 89 | } 90 | X[i] = slap_MatrixFromArray(NSTATES, 1, Xptr); 91 | Xptr += NSTATES; 92 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, Xref_ptr); 93 | slap_Copy(Xref[i], xg); 94 | Xref_ptr += NSTATES; 95 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pptr); 96 | Pptr += NSTATES * NSTATES; 97 | p[i] = slap_MatrixFromArray(NSTATES, 1, pptr); 98 | pptr += NSTATES; 99 | } 100 | slap_Copy(X[0], model.x0); 101 | prob.ninputs = NINPUTS; 102 | prob.nstates = NSTATES; 103 | prob.nhorizon = NHORIZON; 104 | prob.ncstr_inputs = 2 * NINPUTS * (NHORIZON - 1); 105 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 106 | slap_SetIdentity(prob.Q, 1e-1); 107 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 108 | slap_SetIdentity(prob.R, 1e-1); 109 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 110 | slap_SetIdentity(prob.Qf, 1000 * 1e-1); 111 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, umax_data); 112 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, umin_data); 113 | prob.X_ref = Xref; 114 | prob.U_ref = Uref; 115 | prob.x0 = model.x0; 116 | prob.K = K; 117 | prob.d = d; 118 | prob.P = P; 119 | prob.p = p; 120 | 121 | solver.reg = 1e-8; 122 | solver.penalty_mul = 10; 123 | solver.max_primal_iters = 1; 124 | 125 | double Q_temp_data[(NSTATES + NINPUTS) * (NSTATES + NINPUTS + 1)] = {0}; 126 | Q_temp = slap_MatrixFromArray(NSTATES + NINPUTS, NSTATES + NINPUTS + 1, 127 | Q_temp_data); 128 | 129 | tiny_BackwardPassLti(&prob, solver, model, &Q_temp); 130 | tiny_ForwardPassLti(X, U, prob, model); 131 | 132 | // tiny_AugmentedLagrangianLqr(X, U, prob, model, solver, 1); 133 | // for (int k = 0; k < NHORIZON - 1; ++k) { 134 | // // tiny_Print(U[k]); 135 | // } 136 | // tiny_Print(X[NHORIZON-1]); 137 | EXPECT_NEAR(SumOfSquaredError(X[NHORIZON - 1].data, xg_data, NSTATES), 0, 1e-1); 138 | } -------------------------------------------------------------------------------- /test/lqr_lti_track_test.cpp: -------------------------------------------------------------------------------- 1 | // Test tracking LQR 2 | // Scenerio: Drive double integrator to track reference. 3 | 4 | #include 5 | #include 6 | 7 | #include "test_utils.h" 8 | #include "data/lqr_lti_track_data.h" 9 | 10 | #define NSTATES 4 11 | #define NINPUTS 2 12 | #define NHORIZON 51 13 | 14 | class LqrLtiTrackTest : public testing::Test { 15 | public: 16 | double A_data[NSTATES * NSTATES] = {1, 0, 0, 0, 0, 1, 0, 0, 17 | 0.1, 0, 1, 0, 0, 0.1, 0, 1}; 18 | double B_data[NSTATES * NINPUTS] = {0.005, 0, 0.1, 0, 0, 0.005, 0, 0.1}; 19 | double f_data[NSTATES] = {0}; 20 | double x0_data[NSTATES] = {2, 6, 3, -1.5}; 21 | double X_data[NSTATES * NHORIZON] = {0}; 22 | double U_data[NINPUTS * (NHORIZON - 1)] = {0}; 23 | double K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 24 | double d_data[NINPUTS * (NHORIZON - 1)] = {0}; 25 | double P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 26 | double p_data[NSTATES * NHORIZON] = {0}; 27 | double Q_data[NSTATES * NSTATES] = {0}; 28 | double R_data[NINPUTS * NINPUTS] = {0}; 29 | double Qf_data[NSTATES * NSTATES] = {0}; 30 | 31 | tiny_LtiModel model; 32 | tiny_ProblemData prob; 33 | tiny_Solver solver; 34 | 35 | Matrix X[NHORIZON]; 36 | Matrix U[NHORIZON - 1]; 37 | Matrix Xref[NHORIZON]; 38 | Matrix Uref[NHORIZON - 1]; 39 | Matrix K[NHORIZON - 1]; 40 | Matrix d[NHORIZON - 1]; 41 | Matrix P[NHORIZON]; 42 | Matrix p[NHORIZON]; 43 | 44 | double* Xptr = X_data; 45 | double* Xref_ptr = Xref_data; 46 | double* Uptr = U_data; 47 | double* Uref_ptr = Uref_data; 48 | double* Kptr = K_data; 49 | double* dptr = d_data; 50 | double* Pptr = P_data; 51 | double* pptr = p_data; 52 | 53 | private: 54 | void SetUp() override { 55 | tiny_InitLtiModel(&model); 56 | tiny_InitProblemData(&prob); 57 | tiny_InitSolver(&solver); 58 | 59 | model.ninputs = NSTATES; 60 | model.nstates = NINPUTS; 61 | model.A = slap_MatrixFromArray(NSTATES, NSTATES, A_data); 62 | model.B = slap_MatrixFromArray(NSTATES, NINPUTS, B_data); 63 | model.f = slap_MatrixFromArray(NSTATES, 1, f_data); 64 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 65 | } 66 | }; 67 | 68 | TEST_F(LqrLtiTrackTest, LqrLti) { 69 | for (int i = 0; i < NHORIZON; ++i) { 70 | if (i < NHORIZON - 1) { 71 | U[i] = slap_MatrixFromArray(NINPUTS, 1, Uptr); 72 | // slap_SetConst(U[i], 0.01); 73 | Uptr += NINPUTS; 74 | Uref[i] = slap_MatrixFromArray(NINPUTS, 1, Uref_ptr); 75 | Uref_ptr += NINPUTS; 76 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 77 | Kptr += NINPUTS * NSTATES; 78 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 79 | dptr += NINPUTS; 80 | } 81 | X[i] = slap_MatrixFromArray(NSTATES, 1, Xptr); 82 | Xptr += NSTATES; 83 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, Xref_ptr); 84 | Xref_ptr += NSTATES; 85 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pptr); 86 | Pptr += NSTATES * NSTATES; 87 | p[i] = slap_MatrixFromArray(NSTATES, 1, pptr); 88 | pptr += NSTATES; 89 | } 90 | slap_Copy(X[0], model.x0); 91 | prob.ninputs = NINPUTS; 92 | prob.nstates = NSTATES; 93 | prob.nhorizon = NHORIZON; 94 | prob.ncstr_inputs = 2 * NINPUTS * (NHORIZON - 1); 95 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 96 | slap_SetIdentity(prob.Q, 10e-1); 97 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 98 | slap_SetIdentity(prob.R, 1e-1); 99 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 100 | slap_SetIdentity(prob.Qf, 1000 * 1e-1); 101 | prob.X_ref = Xref; 102 | prob.U_ref = Uref; 103 | prob.x0 = model.x0; 104 | prob.K = K; 105 | prob.d = d; 106 | prob.P = P; 107 | prob.p = p; 108 | 109 | solver.reg = 1e-8; 110 | solver.penalty_mul = 10; 111 | solver.max_primal_iters = 1; 112 | 113 | double G_temp_data[(NSTATES + NINPUTS) * (NSTATES + NINPUTS + 1)] = {0}; 114 | Matrix G_temp = slap_MatrixFromArray(NSTATES + NINPUTS, NSTATES + NINPUTS + 1, 115 | G_temp_data); 116 | 117 | for (int i = 0; i < NHORIZON - 1; ++i) { 118 | slap_Copy(model.f, Xref[i + 1]); 119 | slap_MatMulAdd(model.f, model.A, Xref[i], -1, 1); 120 | slap_MatMulAdd(model.f, model.B, Uref[i], -1, 1); 121 | // tiny_Print(model.f); // Check if reference is feasible 122 | // tiny_Print(slap_Transpose(Xref[i])); 123 | } 124 | tiny_BackwardPassLti(&prob, solver, model, &G_temp); 125 | tiny_ForwardPassLti(X, U, prob, model); 126 | // // tiny_AugmentedLagrangianLqr(X, U, prob, model, solver, 1); 127 | for (int k = 0; k < NHORIZON; ++k) { 128 | // printf("ex[%d] = %.4f\n", k, slap_MatrixNormedDifference(X[k], Xref[k])); 129 | } 130 | for (int k = NHORIZON - 5; k < NHORIZON; ++k) { 131 | EXPECT_NEAR(SumOfSquaredError(X[k].data, Xref[k].data, NSTATES), 0, 1e-1); 132 | } 133 | } 134 | -------------------------------------------------------------------------------- /test/lqr_ltv_track_test.cpp: -------------------------------------------------------------------------------- 1 | // Test TVLQR 2 | // Scenerio: Drive bicycle to track references. 3 | 4 | #include 5 | #include 6 | 7 | #include "test_utils.h" 8 | #include "models/bicycle_5d.h" 9 | 10 | #define H 0.1 11 | #define NSTATES 5 12 | #define NINPUTS 2 13 | #define NHORIZON 81 14 | // NO GRADIENT VANISHING/EXPLOSION WHEN NHORIZON = 101 15 | 16 | class LqrLtvTrackTest : public testing::Test { 17 | public: 18 | double x0_data[NSTATES] = {1, -1, 0, 0, 0}; 19 | double xg_data[NSTATES] = {0}; 20 | double ug_data[NINPUTS] = {0}; 21 | double Q_data[NSTATES * NSTATES] = {0}; 22 | double R_data[NINPUTS * NINPUTS] = {0}; 23 | double Qf_data[NSTATES * NSTATES] = {0}; 24 | 25 | Matrix X[NHORIZON]; 26 | Matrix U[NHORIZON - 1]; 27 | Matrix Xref[NHORIZON]; 28 | Matrix Uref[NHORIZON - 1]; 29 | Matrix Xnom[NHORIZON]; 30 | Matrix Unom[NHORIZON - 1]; 31 | Matrix K[NHORIZON - 1]; 32 | Matrix d[NHORIZON - 1]; 33 | Matrix P[NHORIZON]; 34 | Matrix p[NHORIZON]; 35 | Matrix A[NHORIZON - 1]; 36 | Matrix B[NHORIZON - 1]; 37 | Matrix f[NHORIZON - 1]; 38 | 39 | double X_data[NSTATES * NHORIZON] = {0}; 40 | double U_data[NINPUTS * (NHORIZON - 1)] = {0}; 41 | double K_data[NINPUTS * NSTATES * (NHORIZON - 1)] = {0}; 42 | double d_data[NINPUTS * (NHORIZON - 1)] = {0}; 43 | double P_data[NSTATES * NSTATES * (NHORIZON)] = {0}; 44 | double p_data[NSTATES * NHORIZON] = {0}; 45 | double A_data[NSTATES * NSTATES * (NHORIZON - 1)] = {0}; 46 | double B_data[NSTATES * NINPUTS * (NHORIZON - 1)] = {0}; 47 | double f_data[NSTATES * (NHORIZON - 1)] = {0}; 48 | double* Xptr = X_data; 49 | double* Xref_ptr = Xref_data; 50 | double* Uptr = U_data; 51 | double* Uref_ptr = Uref_data; 52 | double* Kptr = K_data; 53 | double* dptr = d_data; 54 | double* Pptr = P_data; 55 | double* pptr = p_data; 56 | double* Aptr = A_data; 57 | double* Bptr = B_data; 58 | double* fptr = f_data; 59 | 60 | tiny_LtvModel model; 61 | tiny_ProblemData prob; 62 | tiny_Solver solver; 63 | 64 | private: 65 | void SetUp() override { 66 | tiny_InitLtvModel(&model); 67 | tiny_InitProblemData(&prob); 68 | tiny_InitSolver(&solver); 69 | 70 | for (int i = 0; i < NHORIZON; ++i) { 71 | if (i < NHORIZON - 1) { 72 | A[i] = slap_MatrixFromArray(NSTATES, NSTATES, Aptr); 73 | Aptr += NSTATES * NSTATES; 74 | B[i] = slap_MatrixFromArray(NSTATES, NINPUTS, Bptr); 75 | Bptr += NSTATES * NINPUTS; 76 | f[i] = slap_MatrixFromArray(NSTATES, 1, fptr); 77 | fptr += NSTATES; 78 | U[i] = slap_MatrixFromArray(NINPUTS, 1, Uptr); 79 | // slap_SetConst(U[i], 0.01); 80 | Uptr += NINPUTS; 81 | Unom[i] = slap_MatrixFromArray(NINPUTS, 1, Uref_ptr); 82 | Uref_ptr += NINPUTS; 83 | Uref[i] = slap_MatrixFromArray(NINPUTS, 1, ug_data); 84 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kptr); 85 | Kptr += NINPUTS * NSTATES; 86 | d[i] = slap_MatrixFromArray(NINPUTS, 1, dptr); 87 | dptr += NINPUTS; 88 | } 89 | X[i] = slap_MatrixFromArray(NSTATES, 1, Xptr); 90 | Xptr += NSTATES; 91 | Xnom[i] = slap_MatrixFromArray(NSTATES, 1, Xref_ptr); 92 | Xref_ptr += NSTATES; 93 | Xref[i] = slap_MatrixFromArray(NSTATES, 1, xg_data); 94 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pptr); 95 | Pptr += NSTATES * NSTATES; 96 | p[i] = slap_MatrixFromArray(NSTATES, 1, pptr); 97 | pptr += NSTATES; 98 | } 99 | 100 | model.ninputs = NSTATES; 101 | model.nstates = NINPUTS; 102 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 103 | model.get_jacobians = tiny_Bicycle5dGetJacobians; // from Bicycle 104 | model.A = A; 105 | model.B = B; 106 | model.f = f; 107 | slap_Copy(X[0], model.x0); 108 | 109 | prob.ninputs = NINPUTS; 110 | prob.nstates = NSTATES; 111 | prob.nhorizon = NHORIZON; 112 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 113 | slap_SetIdentity(prob.Q, 10e-1); 114 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 115 | slap_SetIdentity(prob.R, 1e-1); 116 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 117 | slap_SetIdentity(prob.Qf, 10e-1); 118 | prob.X_ref = Xref; 119 | prob.U_ref = Uref; 120 | prob.x0 = model.x0; 121 | prob.K = K; 122 | prob.d = d; 123 | prob.P = P; 124 | prob.p = p; 125 | } 126 | } 127 | 128 | 129 | TEST_F(LqrLtvTrackTest, DeltaLqrLtv) { 130 | double Q_temp_data[(NSTATES + NINPUTS) * (NSTATES + NINPUTS + 1)] = {0}; 131 | Matrix Q_temp = slap_MatrixFromArray(NSTATES + NINPUTS, NSTATES + NINPUTS + 1, 132 | Q_temp_data); 133 | 134 | // DELTA FORMULATION: Xref in objective and Xnom linearization 135 | // Compute and store A, B offline 136 | for (int i = 0; i < NHORIZON - 1; ++i) { 137 | model.get_jacobians(&(model.A[i]), &(model.B[i]), Xnom[i], Unom[i]); 138 | tiny_Bicycle5dNonlinearDynamics(&(model.f[i]), Xnom[i], Unom[i]); 139 | slap_MatrixAddition(model.f[i], model.f[i], Xnom[i + 1], -1); // = 0 140 | // printf("%f\n",slap_NormTwoSquared(model.f[i])); 141 | } 142 | 143 | tiny_BackwardPassLtv(&prob, solver, model, &Q_temp); 144 | tiny_ForwardPassLtv(X, U, prob, model); // delta_x and delta_u 145 | 146 | for (int k = 0; k < NHORIZON - 1; ++k) { 147 | // printf("ex[%d] = %.4f\n", k, slap_MatrixNormedDifference(X[k], Xref[k])); 148 | // tiny_NonlinearDynamics(&X[k+1], X[k], Uref[k]); 149 | // tiny_Print(Uref[k]); 150 | // tiny_Print(model.B[k]); 151 | } 152 | 153 | for (int k = NHORIZON - 5; k < NHORIZON; ++k) { 154 | EXPECT_LT(SumOfSquaredError(X[k].data, Xref[k].data, NSTATES), 0.5); 155 | } 156 | } 157 | 158 | TEST_F(LqrLtvTrackTest, AbsLqrLtv) { 159 | double Q_temp_data[(NSTATES + NINPUTS) * (NSTATES + NINPUTS + 1)] = {0}; 160 | Matrix Q_temp = slap_MatrixFromArray(NSTATES + NINPUTS, NSTATES + NINPUTS + 1, 161 | Q_temp_data); 162 | 163 | // Absolute formulation 164 | // Compute and store A, B offline 165 | for (int i = 0; i < NHORIZON - 1; ++i) { 166 | model.get_jacobians(&(model.A[i]), &(model.B[i]), prob.X_ref[i], 167 | prob.U_ref[i]); 168 | tiny_Bicycle5dNonlinearDynamics(&(model.f[i]), Xref[i], Uref[i]); 169 | slap_MatMulAdd(model.f[i], model.A[i], Xref[i], -1, 1); 170 | slap_MatMulAdd(model.f[i], model.B[i], Uref[i], -1, 1); 171 | // printf("%f\n",slap_NormTwoSquared(model.f[i])); 172 | } 173 | 174 | tiny_BackwardPassLtv(&prob, solver, model, &Q_temp); 175 | tiny_ForwardPassLtv(X, U, prob, model); 176 | 177 | for (int k = 0; k < NHORIZON - 1; ++k) { 178 | // printf("ex[%d] = %.4f\n", k, slap_MatrixNormedDifference(X[k], Xref[k])); 179 | // tiny_NonlinearDynamics(&X[k+1], X[k], Uref[k]); 180 | // tiny_Print(slap_Transpose(X[k])); 181 | // tiny_Print(model.B[k]); 182 | } 183 | 184 | for (int k = NHORIZON - 5; k < NHORIZON; ++k) { 185 | EXPECT_LT(SumOfSquaredError(X[k].data, Xref[k].data, NSTATES), 0.5); 186 | } 187 | } -------------------------------------------------------------------------------- /test/lqrdata_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "test_utils.h" 10 | 11 | #define NSTATES 2 12 | #define NINPUTS 1 13 | #define NHORIZON 3 14 | 15 | class LqrDataTest : public testing::Test { 16 | public: 17 | // int nx = 2; 18 | // int nu = 1; 19 | // int nh = 2; 20 | double dt = 0.1; 21 | double A_data[NSTATES * NSTATES] = {1, 0, 1, 1}; // NOLINT 22 | double B_data[NSTATES * NINPUTS] = {1, 2}; // NOLINT 23 | double f_data[NSTATES] = {4, 5}; // NOLINT 24 | double t = 1.0; 25 | double Q_data[NSTATES * NSTATES] = {1, 0, 0, 1}; // NOLINT 26 | double R_data[NINPUTS * NINPUTS] = {1}; // NOLINT 27 | double q_data[NSTATES] = {0.1, 0.2}; // NOLINT 28 | double r_data[NINPUTS] = {-0.6}; // NOLINT 29 | double Qf_data[NSTATES * NSTATES] = {0}; 30 | double u_max_data[NINPUTS] = {1.1}; 31 | double u_min_data[NINPUTS] = {-1.1}; 32 | double x_max_data[NSTATES] = {1.6, 1.7}; 33 | double x_min_data[NSTATES] = {-1.6, -1.7}; 34 | double x_ref_data[NSTATES * NHORIZON] = {0.2, 1.1, 2.5, 3.7, 2.1, 4.5}; 35 | double u_ref_data[NINPUTS * (NHORIZON - 1)] = {1, 2}; 36 | double x0_data[NSTATES] = {0.1, 0.2}; 37 | double u0_data[NSTATES] = {1.6}; 38 | double Kd_data[NINPUTS * (NSTATES + 1) * (NHORIZON - 1)] = {0}; 39 | double Pp_data[NSTATES * (NSTATES + 1) * NHORIZON] = {0}; 40 | double reg = 1e-8; 41 | double input_duals_data[2 * NINPUTS * (NHORIZON - 1)] = {1, 2, 3, 4}; 42 | double state_duals_data[2 * NSTATES * (NHORIZON)] = {1, 2, 3, 4, 5, 6, 43 | 7, 8, 2, 4, 5, 6}; 44 | double goal_duals_data[NSTATES] = {1, 2}; 45 | double reg_min = 1; 46 | double reg_max = 100; 47 | double penalty_max = 1e5; 48 | double penalty_mul = 1; 49 | int max_primal_iters = 100; 50 | int max_search_iters = 10; 51 | 52 | const double tol = 1e-8; 53 | 54 | private: 55 | void SetUp() override { 56 | 57 | } 58 | }; 59 | 60 | TEST_F(LqrDataTest, InitLtiModelTest) { 61 | tiny_LtiModel model; 62 | tiny_InitLtiModel(&model); 63 | model.nstates = NSTATES; 64 | model.ninputs = NINPUTS; 65 | model.dt = dt; 66 | model.A = slap_MatrixFromArray(NSTATES, NSTATES, A_data); 67 | model.B = slap_MatrixFromArray(NSTATES, NINPUTS, B_data); 68 | model.f = slap_MatrixFromArray(NSTATES, 1, f_data); 69 | model.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 70 | 71 | EXPECT_EQ(model.dt, dt); 72 | EXPECT_EQ(model.A.rows, model.nstates); 73 | EXPECT_EQ(model.A.cols, model.nstates); 74 | EXPECT_NEAR(SumOfSquaredError(A_data, model.A.data, model.nstates * model.nstates), 0, 75 | tol); 76 | EXPECT_EQ(model.B.rows, model.nstates); 77 | EXPECT_EQ(model.B.cols, model.ninputs); 78 | EXPECT_NEAR(SumOfSquaredError(B_data, model.B.data, model.nstates * model.ninputs), 0, 79 | tol); 80 | EXPECT_EQ(model.f.rows, model.nstates); 81 | EXPECT_EQ(model.f.cols, 1); 82 | EXPECT_NEAR(SumOfSquaredError(f_data, model.f.data, model.nstates), 0, tol); 83 | EXPECT_EQ(model.x0.rows, model.nstates); 84 | EXPECT_EQ(model.x0.cols, 1); 85 | EXPECT_NEAR(SumOfSquaredError(x0_data, model.x0.data, model.nstates), 0, tol); 86 | } 87 | 88 | TEST_F(LqrDataTest, KnotPoint) { 89 | tiny_KnotPoint z; 90 | tiny_InitKnotPoint(&z); 91 | tiny_KnotPoint Z[NHORIZON]; 92 | z.dt = dt; 93 | z.t = t; 94 | z.x = slap_MatrixFromArray(NSTATES, 1, x0_data); 95 | z.u = slap_MatrixFromArray(NINPUTS, 1, u0_data); 96 | 97 | EXPECT_EQ(z.dt, dt); 98 | EXPECT_EQ(z.t, t); 99 | EXPECT_EQ(z.x.rows, NSTATES); 100 | EXPECT_EQ(z.x.cols, 1); 101 | EXPECT_NEAR(SumOfSquaredError(x0_data, z.x.data, z.x.rows), 0, tol); 102 | EXPECT_EQ(z.u.rows, NINPUTS); 103 | EXPECT_EQ(z.u.cols, 1); 104 | EXPECT_NEAR(SumOfSquaredError(u0_data, z.u.data, z.u.rows), 0, tol); 105 | 106 | for (int i = 0; i < NHORIZON; ++i) { 107 | tiny_InitKnotPoint(&Z[i]); 108 | Z[i].dt = dt; 109 | Z[i].t = t + i; 110 | Z[i].x = slap_MatrixFromArray(NSTATES, 1, x0_data); 111 | Z[i].u = slap_MatrixFromArray(NINPUTS, 1, u0_data); 112 | } 113 | 114 | for (int i = 0; i < NHORIZON; ++i) { 115 | EXPECT_EQ(Z[i].dt, dt); 116 | EXPECT_EQ(Z[i].t, (t + i)); 117 | EXPECT_EQ(Z[i].x.rows, NSTATES); 118 | EXPECT_EQ(Z[i].x.cols, 1); 119 | EXPECT_NEAR(SumOfSquaredError(Z[i].x.data, z.x.data, z.x.rows), 0, tol); 120 | EXPECT_EQ(Z[i].u.rows, NINPUTS); 121 | EXPECT_EQ(Z[i].u.cols, 1); 122 | EXPECT_NEAR(SumOfSquaredError(Z[i].u.data, z.u.data, z.u.rows), 0, tol); 123 | } 124 | } 125 | 126 | TEST_F(LqrDataTest, Solver) { 127 | tiny_Solver solver; 128 | tiny_InitSolver(&solver); 129 | 130 | solver.reg = reg; 131 | solver.reg_min = reg_min; 132 | solver.reg_max = reg_max; 133 | solver.penalty_max = penalty_max; 134 | solver.penalty_mul = penalty_mul; 135 | solver.max_primal_iters = max_primal_iters; 136 | solver.max_search_iters = max_search_iters; 137 | 138 | EXPECT_EQ(solver.reg, reg); 139 | EXPECT_EQ(solver.reg_max, reg_max); 140 | EXPECT_EQ(solver.reg_min, reg_min); 141 | EXPECT_EQ(solver.penalty_max, penalty_max); 142 | EXPECT_EQ(solver.penalty_mul, penalty_mul); 143 | EXPECT_EQ(solver.max_primal_iters, max_primal_iters); 144 | EXPECT_EQ(solver.max_search_iters, max_search_iters); 145 | } 146 | 147 | TEST_F(LqrDataTest, ProblemData) { 148 | const double tol = 1e-8; 149 | Matrix U_ref[NHORIZON - 1]; 150 | Matrix X_ref[NHORIZON]; 151 | Matrix K[NHORIZON - 1]; 152 | Matrix d[NHORIZON - 1]; 153 | Matrix P[NHORIZON]; 154 | Matrix p[NHORIZON]; 155 | Matrix input_duals[NHORIZON - 1]; 156 | Matrix state_duals[NHORIZON]; 157 | double* uptr = u_ref_data; 158 | double* xptr = x_ref_data; 159 | double* Kd_ptr = Kd_data; 160 | double* Pp_ptr = Pp_data; 161 | double* input_dual_ptr = input_duals_data; 162 | double* state_dual_ptr = state_duals_data; 163 | for (int i = 0; i < NHORIZON; ++i) { 164 | if (i < NHORIZON - 1) { 165 | U_ref[i] = slap_MatrixFromArray(NINPUTS, 1, uptr); 166 | uptr += NINPUTS; 167 | K[i] = slap_MatrixFromArray(NINPUTS, NSTATES, Kd_ptr); 168 | Kd_ptr += NINPUTS * NSTATES; 169 | d[i] = slap_MatrixFromArray(NINPUTS, 1, Kd_ptr); 170 | Kd_ptr += NINPUTS; 171 | input_duals[i] = slap_MatrixFromArray(NINPUTS, 1, input_dual_ptr); 172 | input_dual_ptr += NINPUTS; 173 | } 174 | X_ref[i] = slap_MatrixFromArray(NSTATES, 1, xptr); 175 | xptr += NSTATES; 176 | P[i] = slap_MatrixFromArray(NSTATES, NSTATES, Pp_ptr); 177 | Pp_ptr += NSTATES * NSTATES; 178 | p[i] = slap_MatrixFromArray(NSTATES, 1, Pp_ptr); 179 | Pp_ptr += NSTATES; 180 | state_duals[i] = slap_MatrixFromArray(NSTATES, 1, state_dual_ptr); 181 | state_dual_ptr += NSTATES; 182 | } 183 | 184 | tiny_ProblemData prob; 185 | tiny_InitProblemData(&prob); 186 | 187 | prob.nstates = NSTATES; 188 | prob.ninputs = NINPUTS; 189 | prob.nhorizon = NHORIZON; 190 | prob.ncstr_goal = NSTATES; 191 | prob.ncstr_inputs = 2 * NINPUTS * (NHORIZON - 1); 192 | prob.ncstr_states = 2 * NSTATES * NHORIZON; 193 | prob.Q = slap_MatrixFromArray(NSTATES, NSTATES, Q_data); 194 | prob.R = slap_MatrixFromArray(NINPUTS, NINPUTS, R_data); 195 | prob.q = slap_MatrixFromArray(NSTATES, 1, q_data); 196 | prob.r = slap_MatrixFromArray(NINPUTS, 1, r_data); 197 | prob.Qf = slap_MatrixFromArray(NSTATES, NSTATES, Qf_data); 198 | prob.u_max = slap_MatrixFromArray(NINPUTS, 1, u_max_data); 199 | prob.u_min = slap_MatrixFromArray(NINPUTS, 1, u_min_data); 200 | prob.x_max = slap_MatrixFromArray(NSTATES, 1, x_max_data); 201 | prob.x_min = slap_MatrixFromArray(NSTATES, 1, x_min_data); 202 | prob.X_ref = X_ref; 203 | prob.U_ref = U_ref; 204 | prob.dt = dt; 205 | prob.x0 = slap_MatrixFromArray(NSTATES, 1, x0_data); 206 | prob.K = K; 207 | prob.d = d; 208 | prob.P = P; 209 | prob.p = p; 210 | prob.input_duals = input_duals; 211 | prob.state_duals = state_duals; 212 | prob.goal_dual = slap_MatrixFromArray(NSTATES, 1, goal_duals_data); 213 | 214 | EXPECT_EQ(prob.nstates, NSTATES); 215 | EXPECT_EQ(prob.ninputs, NINPUTS); 216 | EXPECT_EQ(prob.nhorizon, NHORIZON); 217 | EXPECT_EQ(prob.ncstr_inputs, 2 * NINPUTS * (NHORIZON - 1)); 218 | EXPECT_EQ(prob.ncstr_states, 2 * NSTATES * NHORIZON); 219 | EXPECT_EQ(prob.dt, dt); 220 | EXPECT_NEAR(SumOfSquaredError(prob.Q.data, Q_data, NSTATES * NSTATES), 0, tol); 221 | EXPECT_NEAR(SumOfSquaredError(prob.R.data, R_data, NINPUTS * NINPUTS), 0, tol); 222 | EXPECT_NEAR(SumOfSquaredError(prob.q.data, q_data, NSTATES), 0, tol); 223 | EXPECT_NEAR(SumOfSquaredError(prob.r.data, r_data, NINPUTS), 0, tol); 224 | EXPECT_NEAR(SumOfSquaredError(prob.Qf.data, Qf_data, NSTATES * NSTATES), 0, tol); 225 | EXPECT_NEAR(SumOfSquaredError(prob.u_max.data, u_max_data, NINPUTS), 0, tol); 226 | EXPECT_NEAR(SumOfSquaredError(prob.u_min.data, u_min_data, NINPUTS), 0, tol); 227 | EXPECT_NEAR(SumOfSquaredError(prob.x_max.data, x_max_data, NSTATES), 0, tol); 228 | EXPECT_NEAR(SumOfSquaredError(prob.x_min.data, x_min_data, NSTATES), 0, tol); 229 | EXPECT_NEAR(SumOfSquaredError(prob.x0.data, x0_data, NSTATES), 0, tol); 230 | EXPECT_NEAR(SumOfSquaredError(prob.goal_dual.data, goal_duals_data, NSTATES), 0, tol); 231 | uptr = u_ref_data; 232 | xptr = x_ref_data; 233 | Kd_ptr = Kd_data; 234 | Pp_ptr = Pp_data; 235 | input_dual_ptr = input_duals_data; 236 | state_dual_ptr = state_duals_data; 237 | for (int i = 0; i < NHORIZON; ++i) { 238 | if (i < NHORIZON - 1) { 239 | EXPECT_EQ(prob.U_ref[i].rows, NINPUTS); 240 | EXPECT_EQ(prob.U_ref[i].cols, 1); 241 | EXPECT_NEAR(SumOfSquaredError(prob.U_ref[i].data, uptr, NINPUTS), 0, tol); 242 | uptr += NINPUTS; 243 | EXPECT_EQ(prob.K[i].rows, NINPUTS); 244 | EXPECT_EQ(prob.K[i].cols, NSTATES); 245 | EXPECT_NEAR(SumOfSquaredError(prob.K[i].data, Kd_ptr, NINPUTS * NSTATES), 0, tol); 246 | Kd_ptr += NINPUTS * NSTATES; 247 | EXPECT_EQ(prob.d[i].rows, NINPUTS); 248 | EXPECT_EQ(prob.d[i].cols, 1); 249 | EXPECT_NEAR(SumOfSquaredError(prob.d[i].data, Kd_ptr, NINPUTS), 0, tol); 250 | Kd_ptr += NINPUTS; 251 | EXPECT_EQ(prob.input_duals[i].rows, NINPUTS); 252 | EXPECT_EQ(prob.input_duals[i].cols, 1); 253 | EXPECT_NEAR(SumOfSquaredError(prob.input_duals[i].data, input_dual_ptr, 254 | NINPUTS), 0, tol); 255 | input_dual_ptr += NINPUTS; 256 | } 257 | EXPECT_EQ(prob.X_ref[i].rows, NSTATES); 258 | EXPECT_EQ(prob.X_ref[i].cols, 1); 259 | EXPECT_NEAR(SumOfSquaredError(prob.X_ref[i].data, xptr, NSTATES), 0, tol); 260 | xptr += NSTATES; 261 | EXPECT_EQ(prob.P[i].rows, NSTATES); 262 | EXPECT_EQ(prob.P[i].cols, NSTATES); 263 | EXPECT_NEAR(SumOfSquaredError(prob.P[i].data, Pp_ptr, NSTATES * NSTATES), 0, tol); 264 | Pp_ptr += NSTATES * NSTATES; 265 | EXPECT_EQ(prob.p[i].rows, NSTATES); 266 | EXPECT_EQ(prob.p[i].cols, 1); 267 | EXPECT_NEAR(SumOfSquaredError(prob.p[i].data, Pp_ptr, NSTATES), 0, tol); 268 | Pp_ptr += NSTATES; 269 | EXPECT_EQ(prob.state_duals[i].rows, NSTATES); 270 | EXPECT_EQ(prob.state_duals[i].cols, 1); 271 | EXPECT_NEAR(SumOfSquaredError(prob.state_duals[i].data, state_dual_ptr, NSTATES), 0, 272 | tol); 273 | state_dual_ptr += NSTATES; 274 | } 275 | } -------------------------------------------------------------------------------- /test/models/bicycle_5d.c: -------------------------------------------------------------------------------- 1 | #include "bicycle_5d.h" 2 | 3 | #include 4 | 5 | //======================================== 6 | // Bicycle model parameters 7 | //======================================== 8 | // struct tiny_Model_Bicycle { 9 | // double drive_min[2]; 10 | // double drive_max[2]; 11 | // double u_min[2]; 12 | // double u_max[2]; 13 | // } tiny_DefaultModel_Bicycle = {{-2, -0.5}, {2, 0.5}, {-4, -0.7}, {4, 0.7}}; 14 | 15 | //======================================== 16 | // Codes generated from julia/bicycle_tvlqr 17 | // Discrete dynamics of bicycle model with predefined model params 18 | //======================================== 19 | void tiny_Bicycle5dNonlinearDynamics_Raw(double* xn, const double* x, 20 | const double* u) { 21 | xn[0] = 22 | 0.16666666666666666 * 23 | (0.1 * (0.1 * u[0] + x[3]) * 24 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 25 | x[2]) + 26 | 0.1 * cos(x[2]) * x[3] + 27 | 0.2 * (0.05 * u[0] + x[3]) * 28 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 29 | x[2]) + 30 | 0.2 * (0.05 * u[0] + x[3]) * cos(0.05 * tan(x[4]) * x[3] + x[2])) + 31 | x[0]; 32 | xn[1] = 33 | 0.16666666666666666 * 34 | (0.2 * (0.05 * u[0] + x[3]) * 35 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 36 | x[2]) + 37 | 0.2 * (0.05 * u[0] + x[3]) * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 38 | 0.1 * (0.1 * u[0] + x[3]) * 39 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 40 | x[2]) + 41 | 0.1 * sin(x[2]) * x[3]) + 42 | x[1]; 43 | xn[2] = 0.16666666666666666 * 44 | (0.1 * (0.1 * u[0] + x[3]) * tan(0.1 * u[1] + x[4]) + 45 | 0.4 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + 46 | 0.1 * tan(x[4]) * x[3]) + 47 | x[2]; 48 | xn[3] = 0.1 * u[0] + x[3]; 49 | xn[4] = 0.1 * u[1] + x[4]; 50 | } 51 | 52 | void tiny_Bicycle5dNonlinearDynamics(Matrix* xn, const Matrix x, 53 | const Matrix u) { 54 | tiny_Bicycle5dNonlinearDynamics_Raw(xn->data, x.data, u.data); 55 | } 56 | 57 | //======================================== 58 | // Codes generated from julia/bicycle_tvlqr 59 | // Jacobians of discrete dynamics of bicycle model with predefined model params 60 | //======================================== 61 | void tiny_Bicycle5dGetJacobianA_Raw(double* A, const double* x, 62 | const double* u) { 63 | A[0] = 1; 64 | A[1] = 0; 65 | A[2] = 0; 66 | A[3] = 0; 67 | A[4] = 0; 68 | A[5] = 0; 69 | A[6] = 1; 70 | A[7] = 0; 71 | A[8] = 0; 72 | A[9] = 0; 73 | A[10] = 74 | 0.16666666666666666 * 75 | (-0.2 * (0.05 * u[0] + x[3]) * 76 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 77 | -0.2 * (0.05 * u[0] + x[3]) * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 78 | -0.1 * (0.1 * u[0] + x[3]) * 79 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 80 | -0.1 * sin(x[2]) * x[3]); 81 | A[11] = 82 | 0.16666666666666666 * 83 | (0.1 * (0.1 * u[0] + x[3]) * 84 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 85 | 0.1 * cos(x[2]) * x[3] + 86 | 0.2 * (0.05 * u[0] + x[3]) * 87 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 88 | 0.2 * (0.05 * u[0] + x[3]) * cos(0.05 * tan(x[4]) * x[3] + x[2])); 89 | A[12] = 1; 90 | A[13] = 0; 91 | A[14] = 0; 92 | A[15] = 93 | 0.16666666666666666 * 94 | (0.2 * cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 95 | 0.2 * cos(0.05 * tan(x[4]) * x[3] + x[2]) + 96 | 0.1 * cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 97 | 0.1 * cos(x[2]) + 98 | -0.01 * (0.05 * u[0] + x[3]) * sin(0.05 * tan(x[4]) * x[3] + x[2]) * 99 | tan(x[4]) + 100 | -0.01 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 101 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 102 | -0.01 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 103 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 104 | A[16] = 105 | 0.16666666666666666 * 106 | (0.2 * sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 107 | 0.2 * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 108 | 0.1 * sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 109 | 0.1 * sin(x[2]) + 110 | 0.01 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 111 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 112 | 0.01 * (0.05 * u[0] + x[3]) * 113 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) * 114 | tan(0.05 * u[1] + x[4]) + 115 | 0.01 * (0.05 * u[0] + x[3]) * cos(0.05 * tan(x[4]) * x[3] + x[2]) * 116 | tan(x[4])); 117 | A[17] = 118 | 0.16666666666666666 * (0.4 * tan(0.05 * u[1] + x[4]) + 119 | 0.1 * tan(0.1 * u[1] + x[4]) + 0.1 * tan(x[4])); 120 | A[18] = 1; 121 | A[19] = 0; 122 | A[20] = 123 | 0.16666666666666666 * 124 | (-0.01 * pow(0.05 * u[0] + x[3], 2) * 125 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 126 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 127 | -0.01 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 128 | (0.1 * u[0] + x[3]) * 129 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 130 | -0.01 * (1 + pow(tan(x[4]), 2)) * (0.05 * u[0] + x[3]) * 131 | sin(0.05 * tan(x[4]) * x[3] + x[2]) * x[3]); 132 | A[21] = 133 | 0.16666666666666666 * 134 | (0.01 * pow(0.05 * u[0] + x[3], 2) * 135 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 136 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 137 | 0.01 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 138 | (0.1 * u[0] + x[3]) * 139 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 140 | 0.01 * (1 + pow(tan(x[4]), 2)) * (0.05 * u[0] + x[3]) * 141 | cos(0.05 * tan(x[4]) * x[3] + x[2]) * x[3]); 142 | A[22] = 0.16666666666666666 * 143 | (0.1 * (1 + pow(tan(0.1 * u[1] + x[4]), 2)) * (0.1 * u[0] + x[3]) + 144 | 0.4 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) + 145 | 0.1 * (1 + pow(tan(x[4]), 2)) * x[3]); 146 | A[23] = 0; 147 | A[24] = 1; 148 | } 149 | 150 | void tiny_Bicycle5dGetJacobianB_Raw(double* B, const double* x, 151 | const double* u) { 152 | B[0] = 153 | 0.16666666666666666 * 154 | (0.01 * 155 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 156 | 0.01 * cos(0.05 * tan(x[4]) * x[3] + x[2]) + 157 | 0.01 * cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 158 | -0.0005 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 159 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 160 | -0.0005 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 161 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 162 | B[1] = 163 | 0.16666666666666666 * 164 | (0.01 * 165 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 166 | 0.01 * sin(0.05 * tan(x[4]) * x[3] + x[2]) + 167 | 0.01 * sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 168 | 0.0005 * (0.1 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) * 169 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 170 | 0.0005 * (0.05 * u[0] + x[3]) * 171 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) * 172 | tan(0.05 * u[1] + x[4])); 173 | B[2] = 0.16666666666666666 * 174 | (0.02 * tan(0.05 * u[1] + x[4]) + 0.01 * tan(0.1 * u[1] + x[4])); 175 | B[3] = 0.1; 176 | B[4] = 0; 177 | B[5] = 178 | 0.16666666666666666 * 179 | (-0.0005 * pow(0.05 * u[0] + x[3], 2) * 180 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 181 | sin(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 182 | -0.0005 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 183 | (0.1 * u[0] + x[3]) * 184 | sin(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 185 | B[6] = 186 | 0.16666666666666666 * 187 | (0.0005 * pow(0.05 * u[0] + x[3], 2) * 188 | (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * 189 | cos(0.05 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2]) + 190 | 0.0005 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3]) * 191 | (0.1 * u[0] + x[3]) * 192 | cos(0.1 * (0.05 * u[0] + x[3]) * tan(0.05 * u[1] + x[4]) + x[2])); 193 | B[7] = 0.16666666666666666 * 194 | (0.01 * (1 + pow(tan(0.1 * u[1] + x[4]), 2)) * (0.1 * u[0] + x[3]) + 195 | 0.02 * (1 + pow(tan(0.05 * u[1] + x[4]), 2)) * (0.05 * u[0] + x[3])); 196 | B[8] = 0; 197 | B[9] = 0.1; 198 | } 199 | 200 | void tiny_Bicycle5dGetJacobians(Matrix* A, Matrix* B, const Matrix x, 201 | const Matrix u) { 202 | tiny_Bicycle5dGetJacobianA_Raw(A->data, x.data, u.data); 203 | tiny_Bicycle5dGetJacobianB_Raw(B->data, x.data, u.data); 204 | } -------------------------------------------------------------------------------- /test/models/bicycle_5d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "slap/slap.h" 4 | 5 | #ifdef __cplusplus 6 | extern "C" { 7 | #endif 8 | 9 | //======================================== 10 | // Bicycle model parameters 11 | // X = [x; y; theta; v; delta] : x, y, yaw, linear vel, steering angle 12 | // U = [a; delta_dot] : linear accel and steering rate 13 | //======================================== 14 | // struct tiny_Model_Bicycle { 15 | // double drive_min[2]; 16 | // double drive_max[2]; 17 | // double u_min[2]; 18 | // double u_max[2]; 19 | // } tiny_DefaultModel_Bicycle = {{-2, -0.5}, {2, 0.5}, {-4, -0.7}, {4, 0.7}}; 20 | 21 | //======================================== 22 | // Codes generated from julia/bicycle_tvlqr 23 | // Discrete dynamics of bicycle model with predefined model params 24 | //======================================== 25 | void tiny_Bicycle5dNonlinearDynamics_Raw(double* xn, const double* x, 26 | const double* u); 27 | 28 | void tiny_Bicycle5dNonlinearDynamics(Matrix* xn, const Matrix x, 29 | const Matrix u); 30 | 31 | //======================================== 32 | // Codes generated from julia/bicycle_tvlqr 33 | // Jacobians of discrete dynamics of bicycle model with predefined model params 34 | //======================================== 35 | void tiny_Bicycle5dGetJacobianA_Raw(double* A, const double* x, 36 | const double* u); 37 | 38 | void tiny_Bicycle5dGetJacobianB_Raw(double* B, const double* x, 39 | const double* u); 40 | 41 | void tiny_Bicycle5dGetJacobians(Matrix* A, Matrix* B, const Matrix x, 42 | const Matrix u); 43 | 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif -------------------------------------------------------------------------------- /test/models/planar_quadrotor.c: -------------------------------------------------------------------------------- 1 | #include "planar_quadrotor.h" 2 | 3 | //======================================== 4 | // Planar quadrotor model parameters 5 | //======================================== 6 | // struct tiny_Model_PlanarQuadrotor { 7 | // double g; 8 | // double m; 9 | // double l; 10 | // double J; 11 | // double umin[2]; 12 | // double umax[2]; 13 | // } tiny_DefaultModel_PlanarQuadrotor = {9.81, 1, 0.018, 14 | // 0.3, {0, 0}, {19.62, 19.62}}; 15 | 16 | //======================================== 17 | // Codes generated from julia/planar_quad_gen 18 | // Discrete dynamics of planar quadrotor 19 | //======================================== 20 | void tiny_PQuadNonlinearDynamics_Raw(double* xn, const double* x, 21 | const double* u) { 22 | xn[0] = 0.16666666666666666 * 23 | (0.2 * (0.05 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + x[3]) + 24 | 0.1 * (0.1 * (u[0] + u[1]) * 25 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + 26 | x[5]) + 27 | x[2]) + 28 | x[3]) + 29 | 0.2 * (0.05 * (u[0] + u[1]) * sin(x[2]) + x[3]) + 0.1 * x[3]) + 30 | x[0]; 31 | xn[1] = 0.16666666666666666 * 32 | (0.2 * (0.05 * (-9.81 + (u[0] + u[1]) * cos(0.05 * x[5] + x[2])) + 33 | x[4]) + 34 | 0.2 * (0.05 * (-9.81 + (u[0] + u[1]) * cos(x[2])) + x[4]) + 35 | 0.1 * (0.1 * (-9.81 + (u[0] + u[1]) * 36 | cos(0.05 * (0.41666666666666674 * 37 | (-1 * u[0] + u[1]) + 38 | x[5]) + 39 | x[2])) + 40 | x[4]) + 41 | 0.1 * x[4]) + 42 | x[1]; 43 | xn[2] = 0.16666666666666666 * 44 | (0.4 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 45 | 0.1 * (0.8333333333333335 * (-1 * u[0] + u[1]) + x[5]) + 46 | 0.1 * x[5]) + 47 | x[2]; 48 | xn[3] = 49 | 0.16666666666666666 * 50 | (0.1 * (u[0] + u[1]) * 51 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 52 | x[2]) + 53 | 0.2 * (u[0] + u[1]) * 54 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 55 | x[2]) + 56 | 0.2 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + 57 | 0.1 * (u[0] + u[1]) * sin(x[2])) + 58 | x[3]; 59 | xn[4] = 60 | 0.16666666666666666 * 61 | (0.1 * (-9.81 + (u[0] + u[1]) * cos(0.1 * (0.41666666666666674 * 62 | (-1 * u[0] + u[1]) + 63 | x[5]) + 64 | x[2])) + 65 | 0.2 * (-9.81 + (u[0] + u[1]) * cos(0.05 * (0.41666666666666674 * 66 | (-1 * u[0] + u[1]) + 67 | x[5]) + 68 | x[2])) + 69 | 0.2 * (-9.81 + (u[0] + u[1]) * cos(0.05 * x[5] + x[2])) + 70 | 0.1 * (-9.81 + (u[0] + u[1]) * cos(x[2]))) + 71 | x[4]; 72 | xn[5] = 0.8333333333333336 * (-1 * u[0] + u[1]) + x[5]; 73 | } 74 | 75 | void tiny_PQuadNonlinearDynamics(Matrix* xn, const Matrix x, const Matrix u) { 76 | tiny_PQuadNonlinearDynamics_Raw(xn->data, x.data, u.data); 77 | } 78 | 79 | //======================================== 80 | // Codes generated from julia/planar_quad_gen 81 | // Jacobians of discrete dynamics of planar quadrotor 82 | //======================================== 83 | void tiny_PQuadGetJacobianA_Raw(double* A, const double* x, const double* u) { 84 | A[0] = 1; 85 | A[1] = 0; 86 | A[2] = 0; 87 | A[3] = 0; 88 | A[4] = 0; 89 | A[5] = 0; 90 | A[6] = 0; 91 | A[7] = 1; 92 | A[8] = 0; 93 | A[9] = 0; 94 | A[10] = 0; 95 | A[11] = 0; 96 | A[12] = 0.16666666666666666 * 97 | (0.01 * (u[0] + u[1]) * 98 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 99 | x[2]) + 100 | 0.01 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2]) + 101 | 0.01 * (u[0] + u[1]) * cos(x[2])); 102 | A[13] = 0.16666666666666666 * 103 | (-0.01 * (u[0] + u[1]) * 104 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 105 | x[2]) + 106 | -0.01 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + 107 | -0.01 * (u[0] + u[1]) * sin(x[2])); 108 | A[14] = 1; 109 | A[15] = 110 | 0.16666666666666666 * 111 | (0.1 * (u[0] + u[1]) * 112 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 113 | 0.1 * (u[0] + u[1]) * cos(x[2]) + 114 | 0.2 * (u[0] + u[1]) * 115 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 116 | x[2]) + 117 | 0.2 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2])); 118 | A[16] = 119 | 0.16666666666666666 * 120 | (-0.1 * (u[0] + u[1]) * 121 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 122 | -0.2 * (u[0] + u[1]) * 123 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 124 | x[2]) + 125 | -0.2 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2]) + 126 | -0.1 * (u[0] + u[1]) * sin(x[2])); 127 | A[17] = 0; 128 | A[18] = 0.1; 129 | A[19] = 0; 130 | A[20] = 0; 131 | A[21] = 1; 132 | A[22] = 0; 133 | A[23] = 0; 134 | A[24] = 0; 135 | A[25] = 0.1; 136 | A[26] = 0; 137 | A[27] = 0; 138 | A[28] = 1; 139 | A[29] = 0; 140 | A[30] = 0.16666666666666666 * 141 | (0.0005 * (u[0] + u[1]) * 142 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 143 | x[2]) + 144 | 0.0005 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2])); 145 | A[31] = 0.16666666666666666 * 146 | (-0.0005 * (u[0] + u[1]) * 147 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 148 | x[2]) + 149 | -0.0005 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2])); 150 | A[32] = 0.1; 151 | A[33] = 152 | 0.16666666666666666 * 153 | (0.01 * (u[0] + u[1]) * 154 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 155 | x[2]) + 156 | 0.01 * (u[0] + u[1]) * cos(0.05 * x[5] + x[2]) + 157 | 0.01 * (u[0] + u[1]) * 158 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2])); 159 | A[34] = 160 | 0.16666666666666666 * 161 | (-0.01 * (u[0] + u[1]) * 162 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 163 | x[2]) + 164 | -0.01 * (u[0] + u[1]) * 165 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 166 | -0.01 * (u[0] + u[1]) * sin(0.05 * x[5] + x[2])); 167 | A[35] = 1; 168 | } 169 | 170 | void tiny_PQuadGetJacobianB_Raw(double* B, const double* x, const double* u) { 171 | B[0] = 172 | 0.16666666666666666 * 173 | (0.01 * sin(0.05 * x[5] + x[2]) + 0.01 * sin(x[2]) + 174 | 0.1 * 175 | (0.1 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 176 | x[2]) + 177 | -0.002083333333333334 * (u[0] + u[1]) * 178 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 179 | x[2]))); 180 | B[1] = 181 | 0.16666666666666666 * 182 | (0.01 * 183 | (0.02083333333333334 * (u[0] + u[1]) * 184 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 185 | x[2]) + 186 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 187 | x[2])) + 188 | 0.01 * cos(0.05 * x[5] + x[2]) + 0.01 * cos(x[2])); 189 | B[2] = -0.04166666666666667; 190 | B[3] = 191 | 0.16666666666666666 * 192 | (0.2 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 193 | x[2]) + 194 | 0.1 * 195 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 196 | 0.1 * sin(x[2]) + 0.2 * sin(0.05 * x[5] + x[2]) + 197 | -0.004166666666666668 * (u[0] + u[1]) * 198 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 199 | x[2]) + 200 | -0.004166666666666668 * (u[0] + u[1]) * 201 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2])); 202 | B[4] = 203 | 0.16666666666666666 * 204 | (0.2 * (0.02083333333333334 * (u[0] + u[1]) * 205 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 206 | x[2]) + 207 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 208 | x[2])) + 209 | 0.1 * (0.04166666666666668 * (u[0] + u[1]) * 210 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 211 | x[2]) + 212 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 213 | x[2])) + 214 | 0.2 * cos(0.05 * x[5] + x[2]) + 0.1 * cos(x[2])); 215 | B[5] = -0.8333333333333336; 216 | B[6] = 217 | 0.16666666666666666 * 218 | (0.01 * sin(0.05 * x[5] + x[2]) + 0.01 * sin(x[2]) + 219 | 0.1 * 220 | (0.1 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 221 | x[2]) + 222 | 0.002083333333333334 * (u[0] + u[1]) * 223 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 224 | x[2]))); 225 | B[7] = 226 | 0.16666666666666666 * 227 | (0.01 * 228 | (-0.02083333333333334 * (u[0] + u[1]) * 229 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 230 | x[2]) + 231 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 232 | x[2])) + 233 | 0.01 * cos(0.05 * x[5] + x[2]) + 0.01 * cos(x[2])); 234 | B[8] = 0.04166666666666667; 235 | B[9] = 236 | 0.16666666666666666 * 237 | (0.2 * sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 238 | x[2]) + 239 | 0.1 * 240 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2]) + 241 | 0.1 * sin(x[2]) + 0.2 * sin(0.05 * x[5] + x[2]) + 242 | 0.004166666666666668 * (u[0] + u[1]) * 243 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 244 | x[2]) + 245 | 0.004166666666666668 * (u[0] + u[1]) * 246 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + x[2])); 247 | B[10] = 248 | 0.16666666666666666 * 249 | (0.1 * (-0.04166666666666668 * (u[0] + u[1]) * 250 | sin(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 251 | x[2]) + 252 | cos(0.1 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 253 | x[2])) + 254 | 0.1 * cos(x[2]) + 255 | 0.2 * (-0.02083333333333334 * (u[0] + u[1]) * 256 | sin(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 257 | x[2]) + 258 | cos(0.05 * (0.41666666666666674 * (-1 * u[0] + u[1]) + x[5]) + 259 | x[2])) + 260 | 0.2 * cos(0.05 * x[5] + x[2])); 261 | B[11] = 0.8333333333333336; 262 | } 263 | 264 | void tiny_PQuadGetJacobians(Matrix* A, Matrix* B, const Matrix x, 265 | const Matrix u) { 266 | tiny_PQuadGetJacobianA_Raw(A->data, x.data, u.data); 267 | tiny_PQuadGetJacobianB_Raw(B->data, x.data, u.data); 268 | } -------------------------------------------------------------------------------- /test/models/planar_quadrotor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "slap/slap.h" 6 | 7 | //======================================== 8 | // Planar quadrotor model parameters 9 | //======================================== 10 | // struct tiny_Model_PlanarQuadrotor { 11 | // double g; 12 | // double m; 13 | // double l; 14 | // double J; 15 | // double umin[2]; 16 | // double umax[2]; 17 | // } tiny_DefaultModel_PlanarQuadrotor = {9.81, 1, 0.018, 18 | // 0.3, {0, 0}, {19.62, 19.62}}; 19 | 20 | //======================================== 21 | // Codes generated from julia/planar_quad_gen 22 | // Discrete dynamics of planar quadrotor 23 | //======================================== 24 | void tiny_PQuadNonlinearDynamics_Raw(double* xn, const double* x, 25 | const double* u); 26 | 27 | void tiny_PQuadNonlinearDynamics(Matrix* xn, const Matrix x, const Matrix u); 28 | 29 | //======================================== 30 | // Codes generated from julia/planar_quad_gen 31 | // Jacobians of discrete dynamics of planar quadrotor 32 | //======================================== 33 | void tiny_PQuadGetJacobianA_Raw(double* A, const double* x, const double* u); 34 | 35 | void tiny_PQuadGetJacobianB_Raw(double* B, const double* x, const double* u); 36 | 37 | void tiny_PQuadGetJacobians(Matrix* A, Matrix* B, const Matrix x, 38 | const Matrix u); -------------------------------------------------------------------------------- /test/test_all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | DIR="$( dirname -- "${BASH_SOURCE[0]}"; )"; 3 | echo "Run all tests at $DIR" 4 | build/$DIR/lqrdata_test 5 | build/$DIR/lqr_ineq_utils_test 6 | build/$DIR/forward_pass_test 7 | build/$DIR/cost_test 8 | build/$DIR/lqr_lti_test 9 | build/$DIR/lqr_lti_track_test 10 | build/$DIR/al_lqr_lti_test 11 | build/$DIR/lqr_ltv_test 12 | build/$DIR/lqr_ltv_track_test 13 | build/$DIR/al_lqr_ltv_test 14 | -------------------------------------------------------------------------------- /test/test_utils.c: -------------------------------------------------------------------------------- 1 | #include "test_utils.h" 2 | 3 | #include 4 | 5 | // #include "riccati/riccati_solver.h" 6 | #include "slap/matrix.h" 7 | 8 | double SumOfSquaredError(const double* x, const double* y, int len) { 9 | double err = 0; 10 | for (int i = 0; i < len; ++i) { 11 | double diff = x[i] - y[i]; 12 | err += diff * diff; 13 | } 14 | return sqrt(err); 15 | } 16 | 17 | // void DiscreteDoubleIntegratorDynamics(double h, double dim, Matrix* A, 18 | // Matrix* B) { 19 | // int nstates = 2 * dim; 20 | // slap_MatrixSetConst(A, 0.0); 21 | // slap_MatrixSetConst(B, 0.0); 22 | // for (int i = 0; i < nstates; ++i) { 23 | // slap_MatrixSetElement(A, i, i, 1.0); 24 | // } 25 | // double b = h * h / 2; 26 | // for (int i = 0; i < dim; ++i) { 27 | // slap_MatrixSetElement(A, i, i + dim, h); 28 | // slap_MatrixSetElement(B, i, i, b); 29 | // slap_MatrixSetElement(B, i + dim, i, h); 30 | // } 31 | // } 32 | 33 | // RiccatiSolver* DoubleIntegratorProblem() { 34 | // const int dim = 2; 35 | // const int nstates = 2 * dim; 36 | // const int ninputs = dim; 37 | // const int nhorizon = 11; 38 | // const double h = 0.1; 39 | 40 | // // Generate the problem 41 | // RiccatiSolver* solver = ulqr_NewRiccatiSolver(nstates, ninputs, nhorizon); 42 | 43 | // // Generate dynamics matrices and copy into problem 44 | // Matrix A = slap_NewMatrix(nstates, nstates); 45 | // Matrix B = slap_NewMatrix(nstates, ninputs); 46 | // DiscreteDoubleIntegratorDynamics(h, dim, &A, &B); 47 | // const double f[4] = { 48 | // 1, 49 | // -1, 50 | // 0, 51 | // 0, 52 | // }; 53 | // ulqr_SetDynamics(solver, A.data, B.data, f, 0, nhorizon - 1); 54 | 55 | // return solver; 56 | // } 57 | -------------------------------------------------------------------------------- /test/test_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #ifdef __cplusplus 4 | extern "C" { 5 | #endif 6 | 7 | 8 | // #include "riccati/riccati_solver.h" 9 | double SumOfSquaredError(const double* x, const double* y, int len); 10 | 11 | // void DiscreteDoubleIntegratorDynamics(double h, double dim, Matrix* A, 12 | // Matrix* B); 13 | 14 | // RiccatiSolver* DoubleIntegratorProblem(); 15 | 16 | #ifdef __cplusplus 17 | } 18 | #endif --------------------------------------------------------------------------------