Squashed 'third_party/ct/' content from commit 0048d02
Change-Id: Ia7e5360cbb414f92ce4f118bd9613ea23597db52
git-subtree-dir: third_party/ct
git-subtree-split: 0048d027531b6cf1ea730da17b68a0b7ef9070b1
diff --git a/ct_optcon/CMakeLists.txt b/ct_optcon/CMakeLists.txt
new file mode 100644
index 0000000..ed3a03b
--- /dev/null
+++ b/ct_optcon/CMakeLists.txt
@@ -0,0 +1,329 @@
+cmake_minimum_required (VERSION 2.6)
+
+include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/compilerSettings.cmake)
+include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/explicitTemplateHelpers.cmake)
+include(${CMAKE_CURRENT_SOURCE_DIR}/../ct/cmake/clang-cxx-dev-tools.cmake)
+
+project (ct_optcon)
+
+find_package(catkin REQUIRED
+ ct_core
+)
+
+set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -fopenmp -std=c++11 -Wall")
+SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pthread -std=c++11 -Wall")
+SET(CMAKE_EXPORT_COMPILE_COMMANDS TRUE)
+
+
+option(MATLAB "Compile with matlab support" OFF)
+option(MATLAB_FULL_LOG "Expose all variables to Matlab (very slow)" OFF)
+option(DEBUG_PRINT "Print debug messages" OFF)
+
+option(HPIPM "Compile with HPIPM support" OFF)
+
+
+if(DEBUG_PRINT)
+ message(STATUS "WARNING: Debug Print ON")
+ add_definitions ( -DDEBUG_PRINT )
+endif(DEBUG_PRINT)
+
+if(MATLAB_FULL_LOG)
+ message(WARNING "WARNING: Compiling with full log to matlab. Execution will be very slow.")
+ set(MATLAB ON)
+ add_definitions ( -DMATLAB_FULL_LOG )
+endif(MATLAB_FULL_LOG)
+
+if(MATLAB)
+ message(STATUS "MATLAB support ON")
+ find_package(matlab_cpp_interface REQUIRED)
+ if(matlab_cpp_interface_FOUND)
+ include_directories(${matlab_cpp_interface_INCLUDE_DIRS})
+ set(MATLAB_LIB_DIR ${matlab_cpp_interface_LIBRARIES})
+ add_definitions( -DMATLAB )
+ endif()
+endif(MATLAB)
+set(MATLAB_LIBS ${MATLAB_LIB_DIR})
+
+#todo export this properly from ct_core
+find_package(PythonLibs QUIET)
+if (PYTHONLIBS_FOUND)
+ message(STATUS "Python found")
+ include_directories(${PYTHON_INCLUDE_DIRS})
+ set(PLOTTING_ENABLED true)
+ add_definitions(-DPLOTTING_ENABLED)
+else()
+ message(STATUS "Python not found")
+ set(PYTHON_LIBRARY "")
+endif()
+
+## dummy HPIPM libs
+set(HPIPM_LIBS "")
+
+## include BLASFEO AND HPIPM
+if(HPIPM)
+ message(STATUS "Switching HPIPM ON")
+ if(DEFINED ENV{BLASFEO_DIR})
+ if(DEFINED ENV{HPIPM_DIR})
+ message(WARNING "...Found HPIPM and BLASFEO environment variables")
+
+ set(BLASFEO_INCLUDE_DIR $ENV{BLASFEO_DIR}/include)
+ set(HPIPM_INCLUDE_DIR $ENV{HPIPM_DIR}/include)
+ set(BLASFEO_DIR $ENV{BLASFEO_DIR})
+ set(HPIPM_DIR $ENV{HPIPM_DIR})
+
+ include_directories(${BLASFEO_INCLUDE_DIR})
+ include_directories(${HPIPM_INCLUDE_DIR})
+ link_directories(${BLASFEO_DIR}/lib)
+ link_directories(${HPIPM_DIR}/lib)
+
+ find_library(BLASFEO_LIBRARY blasfeo ${BLASFEO_DIR}/lib REQUIRED)
+ find_library(HPIPM_LIBRARY hpipm ${HPIPM_DIR}/lib REQUIRED)
+
+ set(HPIPM_LIBS hpipm blasfeo)
+
+ add_definitions (-DHPIPM)
+
+ else(DEFINED ENV{HPIPM_DIR})
+ message(WARNING "WARNING: Trying to build with HPIPM, but no ENV variable for HPIPM found.")
+ endif(DEFINED ENV{HPIPM_DIR})
+ else(DEFINED ENV{BLASFEO_DIR})
+ message(WARNING "WARNING: Trying to build with HPIPM, but no ENV variable for Blasfeo found.")
+ endif(DEFINED ENV{BLASFEO_DIR})
+endif(HPIPM)
+
+
+## lapack libs
+set(LAPACK_LIBS "")
+find_package(LAPACK QUIET)
+if(LAPACK_FOUND)
+ message(STATUS "Found LAPACK library")
+ set(LAPACK_LIBS lapack)
+ add_definitions (-DCT_USE_LAPACK)
+ set(USE_LAPACK ON)
+else(LAPACK_FOUND)
+ message(WARNING "Could not find LAPACK library")
+endif(LAPACK_FOUND)
+
+
+## dummy nlp libs
+set(NLP_LIBS "")
+
+## include IPOPT
+find_package(ipopt QUIET)
+if(DEFINED ENV{IPOPT_SOURCE_DIR} OR ipopt_FOUND)
+ set(BUILD_WITH_IPOPT_SUPPORT ON)
+ message(STATUS "Found IPOPT - building with IPOPT support")
+ if(ipopt_FOUND)
+ message(STATUS "Using LOCAL installation of IPOPT")
+ set(IPOPT_BUILD_DIR ${CMAKE_BINARY_DIR}/build/include/coin)
+ elseif(DEFINED ENV{IPOPT_SOURCE_DIR})
+ message(STATUS "using GLOBAL installation of IPOPT")
+ set(IPOPT_BUILD_DIR $ENV{IPOPT_SOURCE_DIR}/build)
+ else()
+ message(FATAL_ERROR "ERROR: Ipopt source directory environment variable not set! Set IPOPT_SOURCE_DIR environment variable!")
+ endif(ipopt_FOUND)
+
+ include_directories("${IPOPT_BUILD_DIR}/include/coin")
+ add_definitions( -DBUILD_WITH_IPOPT_SUPPORT )
+ link_directories(${IPOPT_BUILD_DIR}/lib)
+ set(IPOPT_LIBS ipopt dl coinmumps coinhsl lapack blas gfortran
+ m quadmath coinmetis)
+
+endif(DEFINED ENV{IPOPT_SOURCE_DIR} OR ipopt_FOUND)
+
+
+## include SNOPT
+if(DEFINED ENV{SNOPT_SOURCE_DIR})
+ set(BUILD_WITH_SNOPT_SUPPORT ON)
+ message(STATUS "Found SNOPT - building with SNOPT support")
+ include_directories( "$ENV{SNOPT_SOURCE_DIR}/include")
+
+ add_definitions( -DBUILD_WITH_SNOPT_SUPPORT )
+ find_library(SNOPT_LIBRARY1 snopt7_cpp $ENV{SNOPT_SOURCE_DIR}/lib REQUIRED)
+ find_library(SNOPT_LIBRARY2 snopt7 $ENV{SNOPT_SOURCE_DIR}/lib REQUIRED)
+ set(SNOPT_LIBS ${SNOPT_LIBRARY1} ${SNOPT_LIBRARY2})
+
+ set(CT_SNOPT_LIBS ct_snopt_interface)
+endif(DEFINED ENV{SNOPT_SOURCE_DIR})
+
+set(NLP_LIBS ${IPOPT_LIBS} ${CT_SNOPT_LIBS})
+
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## assemble list of libraries that contain prespecified templates
+ct_configure_explicit_templates("${CMAKE_CURRENT_SOURCE_DIR}/../ct/config/explicit_templates.cfg" "${CMAKE_CURRENT_SOURCE_DIR}/prespec/" "ct_optcon")
+message(WARNING "CT Optcon: Compiling the following explict template libraries: ${PRESPEC_LIB_NAMES}")
+
+#used later for clang-tidy
+set(INC_DIRS ${catkin_INCLUDE_DIRS} "${CMAKE_CURRENT_SOURCE_DIR}/include" ${HPIPM_INCLUDE_DIR} ${BLASFEO_INCLUDE_DIR})
+
+catkin_package(
+ INCLUDE_DIRS
+ include
+ ${HPIPM_INCLUDE_DIR}
+ ${BLASFEO_INCLUDE_DIR}
+ LIBRARIES
+ ${LAPACK_LIBS}
+ ${CT_SNOPT_LIBS}
+ ${HPIPM_LIBRARY}
+ ${BLASFEO_LIBRARY}
+ ${PRESPEC_LIB_NAMES}
+ CATKIN_DEPENDS
+ ct_core
+)
+
+## build SNOPT library
+if(BUILD_WITH_SNOPT_SUPPORT)
+ add_library(ct_snopt_interface src/nlp/solver/SnoptSolver.cpp)
+ target_link_libraries(ct_snopt_interface ${catkin_LIBRARIES} ${SNOPT_LIBS})
+endif()
+
+
+# add libraries for explicit template
+ct_add_explicit_template_libs()
+
+# link against addigional external libraries, such as lapack, if applicable
+if(USE_LAPACK)
+ ct_link_external_library(${LAPACK_LIBS})
+endif(USE_LAPACK)
+if(HPIPM)
+ ct_link_external_library(${BLASFEO_LIBRARY})
+ ct_link_external_library(${HPIPM_LIBRARY})
+endif(HPIPM)
+
+
+if(BUILD_EXAMPLES)
+ message(STATUS "Building with examples.")
+
+ set(CT_OPTCON_EXAMPLE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/examples")
+ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/examples/exampleDir.h.in ${CMAKE_CURRENT_SOURCE_DIR}/examples/exampleDir.h)
+
+ add_executable(ex_ConstraintOutput examples/ConstraintExampleOutput.cpp)
+ target_link_libraries(ex_ConstraintOutput ${catkin_LIBRARIES})
+
+ add_executable(ex_LQR examples/LQR.cpp)
+ target_link_libraries(ex_LQR ${catkin_LIBRARIES} ${LAPACK_LIBS})
+
+ add_executable(ex_NLOC examples/NLOC.cpp)
+ target_link_libraries(ex_NLOC ${catkin_LIBRARIES} ${HPIPM_LIBS})
+
+ add_executable(ex_NLOC_MPC examples/NLOC_MPC.cpp)
+ target_link_libraries(ex_NLOC_MPC ${catkin_LIBRARIES} ${HPIPM_LIBS})
+
+ add_executable(ex_DMS examples/DMS.cpp)
+ target_link_libraries(ex_DMS ${catkin_LIBRARIES} ${NLP_LIBS})
+endif()
+
+
+get_filename_component(DMS_OSC_TEST_MAT_DIR "test/dms/oscillator/matfiles" ABSOLUTE)
+configure_file(test/dms/oscillator/oscDMSTest_settings.h.in test/dms/oscillator/oscDMSTest_settings.h)
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/test/dms/oscillator)
+
+catkin_add_gtest(LqrTest test/lqr/LqrTest.cpp)
+target_link_libraries(LqrTest ${catkin_LIBRARIES} ${LAPACK_LIBS} ${MATLAB_LIBS})
+catkin_add_gtest(LqrTestPrespec test/lqr/LqrTestPrespec.cpp)
+target_link_libraries(LqrTestPrespec ${catkin_LIBRARIES} ${LAPACK_LIBS} ${MATLAB_LIBS} ${PRESPEC_LIB_NAMES})
+
+catkin_add_gtest(iLQRTest test/nloc/iLQRTest.cpp)
+target_link_libraries(iLQRTest ${catkin_LIBRARIES} ${MATLAB_LIBS} ${HPIPM_LIBS})
+
+if(HPIPM)
+ catkin_add_gtest(LQOCSolverTest test/solver/linear/LQOCSolverTest.cpp)
+ target_link_libraries(LQOCSolverTest ${catkin_LIBRARIES} ${MATLAB_LIBS} ${HPIPM_LIBS})
+ catkin_add_gtest(LQOCSolverTestPrespec test/solver/linear/LQOCSolverTestPrespec.cpp)
+ target_link_libraries(LQOCSolverTestPrespec ${catkin_LIBRARIES} ${MATLAB_LIBS} ${HPIPM_LIBS} ${PRESPEC_LIB_NAMES})
+
+ catkin_add_gtest(ConstrainedLQOCSolverTest test/solver/linear/ConstrainedLQOCSolverTest.cpp)
+ target_link_libraries(ConstrainedLQOCSolverTest ${catkin_LIBRARIES} ${MATLAB_LIBS} ${HPIPM_LIBS})
+
+ add_executable(LQOCSolverTiming test/solver/linear/LQOCSolverTiming.cpp)
+ target_link_libraries(LQOCSolverTiming ${catkin_LIBRARIES} ${MATLAB_LIBS} ${HPIPM_LIBS})
+ if(BUILD_EXAMPLES)
+ add_executable(ex_NLOC_boxConstrained examples/NLOC_boxConstrained.cpp)
+ target_link_libraries(ex_NLOC_boxConstrained ${catkin_LIBRARIES} ${HPIPM_LIBS})
+
+ add_executable(ex_NLOC_generalConstrained examples/NLOC_generalConstrained.cpp)
+ target_link_libraries(ex_NLOC_generalConstrained ${catkin_LIBRARIES} ${HPIPM_LIBS})
+ endif()
+endif(HPIPM)
+
+
+get_filename_component(NLOC_TEST_DIR "test/nloc" ABSOLUTE)
+configure_file(test/nloc/nloc_test_dir.h.in test/nloc/nloc_test_dir.h)
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/test/nloc)
+
+catkin_add_gtest(LinearSystemTest test/nloc/LinearSystemTest.cpp)
+target_link_libraries(LinearSystemTest ${catkin_LIBRARIES} ${HPIPM_LIBS} ${MATLAB_LIBS})
+catkin_add_gtest(LinearSystemTestPrespec test/nloc/LinearSystemTestPrespec.cpp)
+target_link_libraries(LinearSystemTestPrespec ${catkin_LIBRARIES} ${HPIPM_LIBS} ${MATLAB_LIBS} ${PRESPEC_LIB_NAMES})
+
+catkin_add_gtest(NonlinearSystemTest test/nloc/NonlinearSystemTest.cpp)
+target_link_libraries(NonlinearSystemTest ${catkin_LIBRARIES} ${HPIPM_LIBS} ${MATLAB_LIBS})
+#todo make prespec test
+
+add_executable(SymplecticTest test/nloc/SymplecticTest.cpp)
+target_link_libraries(SymplecticTest ${catkin_LIBRARIES} ${HPIPM_LIBS} ${MATLAB_LIBS})
+add_executable(SymplecticTestPrespec test/nloc/SymplecticTestPrespec.cpp)
+target_link_libraries(SymplecticTestPrespec ${catkin_LIBRARIES} ${HPIPM_LIBS} ${MATLAB_LIBS} ${PRESPEC_LIB_NAMES})
+
+catkin_add_gtest(constraint_comparison test/constraint/ConstraintComparison.cpp)
+target_link_libraries(constraint_comparison ${catkin_LIBRARIES})
+# note that this test does not have a prespec equivalent
+
+catkin_add_gtest(constraint_test test/constraint/ConstraintTest.cpp)
+target_link_libraries(constraint_test ${catkin_LIBRARIES} )
+catkin_add_gtest(constraint_test_prespec test/constraint/ConstraintTestPrespec.cpp)
+target_link_libraries(constraint_test_prespec ${catkin_LIBRARIES} ${PRESPEC_LIB_NAMES})
+
+catkin_add_gtest(SparseBoxConstraintTest test/constraint/SparseBoxConstraintTest.cpp)
+target_link_libraries(SparseBoxConstraintTest ${catkin_LIBRARIES})
+
+get_filename_component(COSTFUNCTION_TEST_DIR "test/costfunction" ABSOLUTE)
+configure_file(test/costfunction/costfunction_test_dir.h.in test/costfunction/costfunction_test_dir.h)
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/test/costfunction)
+catkin_add_gtest(CostFunctionTests test/costfunction/CostFunctionTests.cpp)
+target_link_libraries(CostFunctionTests ${catkin_LIBRARIES} ${MATLAB_LIBS})
+catkin_add_gtest(LoadFromFileTest test/costfunction/LoadFromFileTest.cpp)
+target_link_libraries(LoadFromFileTest ${catkin_LIBRARIES} ${MATLAB_LIBS})
+
+catkin_add_gtest(dms_test test/dms/oscillator/oscDMSTest.cpp)
+target_link_libraries(dms_test ${catkin_LIBRARIES} ${MATLAB_LIBS} ${NLP_LIBS})
+
+catkin_add_gtest(dms_test_all_var test/dms/oscillator/oscDMSTestAllVariants.cpp)
+target_link_libraries(dms_test_all_var ${catkin_LIBRARIES} ${MATLAB_LIBS} ${NLP_LIBS})
+
+add_executable(matFilesGenerator test/dms/oscillator/matfiles/matFilesGenerator.cpp)
+target_link_libraries(matFilesGenerator ${catkin_LIBRARIES} ${MATLAB_LIBS} ${NLP_LIBS})
+
+get_filename_component(MPC_TEST_MAT_DIR "test/mpc/matfiles" ABSOLUTE)
+configure_file(test/mpc/mpcTestSettings.h.in test/mpc/mpcTestSettings.h)
+include_directories(${CMAKE_CURRENT_BINARY_DIR}/test/mpc)
+
+catkin_add_gtest(NLOC_MPCTest test/mpc/NLOC_MPCTest.cpp)
+target_link_libraries(NLOC_MPCTest ${catkin_LIBRARIES} ${MATLAB_LIBS} ${HPIPM_LIBS})
+catkin_add_gtest(NLOC_MPCTest_prespec test/mpc/NLOC_MPCTest_prespec.cpp)
+target_link_libraries(NLOC_MPCTest_prespec ${catkin_LIBRARIES} ${MATLAB_LIBS} ${HPIPM_LIBS} ${PRESPEC_LIB_NAMES})
+
+
+find_package(Doxygen)
+if(DOXYGEN_FOUND)
+
+ set(doxyfile_in ${CMAKE_CURRENT_SOURCE_DIR}/doc/ct_optcon.doxyfile)
+ set(doxyfile ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile)
+
+ configure_file(${doxyfile_in} ${doxyfile} @ONLY)
+
+ add_custom_target(doc
+ COMMAND ${CMAKE_COMMAND} -E echo_append "Building API Documentation..."
+ COMMAND ${DOXYGEN_EXECUTABLE} ${doxyfile}
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/doc
+ COMMAND ${CMAKE_COMMAND} -E echo_append "API Documentation built in ${CMAKE_CURRENT_SOURCE_DIR}/doc"
+ VERBATIM)
+endif()
+
+
+ct_configure_clang_tidy(INC_DIRS)
diff --git a/ct_optcon/Doxyfile b/ct_optcon/Doxyfile
new file mode 100644
index 0000000..710e2c5
--- /dev/null
+++ b/ct_optcon/Doxyfile
@@ -0,0 +1,2315 @@
+# Doxyfile 1.8.7
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = ct_optcon
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER = v2.1
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF = "Control Toolbox - Optimal Control Module"
+
+# With the PROJECT_LOGO tag one can specify an logo or icon that is included in
+# the documentation. The maximum height of the logo should not exceed 55 pixels
+# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo
+# to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY = .
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a
+# new page for each member. If set to NO, the documentation of a member will be
+# part of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by by putting a % sign in front of the word
+# or globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES, then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = YES
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = YES
+
+# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = NO
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = YES
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# This flag is only useful for Objective-C code. When set to YES local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = YES
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = YES
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO these classes will be included in the various overviews. This option has
+# no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = YES
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = NO
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the
+# todo list. This list is created by putting \todo commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the
+# test list. This list is created by putting \test commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES the list
+# will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE = DoxygenLayout.xml
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. Do not use file names with spaces, bibtex cannot handle them. See
+# also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO doxygen will only warn about wrong or incomplete parameter
+# documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces.
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = ../ ./
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank the
+# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii,
+# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp,
+# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown,
+# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf,
+# *.qsf, *.as and *.js.
+
+FILE_PATTERNS =
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS = */external/cppad/* \
+ */src/codegen/*
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH = ../test \
+ ../examples
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS = *.cpp \
+ *.h
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = YES
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER ) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = YES
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = YES
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES, then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional user-
+# defined cascading style sheet that is included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefor more robust against future updates.
+# Doxygen will copy the style sheet file to the output directory. For an example
+# see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the stylesheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 30
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler ( hhc.exe). If non-empty
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated (
+# YES) or that it should be included in the master .chm file ( NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated (
+# YES) or a normal table of contents ( NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using prerendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. To get the times font for
+# instance you can specify
+# EXTRA_PACKAGES=times
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber. Doxygen will
+# replace them by respectively the title of the page, the current date and time,
+# only the current date, the version number of doxygen, the project name (see
+# PROJECT_NAME), or the project number (see PROJECT_NUMBER).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the LATEX_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES doxygen will generate an AutoGen
+# Definitions (see http://autogen.sf.net) file that captures the structure of
+# the code including all documentation. Note that this feature is still
+# experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = NO
+
+# If the MACRO_EXPANSION tag is set to YES doxygen will expand all macro names
+# in the source code. If set to NO only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES the includes files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES = ../../ct_doc/doc/tags/ct_core.tag=../../../ct_core/doc/html
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE = ../../ct_doc/doc/tags/ct_optcon.tag
+
+# If the ALLEXTERNALS tag is set to YES all external class will be listed in the
+# class index. If set to NO only the inherited external classes will be listed.
+# The default value is: NO.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed in
+# the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = YES
+
+# If the EXTERNAL_PAGES tag is set to YES all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH =
+
+# If set to YES, the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font n the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot.
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd and svg.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = YES
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
diff --git a/ct_optcon/cmake/FindEigen.cmake b/ct_optcon/cmake/FindEigen.cmake
new file mode 100644
index 0000000..e785d28
--- /dev/null
+++ b/ct_optcon/cmake/FindEigen.cmake
@@ -0,0 +1,83 @@
+# - Try to find Eigen3 lib
+#
+# This module supports requiring a minimum version, e.g. you can do
+# find_package(Eigen3 3.1.2)
+# to require version 3.1.2 or newer of Eigen3.
+#
+# Once done this will define
+#
+# EIGEN_FOUND - system has eigen lib with correct version
+# EIGEN_INCLUDE_DIR - the eigen include directory
+# EIGEN_VERSION - eigen version
+
+# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
+# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
+
+if(NOT Eigen_FIND_VERSION)
+ if(NOT Eigen_FIND_VERSION_MAJOR)
+ set(Eigen_FIND_VERSION_MAJOR 2)
+ endif(NOT Eigen_FIND_VERSION_MAJOR)
+ if(NOT Eigen_FIND_VERSION_MINOR)
+ set(Eigen_FIND_VERSION_MINOR 91)
+ endif(NOT Eigen_FIND_VERSION_MINOR)
+ if(NOT Eigen_FIND_VERSION_PATCH)
+ set(Eigen_FIND_VERSION_PATCH 0)
+ endif(NOT Eigen_FIND_VERSION_PATCH)
+
+ set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}")
+endif(NOT Eigen_FIND_VERSION)
+
+macro(_eigen3_check_version)
+ file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
+
+ string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
+ set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
+ set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}")
+ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
+ set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
+
+ set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
+ if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
+ set(EIGEN_VERSION_OK FALSE)
+ else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
+ set(EIGEN_VERSION_OK TRUE)
+ endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION})
+
+ if(NOT EIGEN_VERSION_OK)
+
+ message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, "
+ "but at least version ${Eigen_FIND_VERSION} is required")
+ endif(NOT EIGEN_VERSION_OK)
+endmacro(_eigen3_check_version)
+
+if (EIGEN_INCLUDE_DIRS)
+
+ # in cache already
+ _eigen3_check_version()
+ set(EIGEN_FOUND ${EIGEN_VERSION_OK})
+
+else ()
+
+ find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
+ PATHS
+ ${CMAKE_INSTALL_PREFIX}/include
+ ${KDE4_INCLUDE_DIR}
+ PATH_SUFFIXES eigen3 eigen
+ )
+
+ if(EIGEN_INCLUDE_DIR)
+ _eigen3_check_version()
+ endif(EIGEN_INCLUDE_DIR)
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK)
+
+ mark_as_advanced(EIGEN_INCLUDE_DIR)
+ SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.")
+
+endif()
+
+
diff --git a/ct_optcon/config/example.info b/ct_optcon/config/example.info
new file mode 100644
index 0000000..a502b12
--- /dev/null
+++ b/ct_optcon/config/example.info
@@ -0,0 +1,93 @@
+term0
+{
+ name "state cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 1.6
+ (1,0) 0.0
+ (2,0) 0.0
+ (1,0) 0.0
+ (1,1) -3.1
+ (1,2) 0.0
+ (2,0) 0.0
+ (2,1) 0.0
+ (2,2) 2.0
+ }
+ R
+ {
+ scaling 1.0
+ (0,0) 2.1
+ (1,0) 4.5
+ (2,0) 3.3 ;a2 as was given in main.cpp
+ }
+ }
+}
+
+term1
+{
+ name "state cost linear"
+ kind "Linear"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ a
+ {
+ scaling 1.0
+ (0,0) 1.6
+ (1,0) 0.0
+ (2,0) 0.0
+ (1,0) 0.0
+ (1,1) -3.1
+ (1,2) 0.0
+ (2,0) 0.0
+ (2,1) 0.0
+ (2,2) 2.0
+ }
+ b
+ {
+ scaling 1.0
+ (0,0) 2.1
+ (1,0) 4.5
+ (2,0) 3.3
+ }
+ }
+}
+
+x
+{
+ scaling 1.0
+ (0,0) 2.0
+ (1,0) -1.0
+ (2,0) 3.0 ;X as was given in main.cpp
+}
+
+u
+{
+ scaling 1.0
+ (0,0) -5.0
+ (1,0) 3.2
+ (2,0) 2.3 ;u as was given in main.cpp
+}
+
+x_ref
+{
+ scaling 1.0
+ (0,0) 2.0
+ (1,0) 1.0
+ (2,0) 3.0
+}
+
+u_ref
+{
+ scaling 1.0
+ (0,0) 1.1
+ (1,0) 2.2
+ (2,0) 3.3
+}
diff --git a/ct_optcon/doc/ct_optcon.doxyfile b/ct_optcon/doc/ct_optcon.doxyfile
new file mode 100644
index 0000000..15006e3
--- /dev/null
+++ b/ct_optcon/doc/ct_optcon.doxyfile
@@ -0,0 +1,2315 @@
+# Doxyfile 1.8.7
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = ct_optcon
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER = v2.1
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF = "Control Toolbox - Optimal Control Module"
+
+# With the PROJECT_LOGO tag one can specify an logo or icon that is included in
+# the documentation. The maximum height of the logo should not exceed 55 pixels
+# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo
+# to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY = .
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a
+# new page for each member. If set to NO, the documentation of a member will be
+# part of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by by putting a % sign in front of the word
+# or globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES, then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = YES
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = YES
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = NO
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# This flag is only useful for Objective-C code. When set to YES local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = YES
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO these classes will be included in the various overviews. This option has
+# no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = YES
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = NO
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the
+# todo list. This list is created by putting \todo commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the
+# test list. This list is created by putting \test commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES the list
+# will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE = DoxygenLayout.xml
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. Do not use file names with spaces, bibtex cannot handle them. See
+# also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO doxygen will only warn about wrong or incomplete parameter
+# documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces.
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = ../ ./
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank the
+# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii,
+# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp,
+# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown,
+# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf,
+# *.qsf, *.as and *.js.
+
+FILE_PATTERNS =
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS = */external/cppad/* \
+ */src/codegen/*
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH = ../test \
+ ../examples
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS = *.cpp \
+ *.h
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = YES
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER ) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = YES
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = YES
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES, then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional user-
+# defined cascading style sheet that is included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefor more robust against future updates.
+# Doxygen will copy the style sheet file to the output directory. For an example
+# see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the stylesheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 30
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler ( hhc.exe). If non-empty
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated (
+# YES) or that it should be included in the master .chm file ( NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated (
+# YES) or a normal table of contents ( NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = YES
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using prerendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. To get the times font for
+# instance you can specify
+# EXTRA_PACKAGES=times
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber. Doxygen will
+# replace them by respectively the title of the page, the current date and time,
+# only the current date, the version number of doxygen, the project name (see
+# PROJECT_NAME), or the project number (see PROJECT_NUMBER).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the LATEX_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES doxygen will generate an AutoGen
+# Definitions (see http://autogen.sf.net) file that captures the structure of
+# the code including all documentation. Note that this feature is still
+# experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES doxygen will expand all macro names
+# in the source code. If set to NO only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES the includes files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES = ../../ct_doc/doc/tags/ct_core.tag=../../../ct_core/doc/html
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE = ../../ct_doc/doc/tags/ct_optcon.tag
+
+# If the ALLEXTERNALS tag is set to YES all external class will be listed in the
+# class index. If set to NO only the inherited external classes will be listed.
+# The default value is: NO.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed in
+# the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = YES
+
+# If the EXTERNAL_PAGES tag is set to YES all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH =
+
+# If set to YES, the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font n the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot.
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd and svg.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = YES
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
diff --git a/ct_optcon/doc/mainpage.dox b/ct_optcon/doc/mainpage.dox
new file mode 100644
index 0000000..13f9373
--- /dev/null
+++ b/ct_optcon/doc/mainpage.dox
@@ -0,0 +1,15 @@
+/*!
+\mainpage CT Optimal Control
+
+\section ct_optcon_mod Control Toolbox - Optimal Control Module
+
+This module is part of the ADRL Control Toolbox ('CT'), an open-source C++ library for efficient modelling, control,
+estimation and trajectory optimization for robotics.
+
+The Optimal Control Module implements different Trajectory Optimizers, such as Sequential Linear Quadratic Optimal Control,
+Direct Multiple Shooting, but also pure Feedback controllers like LQR.
+
+\section ct_optcon_install Installation and Usage
+CT Optimal Control is part of the Control Toolbox suite. See the <a href="../../../ct_doc/doc/html/index.html">CT main documentation</a> for installation and usage instructions.
+
+*/
diff --git a/ct_optcon/examples/ConstraintExampleOutput.cpp b/ct_optcon/examples/ConstraintExampleOutput.cpp
new file mode 100644
index 0000000..6d355cb
--- /dev/null
+++ b/ct_optcon/examples/ConstraintExampleOutput.cpp
@@ -0,0 +1,154 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This Example displays some constraint toolbox outputs, giving the user a feeling for
+ * sparsity patterns, etc.
+ */
+
+#include <ct/optcon/optcon.h>
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+// some random state and control dimensions
+const size_t state_dim = 10;
+const size_t control_dim = 5;
+
+
+void controlInputBoxConstraintExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // desired boundaries
+ ControlVector<control_dim> u_lb = -ControlVector<control_dim>::Ones();
+ ControlVector<control_dim> u_ub = ControlVector<control_dim>::Ones();
+
+ // constraint term
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub));
+ controlConstraint->setName("ControlInputConstraint");
+
+ // add and initialize constraint term
+ constraints->addIntermediateConstraint(controlConstraint, true);
+ constraints->initialize();
+
+ std::cout << "=============================================" << std::endl;
+ std::cout << "Printing example for control input constraint:" << std::endl;
+ std::cout << "=============================================" << std::endl;
+ constraints->printout();
+}
+
+
+void terminalConstraintExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // desired terminal state
+ StateVector<state_dim> x_final = StateVector<state_dim>::Random();
+
+ // terminal constrain term
+ std::shared_ptr<TerminalConstraint<state_dim, control_dim>> terminalConstraint(
+ new TerminalConstraint<state_dim, control_dim>(x_final));
+ terminalConstraint->setName("TerminalConstraint");
+
+ // add and initialize terminal constraint term
+ constraints->addTerminalConstraint(terminalConstraint, true);
+ constraints->initialize();
+
+ std::cout << "==============================================" << std::endl;
+ std::cout << "Printing example for terminal state constraint:" << std::endl;
+ std::cout << "==============================================" << std::endl;
+ constraints->printout();
+}
+
+
+void boxConstraintsExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // desired terminal state
+ ControlVector<control_dim> u_lb = -1.11 * ControlVector<control_dim>::Ones();
+ ControlVector<control_dim> u_ub = 1.11 * ControlVector<control_dim>::Ones();
+ StateVector<state_dim> x_lb = -3.33 * StateVector<state_dim>::Ones();
+ StateVector<state_dim> x_ub = 3.33 * StateVector<state_dim>::Ones();
+
+ // constrain terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub));
+ controlConstraint->setName("ControlInputConstraint");
+ std::shared_ptr<StateConstraint<state_dim, control_dim>> stateConstraint(
+ new StateConstraint<state_dim, control_dim>(x_lb, x_ub));
+ stateConstraint->setName("StateConstraint");
+
+ // add and initialize constraint terms
+ constraints->addIntermediateConstraint(controlConstraint, true);
+ constraints->addIntermediateConstraint(stateConstraint, true);
+ constraints->initialize();
+
+ std::cout << "=============================================" << std::endl;
+ std::cout << "Printing example for combined box constraint:" << std::endl;
+ std::cout << "=============================================" << std::endl;
+ constraints->printout();
+}
+
+
+void sparseBoxConstraintsExample()
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // box constraint boundaries with sparsities
+ Eigen::VectorXi sp_control(control_dim);
+ sp_control << 0, 1, 0, 0, 1;
+ Eigen::VectorXd u_lb(2);
+ Eigen::VectorXd u_ub(2);
+ u_lb.setConstant(-1.11);
+ u_ub = -u_lb;
+
+ Eigen::VectorXi sp_state(state_dim);
+ sp_state << 0, 1, 0, 0, 1, 0, 1, 1, 0, 0;
+ Eigen::VectorXd x_lb(4);
+ Eigen::VectorXd x_ub(4);
+ x_lb.setConstant(-3.33);
+ x_ub = -x_lb;
+
+ // constrain terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub, sp_control));
+ controlConstraint->setName("ControlInputConstraint");
+
+ std::shared_ptr<StateConstraint<state_dim, control_dim>> stateConstraint(
+ new StateConstraint<state_dim, control_dim>(x_lb, x_ub, sp_state));
+ stateConstraint->setName("StateConstraint");
+
+ // add and initialize constraint terms
+ constraints->addIntermediateConstraint(controlConstraint, true);
+ constraints->addIntermediateConstraint(stateConstraint, true);
+ constraints->initialize();
+
+ std::cout << "=============================================" << std::endl;
+ std::cout << "Printing example for sparse box constraint:" << std::endl;
+ std::cout << "=============================================" << std::endl;
+ constraints->printout();
+}
+
+
+int main(int argc, char **argv)
+{
+ controlInputBoxConstraintExample();
+ terminalConstraintExample();
+ boxConstraintsExample();
+ sparseBoxConstraintsExample();
+ return 1;
+}
diff --git a/ct_optcon/examples/DMS.cpp b/ct_optcon/examples/DMS.cpp
new file mode 100644
index 0000000..37442cf
--- /dev/null
+++ b/ct_optcon/examples/DMS.cpp
@@ -0,0 +1,122 @@
+
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+/*!
+ * This example shows how to use classical Direct Multiple Shooting with an oscillator system,
+ * using IPOPT as NLP solver.
+ *
+ * \example DMS.cpp
+ */
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon;
+ using namespace ct::core;
+
+ const size_t state_dim = SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = SecondOrderSystem::CONTROL_DIM;
+
+
+ /**
+ * STEP 1 : set up the optimal control problem
+ * */
+
+ StateVector<state_dim> x_0;
+ StateVector<state_dim> x_final;
+
+ x_0 << 0.0, 0.0;
+ x_final << 2.0, -1.0;
+
+ double w_n = 0.5; // oscillator frequency
+ double zeta = 0.01; // oscillator damping
+
+ // create oscillator system
+ std::shared_ptr<SecondOrderSystem> oscillator(new SecondOrderSystem(w_n, zeta));
+
+
+ // load the cost weighting matrices from file and store them in terms. Note that we only use intermediate cost
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/dmsCost.info", "intermediateCost", true);
+
+ // create a cost function and add the terms to it.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+
+
+ // we include the desired terminal state as a hard constraint
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> finalConstraints(
+ new ct::optcon::ConstraintContainerAnalytical<2, 1>());
+
+ std::shared_ptr<TerminalConstraint<state_dim, control_dim>> terminalConstraint(
+ new TerminalConstraint<2, 1>(x_final));
+ terminalConstraint->setName("TerminalConstraint");
+ finalConstraints->addTerminalConstraint(terminalConstraint, true);
+ finalConstraints->initialize();
+
+
+ // define optcon problem and add constraint
+ OptConProblem<state_dim, control_dim> optConProblem(oscillator, costFunction);
+ optConProblem.setInitialState(x_0);
+ optConProblem.setGeneralConstraints(finalConstraints);
+
+
+ /**
+ * STEP 2 : determine solver settings
+ */
+ DmsSettings settings;
+ settings.N_ = 25; // number of nodes
+ settings.T_ = 5.0; // final time horizon
+ settings.nThreads_ = 4; // number of threads for multi-threading
+ settings.splineType_ = DmsSettings::PIECEWISE_LINEAR;
+ settings.costEvaluationType_ = DmsSettings::FULL; // we evaluate the full cost and use no trapezoidal approximation
+ settings.objectiveType_ = DmsSettings::KEEP_TIME_AND_GRID; // don't optimize the time spacing between the nodes
+ settings.h_min_ = 0.1; // minimum admissible distance between two nodes in [sec]
+ settings.integrationType_ = DmsSettings::RK4; // type of the shot integrator
+ settings.dt_sim_ = 0.01; // forward simulation dt
+ settings.solverSettings_.solverType_ = NlpSolverSettings::SolverType::IPOPT; // use IPOPT
+ settings.absErrTol_ = 1e-8;
+ settings.relErrTol_ = 1e-8;
+
+ settings.print();
+
+
+ /**
+ * STEP 3 : Calculate an appropriate initial guess
+ */
+ StateVectorArray<state_dim> x_initguess;
+ ControlVectorArray<control_dim> u_initguess;
+ DmsPolicy<state_dim, control_dim> initialPolicy;
+
+
+ x_initguess.resize(settings.N_ + 1, StateVector<state_dim>::Zero());
+ u_initguess.resize(settings.N_ + 1, ControlVector<control_dim>::Zero());
+ for (size_t i = 0; i < settings.N_ + 1; ++i)
+ {
+ x_initguess[i] = x_0 + (x_final - x_0) * (i / settings.N_);
+ }
+
+ initialPolicy.xSolution_ = x_initguess;
+ initialPolicy.uSolution_ = u_initguess;
+
+
+ /**
+ * STEP 4: solve DMS with IPOPT
+ */
+
+ optConProblem.setTimeHorizon(settings.T_);
+
+ std::shared_ptr<DmsSolver<state_dim, control_dim>> dmsSolver(
+ new DmsSolver<state_dim, control_dim>(optConProblem, settings));
+
+ dmsSolver->setInitialGuess(initialPolicy);
+ dmsSolver->solve();
+
+ // retrieve the solution
+ DmsPolicy<state_dim, control_dim> solution = dmsSolver->getSolution();
+
+ // let's plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.xSolution_, solution.uSolution_, solution.tSolution_);
+}
diff --git a/ct_optcon/examples/LQR.cpp b/ct_optcon/examples/LQR.cpp
new file mode 100644
index 0000000..dcd2ae0
--- /dev/null
+++ b/ct_optcon/examples/LQR.cpp
@@ -0,0 +1,59 @@
+/*!
+ * Simple example how to linearize a system and design an LQR controller.
+ *
+ * \example LQR.cpp
+ */
+
+
+#include <ct/optcon/optcon.h> // also includes ct_core
+#include "exampleDir.h"
+
+int main(int argc, char** argv)
+{
+ // get the state and control input dimension of the oscillator
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+ // create an auto-differentiable instance of the oscillator dynamics
+ ct::core::ADCGScalar w_n(50.0);
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, ct::core::ADCGScalar>> oscillatorDynamics(
+ new ct::core::tpl::SecondOrderSystem<ct::core::ADCGScalar>(w_n));
+
+ // create an Auto-Differentiation Linearizer with code generation on the quadrotor model
+ ct::core::ADCodegenLinearizer<state_dim, control_dim> adLinearizer(oscillatorDynamics);
+
+ // compile the linearized model just-in-time
+ adLinearizer.compileJIT();
+
+ // define the linearization point around steady state
+ ct::core::StateVector<state_dim> x;
+ x.setZero();
+ ct::core::ControlVector<control_dim> u;
+ u.setZero();
+ double t = 0.0;
+
+ // compute the linearization around the nominal state using the Auto-Diff Linearizer
+ auto A = adLinearizer.getDerivativeState(x, u, t);
+ auto B = adLinearizer.getDerivativeControl(x, u, t);
+
+ // load the weighting matrices
+ ct::optcon::TermQuadratic<state_dim, control_dim> quadraticCost;
+ quadraticCost.loadConfigFile(ct::optcon::exampleDir + "/lqrCost.info", "termLQR");
+ auto Q = quadraticCost.stateSecondDerivative(x, u, t); // x, u and t can be arbitrary here
+ auto R = quadraticCost.controlSecondDerivative(x, u, t); // x, u and t can be arbitrary here
+
+ // design the LQR controller
+ ct::optcon::LQR<state_dim, control_dim> lqrSolver;
+ ct::core::FeedbackMatrix<state_dim, control_dim> K;
+
+ std::cout << "A: " << std::endl << A << std::endl << std::endl;
+ std::cout << "B: " << std::endl << B << std::endl << std::endl;
+ std::cout << "Q: " << std::endl << Q << std::endl << std::endl;
+ std::cout << "R: " << std::endl << R << std::endl << std::endl;
+
+ lqrSolver.compute(Q, R, A, B, K);
+
+ std::cout << "LQR gain matrix:" << std::endl << K << std::endl;
+
+ return 1;
+}
diff --git a/ct_optcon/examples/NLOC.cpp b/ct_optcon/examples/NLOC.cpp
new file mode 100644
index 0000000..73b17f8
--- /dev/null
+++ b/ct_optcon/examples/NLOC.cpp
@@ -0,0 +1,125 @@
+/*!
+ * \example NLOC.cpp
+ *
+ * This example shows how to use the nonlinear optimal control solvers iLQR, unconstrained Gauss-Newton-Multiple-Shooting (GNMS),
+ * as well as the hybrid methods iLQR-GNMS(M), where M denotes the number of multiple-shooting intervals. This example applies
+ * them to a simple second-order oscillator.
+ *
+ */
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+int main(int argc, char **argv)
+{
+ /*get the state and control input dimension of the oscillator. Since we're dealing with a simple oscillator,
+ the state and control dimensions will be state_dim = 2, and control_dim = 1. */
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+
+ /* STEP 1: set up the Nonlinear Optimal Control Problem
+ * First of all, we need to create instances of the system dynamics, the linearized system and the cost function. */
+
+ /* STEP 1-A: create a instance of the oscillator dynamics for the optimal control problem.
+ * Please also compare the documentation of SecondOrderSystem.h */
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+
+ /* STEP 1-B: Although the first order derivatives of the oscillator are easy to derive, let's illustrate the use of the System Linearizer,
+ * which performs numerical differentiation by the finite-difference method. The system linearizer simply takes the
+ * the system dynamics as argument. Alternatively, you could implement your own first-order derivatives by overloading the class LinearSystem.h */
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+
+ /* STEP 1-C: create a cost function. We have pre-specified the cost-function weights for this problem in "nlocCost.info".
+ * Here, we show how to create terms for intermediate and final cost and how to automatically load them from the configuration file.
+ * The verbose option allows to print information about the loaded terms on the terminal. */
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "finalCost", verbose);
+
+ // Since we are using quadratic cost function terms in this example, the first and second order derivatives are immediately known and we
+ // define the cost function to be an "Analytical Cost Function". Let's create the corresponding object and add the previously loaded
+ // intermediate and final term.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+
+ /* STEP 1-D: initialization with initial state and desired time horizon */
+
+ StateVector<state_dim> x0;
+ x0.setRandom(); // in this example, we choose a random initial state x0
+
+ ct::core::Time timeHorizon = 3.0; // and a final time horizon in [sec]
+
+
+ // STEP 1-E: create and initialize an "optimal control problem"
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+
+ /* STEP 2: set up a nonlinear optimal control solver. */
+
+ /* STEP 2-A: Create the settings.
+ * the type of solver, and most parameters, like number of shooting intervals, etc.,
+ * can be chosen using the following settings struct. Let's use, the iterative
+ * linear quadratic regulator, iLQR, for this example. In the following, we
+ * modify only a few settings, for more detail, check out the NLOptConSettings class. */
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.nThreads = 4; // use multi-threading
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver =
+ NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER; // solve LQ-problems using custom Riccati solver
+ ilqr_settings.printSummary = true;
+ ilqr_settings.debugPrint = true;
+
+
+ /* STEP 2-B: provide an initial guess */
+ // calculate the number of time steps K
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ /* design trivial initial controller for iLQR. Note that in this simple example,
+ * we can simply use zero feedforward with zero feedback gains around the initial position.
+ * In more complex examples, a more elaborate initial guess may be required.*/
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // STEP 3: solve the optimal control problem
+ iLQR.solve();
+
+
+ // STEP 4: retrieve the solution
+ ct::core::StateFeedbackController<state_dim, control_dim> solution = iLQR.getSolution();
+
+ // let's plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.x_ref(), solution.uff(), solution.time());
+}
diff --git a/ct_optcon/examples/NLOC_MPC.cpp b/ct_optcon/examples/NLOC_MPC.cpp
new file mode 100644
index 0000000..a2fe427
--- /dev/null
+++ b/ct_optcon/examples/NLOC_MPC.cpp
@@ -0,0 +1,164 @@
+
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+/*!
+ * This tutorial example shows how to use the MPC class. In the CT, every optimal control solver can be wrapped into the MPC-class,
+ * allowing for very rapid prototyping of different MPC applications.
+ * In this example, we apply iLQR-MPC to a simple second order system, a damped oscillator.
+ * This tutorial builds up on the example NLOC.cpp, please consider this one as well.
+ *
+ * \example NLOC_MPC.cpp
+ */
+int main(int argc, char **argv)
+{
+ /* PRELIMINIARIES, see example NLOC.cpp */
+
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/mpcCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/mpcCost.info", "finalCost", verbose);
+
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+ StateVector<state_dim> x0;
+ x0.setRandom();
+
+ ct::core::Time timeHorizon = 3.0;
+
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::
+ GNRICCATI_SOLVER; // the LQ-problems are solved using a custom Gauss-Newton Riccati solver
+ ilqr_settings.printSummary = true;
+
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // we solve the optimal control problem and retrieve the solution
+ iLQR.solve();
+ ct::core::StateFeedbackController<state_dim, control_dim> initialSolution = iLQR.getSolution();
+
+
+ /* MPC-EXAMPLE
+ * we store the initial solution obtained from solving the initial optimal control problem,
+ * and re-use it to initialize the MPC solver in the following. */
+
+ /* STEP 1: first, we set up an MPC instance for the iLQR solver and configure it. Since the MPC
+ * class is wrapped around normal Optimal Control Solvers, we need to different kind of settings,
+ * those for the optimal control solver, and those specific to MPC: */
+
+ // 1) settings for the iLQR instance used in MPC. Of course, we use the same settings
+ // as for solving the initial problem ...
+ NLOptConSettings ilqr_settings_mpc = ilqr_settings;
+ // ... however, in MPC-mode, it makes sense to limit the overall number of iLQR iterations (real-time iteration scheme)
+ ilqr_settings_mpc.max_iterations = 1;
+ // and we limited the printouts, too.
+ ilqr_settings_mpc.printSummary = false;
+
+
+ // 2) settings specific to model predictive control. For a more detailed description of those, visit ct/optcon/mpc/MpcSettings.h
+ ct::optcon::mpc_settings mpc_settings;
+ mpc_settings.stateForwardIntegration_ = true;
+ mpc_settings.postTruncation_ = true;
+ mpc_settings.measureDelay_ = true;
+ mpc_settings.delayMeasurementMultiplier_ = 1.0;
+ mpc_settings.mpc_mode = ct::optcon::MPC_MODE::FIXED_FINAL_TIME;
+ mpc_settings.coldStart_ = false;
+
+
+ // STEP 2 : Create the iLQR-MPC object, based on the optimal control problem and the selected settings.
+ MPC<NLOptConSolver<state_dim, control_dim>> ilqr_mpc(optConProblem, ilqr_settings_mpc, mpc_settings);
+
+ // initialize it using the previously computed initial controller
+ ilqr_mpc.setInitialGuess(initialSolution);
+
+
+ /* STEP 3: running MPC
+ * Here, we run the MPC loop. Note that the general underlying idea is that you receive a state-estimate
+ * together with a time-stamp from your robot or system. MPC needs to receive both that time information and
+ * the state from your control system. Here, "simulate" the time measurement using std::chrono and wrap
+ * everything into a for-loop.
+ * The basic idea of operation is that after receiving time and state information, one executes the finishIteration() method of MPC.
+ */
+ auto start_time = std::chrono::high_resolution_clock::now();
+
+
+ // limit the maximum number of runs in this example
+ size_t maxNumRuns = 100;
+
+ std::cout << "Starting to run MPC" << std::endl;
+
+ for (size_t i = 0; i < maxNumRuns; i++)
+ {
+ // let's for simplicity, assume that the "measured" state is the first state from the optimal trajectory plus some noise
+ if (i > 0)
+ x0 = 0.1 * StateVector<state_dim>::Random();
+
+ // time which has passed since start of MPC
+ auto current_time = std::chrono::high_resolution_clock::now();
+ ct::core::Time t =
+ 1e-6 * std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count();
+
+ // prepare mpc iteration
+ ilqr_mpc.prepareIteration(t);
+
+ // new optimal policy
+ ct::core::StateFeedbackController<state_dim, control_dim> newPolicy;
+
+ // timestamp of the new optimal policy
+ ct::core::Time ts_newPolicy;
+
+ current_time = std::chrono::high_resolution_clock::now();
+ t = 1e-6 * std::chrono::duration_cast<std::chrono::microseconds>(current_time - start_time).count();
+ bool success = ilqr_mpc.finishIteration(x0, t, newPolicy, ts_newPolicy);
+
+ // we break the loop in case the time horizon is reached or solve() failed
+ if (ilqr_mpc.timeHorizonReached() | !success)
+ break;
+ }
+
+
+ // the summary contains some statistical data about time delays, etc.
+ ilqr_mpc.printMpcSummary();
+}
diff --git a/ct_optcon/examples/NLOC_boxConstrained.cpp b/ct_optcon/examples/NLOC_boxConstrained.cpp
new file mode 100644
index 0000000..1f0d219
--- /dev/null
+++ b/ct_optcon/examples/NLOC_boxConstrained.cpp
@@ -0,0 +1,149 @@
+/*!
+ * \example NLOC_boxConstrained.cpp
+ *
+ * This example shows how to use box constraints alongside NLOC and requires HPIPM to be installed
+ * The unconstrained Riccati backward-pass is replaced by a high-performance interior-point
+ * constrained linear-quadratic Optimal Control solver.
+ *
+ */
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+int main(int argc, char **argv)
+{
+ /*get the state and control input dimension of the oscillator. Since we're dealing with a simple oscillator,
+ the state and control dimensions will be state_dim = 2, and control_dim = 1. */
+ const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+ const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+
+ /* STEP 1: set up the Nonlinear Optimal Control Problem
+ * First of all, we need to create instances of the system dynamics, the linearized system and the cost function. */
+
+ /* STEP 1-A: create a instance of the oscillator dynamics for the optimal control problem.
+ * Please also compare the documentation of SecondOrderSystem.h */
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+
+ /* STEP 1-B: Although the first order derivatives of the oscillator are easy to derive, let's illustrate the use of the System Linearizer,
+ * which performs numerical differentiation by the finite-difference method. The system linearizer simply takes the
+ * the system dynamics as argument. Alternatively, you could implement your own first-order derivatives by overloading the class LinearSystem.h */
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+
+ /* STEP 1-C: create a cost function. We have pre-specified the cost-function weights for this problem in "nlocCost.info".
+ * Here, we show how to create terms for intermediate and final cost and how to automatically load them from the configuration file.
+ * The verbose option allows to print information about the loaded terms on the terminal. */
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "finalCost", verbose);
+
+ // Since we are using quadratic cost function terms in this example, the first and second order derivatives are immediately known and we
+ // define the cost function to be an "Analytical Cost Function". Let's create the corresponding object and add the previously loaded
+ // intermediate and final term.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+
+ /* STEP 1-D: set up the box constraints for the control input*/
+ // input box constraint boundaries with sparsities in constraint toolbox format
+ Eigen::VectorXi sp_control(control_dim);
+ sp_control << 1;
+ Eigen::VectorXd u_lb(control_dim);
+ Eigen::VectorXd u_ub(control_dim);
+ u_lb.setConstant(-0.5);
+ u_ub = -u_lb;
+
+ // constraint terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub, sp_control));
+ controlConstraint->setName("ControlInputConstraint");
+
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> boxConstraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // add and initialize constraint terms
+ boxConstraints->addIntermediateConstraint(controlConstraint, verbose);
+ boxConstraints->initialize();
+
+
+ /* STEP 1-E: initialization with initial state and desired time horizon */
+
+ StateVector<state_dim> x0;
+ x0.setRandom(); // in this example, we choose a random initial state x0
+
+ ct::core::Time timeHorizon = 3.0; // and a final time horizon in [sec]
+
+
+ // STEP 1-E: create and initialize an "optimal control problem"
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+ // add the box constraints to the optimal control problem
+ optConProblem.setBoxConstraints(boxConstraints);
+
+ /* STEP 2: set up a nonlinear optimal control solver. */
+
+ /* STEP 2-A: Create the settings.
+ * the type of solver, and most parameters, like number of shooting intervals, etc.,
+ * can be chosen using the following settings struct. Let's use, the iterative
+ * linear quadratic regulator, iLQR, for this example. In the following, we
+ * modify only a few settings, for more detail, check out the NLOptConSettings class. */
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.nThreads = 4;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER; // solve LQ-problems using HPIPM
+ ilqr_settings.lqoc_solver_settings.num_lqoc_iterations = 10; // number of riccati sub-iterations
+ ilqr_settings.printSummary = true;
+
+
+ /* STEP 2-B: provide an initial guess */
+ // calculate the number of time steps K
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ /* design trivial initial controller for iLQR. Note that in this simple example,
+ * we can simply use zero feedforward with zero feedback gains around the initial position.
+ * In more complex examples, a more elaborate initial guess may be required.*/
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // STEP 3: solve the optimal control problem
+ iLQR.solve();
+
+
+ // STEP 4: retrieve the solution
+ ct::core::StateFeedbackController<state_dim, control_dim> solution = iLQR.getSolution();
+
+ // let's plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.x_ref(), solution.uff(), solution.time());
+}
diff --git a/ct_optcon/examples/NLOC_generalConstrained.cpp b/ct_optcon/examples/NLOC_generalConstrained.cpp
new file mode 100644
index 0000000..0329fd7
--- /dev/null
+++ b/ct_optcon/examples/NLOC_generalConstrained.cpp
@@ -0,0 +1,198 @@
+/*!
+ * \example NLOC_generalConstrained.cpp
+ *
+ * This example shows how to use general constraints alongside NLOC and requires HPIPM to be installed
+ * The unconstrained Riccati backward-pass is replaced by a high-performance interior-point
+ * constrained linear-quadratic Optimal Control solver.
+ *
+ */
+
+#include <ct/optcon/optcon.h>
+#include "exampleDir.h"
+#include "plotResultsOscillator.h"
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+
+/*get the state and control input dimension of the oscillator. Since we're dealing with a simple oscillator,
+ the state and control dimensions will be state_dim = 2, and control_dim = 1. */
+static const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM;
+static const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM;
+
+/*!
+ * @brief A simple 1d constraint term.
+ *
+ * This term implements the general inequality constraints
+ * \f$ d_{lb} \leq u \cdot p^2 \leq d_{ub} \f$
+ * where \f$ p \f$ denotes the position of the oscillator mass.
+ *
+ * This constraint can be thought of a position-varying bound on the control input.
+ * At large oscillator deflections, the control bounds shrink
+ */
+class ConstraintTerm1D : public ct::optcon::ConstraintBase<state_dim, control_dim>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename ct::core::tpl::TraitSelector<double>::Trait Trait;
+ typedef typename ct::core::tpl::TraitSelector<ct::core::ADCGScalar>::Trait TraitCG;
+ typedef ct::optcon::ConstraintBase<state_dim, control_dim> Base;
+ typedef ct::core::StateVector<state_dim> state_vector_t;
+ typedef ct::core::ControlVector<control_dim> control_vector_t;
+
+ //! constructor with hard-coded constraint boundaries.
+ ConstraintTerm1D()
+ {
+ Base::lb_.resize(1);
+ Base::ub_.resize(1);
+ Base::lb_.setConstant(-0.5);
+ Base::ub_.setConstant(0.5);
+ }
+
+ virtual ~ConstraintTerm1D() {}
+ virtual ConstraintTerm1D* clone() const override { return new ConstraintTerm1D(); }
+ virtual size_t getConstraintSize() const override { return 1; }
+ virtual Eigen::VectorXd evaluate(const state_vector_t& x, const control_vector_t& u, const double t) override
+ {
+ Eigen::Matrix<double, 1, 1> val;
+ val.template segment<1>(0) << u(0) * x(0) * x(0);
+ return val;
+ }
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const ct::core::StateVector<state_dim, ct::core::ADCGScalar>& x,
+ const ct::core::ControlVector<control_dim, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ Eigen::Matrix<ct::core::ADCGScalar, 1, 1> val;
+
+ val.template segment<1>(0) << u(0) * x(0) * x(0);
+
+ return val;
+ }
+};
+
+
+int main(int argc, char** argv)
+{
+ /* STEP 1: set up the Nonlinear Optimal Control Problem
+ * First of all, we need to create instances of the system dynamics, the linearized system and the cost function. */
+
+ /* STEP 1-A: create a instance of the oscillator dynamics for the optimal control problem.
+ * Please also compare the documentation of SecondOrderSystem.h */
+ double w_n = 0.1;
+ double zeta = 5.0;
+ std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>> oscillatorDynamics(
+ new ct::core::SecondOrderSystem(w_n, zeta));
+
+
+ /* STEP 1-B: Although the first order derivatives of the oscillator are easy to derive, let's illustrate the use of the System Linearizer,
+ * which performs numerical differentiation by the finite-difference method. The system linearizer simply takes the
+ * the system dynamics as argument. Alternatively, you could implement your own first-order derivatives by overloading the class LinearSystem.h */
+ std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>> adLinearizer(
+ new ct::core::SystemLinearizer<state_dim, control_dim>(oscillatorDynamics));
+
+
+ /* STEP 1-C: create a cost function. We have pre-specified the cost-function weights for this problem in "nlocCost.info".
+ * Here, we show how to create terms for intermediate and final cost and how to automatically load them from the configuration file.
+ * The verbose option allows to print information about the loaded terms on the terminal. */
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> intermediateCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> finalCost(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>());
+ bool verbose = true;
+ intermediateCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "intermediateCost", verbose);
+ finalCost->loadConfigFile(ct::optcon::exampleDir + "/nlocCost.info", "finalCost", verbose);
+
+ // Since we are using quadratic cost function terms in this example, the first and second order derivatives are immediately known and we
+ // define the cost function to be an "Analytical Cost Function". Let's create the corresponding object and add the previously loaded
+ // intermediate and final term.
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+ costFunction->addIntermediateTerm(intermediateCost);
+ costFunction->addFinalTerm(finalCost);
+
+
+ /* STEP 1-D: set up the general constraints */
+ // constraint terms
+ std::shared_ptr<ConstraintTerm1D> pathConstraintTerm(new ConstraintTerm1D());
+
+ // create constraint container
+ std::shared_ptr<ct::optcon::ConstraintContainerAD<state_dim, control_dim>> generalConstraints(
+ new ct::optcon::ConstraintContainerAD<state_dim, control_dim>());
+
+
+ // add and initialize constraint terms
+ generalConstraints->addIntermediateConstraint(pathConstraintTerm, verbose);
+ generalConstraints->initialize();
+
+
+ /* STEP 1-E: initialization with initial state and desired time horizon */
+
+ StateVector<state_dim> x0;
+ x0.setRandom(); // in this example, we choose a random initial state x0
+
+ ct::core::Time timeHorizon = 3.0; // and a final time horizon in [sec]
+
+
+ // STEP 1-E: create and initialize an "optimal control problem"
+ OptConProblem<state_dim, control_dim> optConProblem(
+ timeHorizon, x0, oscillatorDynamics, costFunction, adLinearizer);
+
+ // add the box constraints to the optimal control problem
+ optConProblem.setGeneralConstraints(generalConstraints);
+
+
+ /* STEP 2: set up a nonlinear optimal control solver. */
+
+ /* STEP 2-A: Create the settings.
+ * the type of solver, and most parameters, like number of shooting intervals, etc.,
+ * can be chosen using the following settings struct. Let's use, the iterative
+ * linear quadratic regulator, iLQR, for this example. In the following, we
+ * modify only a few settings, for more detail, check out the NLOptConSettings class. */
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.01; // the control discretization in [sec]
+ ilqr_settings.integrator = ct::core::IntegrationType::EULERCT;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.max_iterations = 10;
+ ilqr_settings.min_cost_improvement = 1e-6;
+ ilqr_settings.meritFunctionRhoConstraints = 10;
+ ilqr_settings.nThreads = 1;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER; // solve LQ-problems using HPIPM
+ ilqr_settings.lqoc_solver_settings.num_lqoc_iterations = 10; // number of riccati sub-iterations
+ ilqr_settings.lineSearchSettings.active = true;
+ ilqr_settings.lineSearchSettings.debugPrint = true;
+ ilqr_settings.printSummary = true;
+
+
+ /* STEP 2-B: provide an initial guess */
+ // calculate the number of time steps K
+ size_t K = ilqr_settings.computeK(timeHorizon);
+
+ /* design trivial initial controller for iLQR. Note that in this simple example,
+ * we can simply use zero feedforward with zero feedback gains around the initial position.
+ * In more complex examples, a more elaborate initial guess may be required.*/
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ NLOptConSolver<state_dim, control_dim>::Policy_t initController(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+
+ // STEP 2-C: create an NLOptConSolver instance
+ NLOptConSolver<state_dim, control_dim> iLQR(optConProblem, ilqr_settings);
+
+ // set the initial guess
+ iLQR.setInitialGuess(initController);
+
+
+ // STEP 3: solve the optimal control problem
+ iLQR.solve();
+
+
+ // STEP 4: retrieve the solution
+ ct::core::StateFeedbackController<state_dim, control_dim> solution = iLQR.getSolution();
+
+ // plot the output
+ plotResultsOscillator<state_dim, control_dim>(solution.x_ref(), solution.uff(), solution.time());
+}
diff --git a/ct_optcon/examples/dmsCost.info b/ct_optcon/examples/dmsCost.info
new file mode 100644
index 0000000..59a6a0e
--- /dev/null
+++ b/ct_optcon/examples/dmsCost.info
@@ -0,0 +1,26 @@
+intermediateCost
+{
+ name "intermediate cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 0
+ (1,1) 1
+ }
+ R
+ {
+ scaling 0.1
+ (0,0) 1
+ }
+ x_des
+ {
+ (0,0) 0
+ (1,0) 0
+ }
+ }
+}
diff --git a/ct_optcon/examples/exampleDir.h.in b/ct_optcon/examples/exampleDir.h.in
new file mode 100644
index 0000000..3d6b34c
--- /dev/null
+++ b/ct_optcon/examples/exampleDir.h.in
@@ -0,0 +1,15 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+static const std::string exampleDir = "${CT_OPTCON_EXAMPLE_DIR}";
+
+}
+}
diff --git a/ct_optcon/examples/lqrCost.info b/ct_optcon/examples/lqrCost.info
new file mode 100644
index 0000000..18f9cf3
--- /dev/null
+++ b/ct_optcon/examples/lqrCost.info
@@ -0,0 +1,21 @@
+termLQR
+{
+ name "state cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 1.6
+ (1,1) 1.0
+ }
+ R
+ {
+ scaling 1.0
+ (0,0) 1.0
+ }
+ }
+}
diff --git a/ct_optcon/examples/mpcCost.info b/ct_optcon/examples/mpcCost.info
new file mode 100644
index 0000000..46f9244
--- /dev/null
+++ b/ct_optcon/examples/mpcCost.info
@@ -0,0 +1,54 @@
+intermediateCost
+{
+ name "intermediate cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 0
+ (1,1) 0.1
+ }
+ R
+ {
+ scaling 1
+ (0,0) 1
+ }
+ x_des
+ {
+ (0,0) 0
+ (1,0) 0
+ }
+ }
+}
+
+
+finalCost
+{
+ name "final cost quadratic"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 10.0
+ (0,0) 1.0
+ (1,1) 1.0
+ }
+ R
+ {
+ scaling 0
+ (0,0) 0
+ }
+ x_des
+ {
+ (0,0) 2
+ (1,0) 0
+ }
+ }
+}
diff --git a/ct_optcon/examples/nlocCost.info b/ct_optcon/examples/nlocCost.info
new file mode 100644
index 0000000..f68ae38
--- /dev/null
+++ b/ct_optcon/examples/nlocCost.info
@@ -0,0 +1,54 @@
+intermediateCost
+{
+ name "intermediate cost quadratic"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 1.0
+ (0,0) 0
+ (1,1) 0.1
+ }
+ R
+ {
+ scaling 1
+ (0,0) 1
+ }
+ x_des
+ {
+ (0,0) 0
+ (1,0) 0
+ }
+ }
+}
+
+
+finalCost
+{
+ name "final cost quadratic"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ Q
+ {
+ scaling 100.0
+ (0,0) 1.0
+ (1,1) 1.0
+ }
+ R
+ {
+ scaling 0
+ (0,0) 0
+ }
+ x_des
+ {
+ (0,0) 1.5
+ (1,0) 0
+ }
+ }
+}
diff --git a/ct_optcon/examples/plotResultsOscillator.h b/ct_optcon/examples/plotResultsOscillator.h
new file mode 100644
index 0000000..08af55a
--- /dev/null
+++ b/ct_optcon/examples/plotResultsOscillator.h
@@ -0,0 +1,68 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+void plotResultsOscillator(const ct::core::StateVectorArray<STATE_DIM>& stateArray,
+ const ct::core::ControlVectorArray<CONTROL_DIM>& controlArray,
+ const ct::core::TimeArray& timeArray)
+{
+#ifdef PLOTTING_ENABLED
+
+ using namespace ct::core;
+
+ try
+ {
+ plot::ion();
+ plot::figure();
+
+ if (timeArray.size() != stateArray.size())
+ {
+ std::cout << timeArray.size() << std::endl;
+ std::cout << stateArray.size() << std::endl;
+ throw std::runtime_error("Cannot plot data, x and t not equal length");
+ }
+
+ std::vector<double> position;
+ std::vector<double> velocity;
+ std::vector<double> time_state;
+ for (size_t j = 0; j < stateArray.size(); j++)
+ {
+ position.push_back(stateArray[j](0));
+ velocity.push_back(stateArray[j](1));
+ time_state.push_back(timeArray[j]);
+ }
+
+ std::vector<double> control;
+ std::vector<double> time_control;
+ for (size_t j = 0; j < controlArray.size(); j++)
+ {
+ control.push_back(controlArray[j](0));
+ time_control.push_back(timeArray[j]);
+ }
+
+ plot::subplot(3, 1, 1);
+ plot::plot(time_state, position);
+ plot::title("position");
+
+ plot::subplot(3, 1, 2);
+ plot::plot(time_state, velocity);
+ plot::title("velocity");
+
+ plot::subplot(3, 1, 3);
+ plot::plot(time_control, control);
+ plot::title("control");
+
+ plot::show();
+ } catch (const std::exception& e)
+ {
+ std::cout << e.what() << std::endl;
+ }
+#else
+ std::cout << "Plotting is disabled." << std::endl;
+#endif
+}
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h
new file mode 100644
index 0000000..3668114
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h
@@ -0,0 +1,571 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD()
+{
+ stateControlD_.setZero();
+
+ fIntermediate_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateIntermediateCodegen(stateinput);
+ };
+
+ fTerminal_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateTerminalCodegen(stateinput);
+ };
+
+ intermediateCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount()));
+ terminalCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount()));
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR& t)
+{
+ //Set to some random number which is != the initguess of the problem
+ stateControlD_ << x, u;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD(const ConstraintContainerAD& arg)
+ : LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ fIntermediate_(arg.fIntermediate_),
+ fTerminal_(arg.fTerminal_),
+ sparsityIntermediateRows_(arg.sparsityIntermediateRows_),
+ sparsityStateIntermediateRows_(arg.sparsityStateIntermediateRows_),
+ sparsityStateIntermediateCols_(arg.sparsityStateIntermediateCols_),
+ sparsityInputIntermediateRows_(arg.sparsityInputIntermediateRows_),
+ sparsityInputIntermediateCols_(arg.sparsityInputIntermediateCols_),
+ sparsityTerminalRows_(arg.sparsityTerminalRows_),
+ sparsityStateTerminalRows_(arg.sparsityStateTerminalRows_),
+ sparsityStateTerminalCols_(arg.sparsityStateTerminalCols_),
+ sparsityInputTerminalRows_(arg.sparsityInputTerminalRows_),
+ sparsityInputTerminalCols_(arg.sparsityInputTerminalCols_),
+ stateControlD_(arg.stateControlD_)
+{
+ constraintsIntermediate_.resize(arg.constraintsIntermediate_.size());
+ constraintsTerminal_.resize(arg.constraintsTerminal_.size());
+
+ for (size_t i = 0; i < constraintsIntermediate_.size(); ++i)
+ constraintsIntermediate_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsIntermediate_[i]->clone());
+
+ for (size_t i = 0; i < constraintsTerminal_.size(); ++i)
+ constraintsTerminal_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsTerminal_[i]->clone());
+
+ intermediateCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount()));
+ terminalCodegen_ =
+ std::shared_ptr<JacCG>(new JacCG(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount()));
+
+ // make sure libraries get compiled in clone
+ initializeIntermediate();
+ initializeTerminal();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAD_Raw_Ptr_t
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new ConstraintContainerAD(*this);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintContainerAD()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsIntermediate_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as AD intermediate constraint " << std::endl;
+ }
+
+ fIntermediate_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateIntermediateCodegen(stateinput);
+ };
+
+ intermediateCodegen_->update(fIntermediate_, STATE_DIM + CONTROL_DIM, getIntermediateConstraintsCount());
+
+ this->initializedIntermediate_ = false;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::addTerminalConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsTerminal_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as AD terminal constraint " << std::endl;
+ }
+
+ fTerminal_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput) {
+ return this->evaluateTerminalCodegen(stateinput);
+ };
+
+ terminalCodegen_->update(fTerminal_, STATE_DIM + CONTROL_DIM, getTerminalConstraintsCount());
+
+ this->initializedTerminal_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("evaluateIntermediateConstraints not initialized yet. Call 'initialize()' before");
+
+ return intermediateCodegen_->forwardZero(stateControlD_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("evaluateTerminalConstraints not initialized yet. Call 'initialize()' before");
+
+ return terminalCodegen_->forwardZero(stateControlD_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateConstraintsCount()
+{
+ size_t count = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getTerminalConstraintsCount()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+
+ // std::cout << "jacTot" << std::endl;
+
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianStateNonZeroCountIntermediate());
+ for (size_t i = 0; i < getJacobianStateNonZeroCountIntermediate(); ++i)
+ jacSparse(i) = (jacTot.template leftCols<STATE_DIM>())(
+ sparsityStateIntermediateRows_(i), sparsityStateIntermediateCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+ return jacTot.template leftCols<STATE_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianStateNonZeroCountTerminal());
+ for (size_t i = 0; i < getJacobianStateNonZeroCountTerminal(); ++i)
+ jacSparse(i) =
+ (jacTot.template leftCols<STATE_DIM>())(sparsityStateTerminalRows_(i), sparsityStateTerminalCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ return jacTot.template leftCols<STATE_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseIntermediate()
+{
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianInputNonZeroCountIntermediate());
+ for (size_t i = 0; i < getJacobianInputNonZeroCountIntermediate(); ++i)
+ jacSparse(i) = (jacTot.template rightCols<CONTROL_DIM>())(
+ sparsityInputIntermediateRows_(i), sparsityInputIntermediateCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = intermediateCodegen_->jacobian(stateControlD_);
+ return jacTot.template rightCols<CONTROL_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ VectorXs jacSparse;
+ jacSparse.resize(getJacobianInputNonZeroCountTerminal());
+ for (size_t i = 0; i < getJacobianInputNonZeroCountTerminal(); ++i)
+ jacSparse(i) =
+ (jacTot.template rightCols<CONTROL_DIM>())(sparsityInputTerminalRows_(i), sparsityInputTerminalCols_(i));
+
+ return jacSparse;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ MatrixXs jacTot = terminalCodegen_->jacobian(stateControlD_);
+ return jacTot.template rightCols<CONTROL_DIM>();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateIntermediate(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityStateIntermediateRows_;
+ jCols = sparsityStateIntermediateCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityStateTerminalRows_;
+ jCols = sparsityStateTerminalCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputIntermediate(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityInputIntermediateRows_;
+ jCols = sparsityInputIntermediateCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ iRows = sparsityInputTerminalRows_;
+ jCols = sparsityInputTerminalCols_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityStateIntermediateRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityStateTerminalRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountIntermediate()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityInputIntermediateRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountTerminal()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Constraints not initialized yet. Call 'initialize()' before");
+
+ return sparsityInputTerminalRows_.rows();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::initializeIntermediate()
+{
+ Eigen::VectorXi sparsityRows;
+ Eigen::VectorXi sparsityCols;
+
+ if (getIntermediateConstraintsCount() > 0)
+ {
+ ct::core::DerivativesCppadSettings settings;
+ settings.createForwardZero_ = true;
+ settings.createJacobian_ = true;
+ settings.createSparseJacobian_ = true;
+
+ intermediateCodegen_->compileJIT(settings, "intermediateConstraints");
+ intermediateCodegen_->getSparsityPatternJacobian(sparsityRows, sparsityCols);
+
+ std::cout << "sparsityPattern Intermediate: " << std::endl
+ << intermediateCodegen_->getSparsityPatternJacobian() << std::endl;
+ assert(sparsityRows.rows() == sparsityRows.rows());
+
+ int nonZerosState = (sparsityCols.array() < STATE_DIM).count();
+ int nonZerosInput = (sparsityCols.array() >= STATE_DIM).count();
+
+ sparsityStateIntermediateRows_.resize(nonZerosState);
+ sparsityStateIntermediateCols_.resize(nonZerosState);
+ sparsityInputIntermediateRows_.resize(nonZerosInput);
+ sparsityInputIntermediateCols_.resize(nonZerosInput);
+
+ size_t count = 0;
+
+ this->lowerBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+ this->upperBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsIntermediate_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsIntermediate_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+
+ size_t stateIndex = 0;
+ size_t inputIndex = 0;
+
+ for (size_t i = 0; i < sparsityRows.rows(); ++i)
+ {
+ if (sparsityCols(i) < STATE_DIM)
+ {
+ sparsityStateIntermediateRows_(stateIndex) = sparsityRows(i);
+ sparsityStateIntermediateCols_(stateIndex) = sparsityCols(i);
+ stateIndex++;
+ }
+ else
+ {
+ sparsityInputIntermediateRows_(inputIndex) = sparsityRows(i);
+ sparsityInputIntermediateCols_(inputIndex) = sparsityCols(i) - STATE_DIM;
+ inputIndex++;
+ }
+ }
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::initializeTerminal()
+{
+ Eigen::VectorXi sparsityRows;
+ Eigen::VectorXi sparsityCols;
+
+ if (getTerminalConstraintsCount() > 0)
+ {
+ ct::core::DerivativesCppadSettings settings;
+ settings.createForwardZero_ = true;
+ settings.createJacobian_ = true;
+ settings.createSparseJacobian_ = true;
+
+ terminalCodegen_->compileJIT(settings, "terminalConstraints");
+ terminalCodegen_->getSparsityPatternJacobian(sparsityRows, sparsityCols);
+
+ std::cout << "sparsityPattern Terminal: " << std::endl
+ << terminalCodegen_->getSparsityPatternJacobian() << std::endl;
+ assert(sparsityRows.rows() == sparsityRows.rows());
+
+ int nonZerosState = (sparsityCols.array() < STATE_DIM).count();
+ int nonZerosInput = (sparsityCols.array() >= STATE_DIM).count();
+
+ sparsityStateTerminalRows_.resize(nonZerosState);
+ sparsityStateTerminalCols_.resize(nonZerosState);
+ sparsityInputTerminalRows_.resize(nonZerosInput);
+ sparsityInputTerminalCols_.resize(nonZerosInput);
+
+ size_t count = 0;
+
+ this->lowerBoundsTerminal_.resize(getTerminalConstraintsCount());
+ this->upperBoundsTerminal_.resize(getTerminalConstraintsCount());
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsTerminal_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsTerminal_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+
+ size_t stateIndex = 0;
+ size_t inputIndex = 0;
+
+ for (size_t i = 0; i < sparsityRows.rows(); ++i)
+ {
+ if (sparsityCols(i) < STATE_DIM)
+ {
+ sparsityStateTerminalRows_(stateIndex) = sparsityRows(i);
+ sparsityStateTerminalCols_(stateIndex) = sparsityCols(i);
+ stateIndex++;
+ }
+ else
+ {
+ sparsityInputTerminalRows_(inputIndex) = sparsityRows(i);
+ sparsityInputTerminalCols_(inputIndex) = sparsityCols(i) - STATE_DIM;
+ inputIndex++;
+ }
+ }
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::update()
+{
+ stateControlD_ << this->x_, this->u_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar, Eigen::Dynamic, 1>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediateCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput)
+{
+ size_t count = 0;
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> gLocal;
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ gLocal.conservativeResize(count + constraint_dim);
+ gLocal.segment(count, constraint_dim) = constraint->evaluateCppadCg(
+ stateinput.segment(0, STATE_DIM), stateinput.segment(STATE_DIM, CONTROL_DIM), CGScalar(0.0));
+ count += constraint_dim;
+ }
+ return gLocal;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<typename ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar, Eigen::Dynamic, 1>
+ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminalCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput)
+{
+ size_t count = 0;
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> gLocal;
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ gLocal.conservativeResize(count + constraint_dim);
+ gLocal.segment(count, constraint_dim) = constraint->evaluateCppadCg(
+ stateinput.segment(0, STATE_DIM), stateinput.segment(STATE_DIM, CONTROL_DIM), CGScalar(0.0));
+ count += constraint_dim;
+ }
+
+ return gLocal;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD.h
new file mode 100644
index 0000000..022b7af
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD.h
@@ -0,0 +1,188 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "LinearConstraintContainer.h"
+#include "term/ConstraintBase.h"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Contains all the constraints using with AD generated jacobians
+ *
+ * @tparam STATE_DIM { description }
+ * @tparam CONTROL_DIM { description }
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintContainerAD : public LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::DerivativesCppadJIT<STATE_DIM + CONTROL_DIM, -1> JacCG;
+ typedef typename JacCG::CG_SCALAR CGScalar;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef ConstraintContainerAD<STATE_DIM, CONTROL_DIM, SCALAR>* ConstraintContainerAD_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+
+ /**
+ * @brief Basic constructor
+ */
+ ConstraintContainerAD();
+
+ /**
+ * \brief Constructor using state, control and time
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ ConstraintContainerAD(const state_vector_t& x, const input_vector_t& u, const SCALAR& t = 0.0);
+
+
+ /**
+ * \brief Copy constructor
+ * @param arg constraint class to copy
+ */
+ ConstraintContainerAD(const ConstraintContainerAD& arg);
+
+ /**
+ * Deep-cloning of Constraint
+ *
+ * @return base pointer to the clone
+ */
+ virtual ConstraintContainerAD_Raw_Ptr_t clone() const override;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintContainerAD();
+
+ /**
+ * @brief Adds an intermediate constraint.
+ *
+ * @param[in] constraint The constraint
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addIntermediateConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ /**
+ * @brief Adds a terminal constraint.
+ *
+ * @param[in] constraint The constraint
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addTerminalConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ virtual VectorXs evaluateIntermediate() override;
+
+ virtual VectorXs evaluateTerminal() override;
+
+ virtual size_t getIntermediateConstraintsCount() override;
+
+ virtual size_t getTerminalConstraintsCount() override;
+
+ virtual VectorXs jacobianStateSparseIntermediate() override;
+
+ virtual MatrixXs jacobianStateIntermediate() override;
+
+ virtual VectorXs jacobianStateSparseTerminal() override;
+
+ virtual MatrixXs jacobianStateTerminal() override;
+
+ virtual VectorXs jacobianInputSparseIntermediate() override;
+
+ virtual MatrixXs jacobianInputIntermediate() override;
+
+ virtual VectorXs jacobianInputSparseTerminal() override;
+
+ virtual MatrixXs jacobianInputTerminal() override;
+
+ virtual void sparsityPatternStateIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternStateTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual size_t getJacobianStateNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianStateNonZeroCountTerminal() override;
+
+ virtual size_t getJacobianInputNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianInputNonZeroCountTerminal() override;
+
+ virtual bool initializeIntermediate() override;
+
+ virtual bool initializeTerminal() override;
+
+private:
+ // cache the matrix here, no need to call it at every eval matrix
+ virtual void update() override;
+
+ /**
+ * @brief Helper function to keep track of the constraint evaluation
+ * used for cppad codegeneration
+ *
+ * @param[in] stateinput The stacked state input vector
+ *
+ * @return The evaluated intermediate constraints
+ */
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> evaluateIntermediateCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput);
+
+ /**
+ * @brief Helper function to keep track of the constraint evaluation
+ * used for cppad codegeneration
+ *
+ * @param[in] stateinput The stacked state input vector
+ *
+ * @return The evaluated terminal constraints
+ */
+ Eigen::Matrix<CGScalar, Eigen::Dynamic, 1> evaluateTerminalCodegen(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM, 1>& stateinput);
+
+ //containers
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsIntermediate_;
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsTerminal_;
+
+ std::shared_ptr<JacCG> intermediateCodegen_;
+ std::shared_ptr<JacCG> terminalCodegen_;
+
+ typename JacCG::FUN_TYPE_CG fIntermediate_;
+ typename JacCG::FUN_TYPE_CG fTerminal_;
+
+ Eigen::VectorXi sparsityIntermediateRows_;
+ Eigen::VectorXi sparsityStateIntermediateRows_;
+ Eigen::VectorXi sparsityStateIntermediateCols_;
+ Eigen::VectorXi sparsityInputIntermediateRows_;
+ Eigen::VectorXi sparsityInputIntermediateCols_;
+
+ Eigen::VectorXi sparsityTerminalRows_;
+ Eigen::VectorXi sparsityStateTerminalRows_;
+ Eigen::VectorXi sparsityStateTerminalCols_;
+ Eigen::VectorXi sparsityInputTerminalRows_;
+ Eigen::VectorXi sparsityInputTerminalCols_;
+
+
+ Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM, 1> stateControlD_; /** contains x, u in stacked form */
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical-impl.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical-impl.h
new file mode 100644
index 0000000..c747332
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical-impl.h
@@ -0,0 +1,590 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR& t)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical(
+ const ConstraintContainerAnalytical& arg)
+ : LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ constraintsIntermediate_(arg.constraintsIntermediate_),
+ constraintsTerminal_(arg.constraintsTerminal_),
+ evalIntermediate_(arg.evalIntermediate_),
+ evalJacSparseStateIntermediate_(arg.evalJacSparseStateIntermediate_),
+ evalJacSparseInputIntermediate_(arg.evalJacSparseInputIntermediate_),
+ evalJacDenseStateIntermediate_(arg.evalJacDenseStateIntermediate_),
+ evalJacDenseInputIntermediate_(arg.evalJacDenseInputIntermediate_),
+ evalTerminal_(arg.evalTerminal_),
+ evalJacSparseStateTerminal_(arg.evalJacSparseStateTerminal_),
+ evalJacSparseInputTerminal_(arg.evalJacSparseInputTerminal_),
+ evalJacDenseStateTerminal_(arg.evalJacDenseStateTerminal_),
+ evalJacDenseInputTerminal_(arg.evalJacDenseInputTerminal_)
+{
+ // vectors of terms can be resized easily
+ constraintsIntermediate_.resize(arg.constraintsIntermediate_.size());
+ constraintsTerminal_.resize(arg.constraintsTerminal_.size());
+
+ for (size_t i = 0; i < constraintsIntermediate_.size(); ++i)
+ constraintsIntermediate_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsIntermediate_[i]->clone());
+
+ for (size_t i = 0; i < constraintsTerminal_.size(); ++i)
+ constraintsTerminal_[i] =
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.constraintsTerminal_[i]->clone());
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerAnalytical_Raw_Ptr_t
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintContainerAnalytical()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsIntermediate_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as Analytical intermediate constraint " << std::endl;
+ }
+ this->initializedIntermediate_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::addTerminalConstraint(
+ std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose)
+{
+ constraintsTerminal_.push_back(constraint);
+ if (verbose)
+ {
+ std::string name;
+ constraint->getName(name);
+ std::cout << "''" << name << "'' added as Analytical terminal constraint " << std::endl;
+ }
+ this->initializedTerminal_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalIntermediate_.segment(count, constraint_dim) = constraint->evaluate(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+ return evalIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ checkTerminalConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalTerminal_.segment(count, constraint_dim) = constraint->evaluate(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+ return evalTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateConstraintsCount()
+{
+ size_t count = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getTerminalConstraintsCount()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getConstraintSize();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+
+ if (nonZerosState != 0)
+ {
+ evalJacSparseStateIntermediate_.segment(count, nonZerosState) =
+ constraint->jacobianStateSparse(this->x_, this->u_, this->t_);
+ count += nonZerosState;
+ }
+ }
+ return evalJacSparseStateIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateIntermediate()
+{
+ checkIntermediateConstraints();
+
+ MatrixXs jacLocal;
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseStateIntermediate_.block(count, 0, constraint_dim, STATE_DIM) =
+ constraint->jacobianState(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseStateIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparseTerminal()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+
+ if (nonZerosState != 0)
+ {
+ evalJacSparseStateTerminal_.segment(count, nonZerosState) =
+ constraint->jacobianStateSparse(this->x_, this->u_, this->t_);
+ count += nonZerosState;
+ }
+ }
+ return evalJacSparseStateTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateTerminal()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseStateTerminal_.block(count, 0, constraint_dim, STATE_DIM) =
+ constraint->jacobianState(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseStateTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+
+ if (nonZerosInput != 0)
+ {
+ evalJacSparseInputIntermediate_.segment(count, nonZerosInput) =
+ constraint->jacobianInputSparse(this->x_, this->u_, this->t_);
+ count += nonZerosInput;
+ }
+ }
+ return evalJacSparseInputIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputIntermediate()
+{
+ checkIntermediateConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseInputIntermediate_.block(count, 0, constraint_dim, CONTROL_DIM) =
+ constraint->jacobianInput(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseInputIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparseTerminal()
+{
+ checkTerminalConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+
+ if (nonZerosInput != 0)
+ {
+ evalJacSparseInputTerminal_.segment(count, nonZerosInput) =
+ constraint->jacobianInputSparse(this->x_, this->u_, this->t_);
+ count += nonZerosInput;
+ }
+ }
+ return evalJacSparseInputTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputTerminal()
+{
+ checkTerminalConstraints();
+
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraint_dim = constraint->getConstraintSize();
+ evalJacDenseInputTerminal_.block(count, 0, constraint_dim, CONTROL_DIM) =
+ constraint->jacobianInput(this->x_, this->u_, this->t_);
+ count += constraint_dim;
+ }
+
+ return evalJacDenseInputTerminal_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateIntermediate(
+ Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ checkIntermediateConstraints();
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+ if (nonZerosState > 0)
+ {
+ constraint->sparsityPatternState(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosState);
+ iRowTot.segment(count, nonZerosState) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosState);
+ jColTot.segment(count, nonZerosState) = jColLocal;
+
+ count += nonZerosState;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternStateTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error(
+ "sparsityPatternStateTerminalConstraints not initialized yet. Call 'initialize()' before");
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosState = constraint->getNumNonZerosJacobianState();
+ if (nonZerosState > 0)
+ {
+ constraint->sparsityPatternState(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosState);
+ iRowTot.segment(count, nonZerosState) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosState);
+ jColTot.segment(count, nonZerosState) = jColLocal;
+
+ count += nonZerosState;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputIntermediate(
+ Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ checkIntermediateConstraints();
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+ if (nonZerosInput > 0)
+ {
+ constraint->sparsityPatternInput(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosInput);
+ iRowTot.segment(count, nonZerosInput) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosInput);
+ jColTot.segment(count, nonZerosInput) = jColLocal;
+
+ count += nonZerosInput;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInputTerminal(Eigen::VectorXi& iRows,
+ Eigen::VectorXi& jCols)
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error(
+ "sparsityPatternInputTerminalConstraints not initialized yet. Call 'initialize()' before");
+
+ Eigen::VectorXi iRowLocal;
+ Eigen::VectorXi jColLocal;
+ Eigen::VectorXi iRowTot;
+ Eigen::VectorXi jColTot;
+
+ size_t count = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t nonZerosInput = constraint->getNumNonZerosJacobianInput();
+ if (nonZerosInput > 0)
+ {
+ constraint->sparsityPatternInput(iRowLocal, jColLocal);
+
+ iRowTot.conservativeResize(count + nonZerosInput);
+ iRowTot.segment(count, nonZerosInput) = iRowLocal.array() + constraintCount;
+
+ jColTot.conservativeResize(count + nonZerosInput);
+ jColTot.segment(count, nonZerosInput) = jColLocal;
+
+ count += nonZerosInput;
+ }
+ constraintCount += constraint->getConstraintSize();
+ }
+
+ iRows = iRowTot;
+ jCols = jColTot;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountIntermediate()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getNumNonZerosJacobianState();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianStateNonZeroCountTerminal()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getNumNonZerosJacobianState();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountIntermediate()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsIntermediate_)
+ count += constraint->getNumNonZerosJacobianInput();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::getJacobianInputNonZeroCountTerminal()
+{
+ size_t count = 0;
+ for (auto constraint : constraintsTerminal_)
+ count += constraint->getNumNonZerosJacobianInput();
+
+ return count;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::initializeIntermediate()
+{
+ if (getIntermediateConstraintsCount() > 0)
+ {
+ evalIntermediate_.resize(getIntermediateConstraintsCount());
+ evalIntermediate_.setZero();
+ evalJacSparseStateIntermediate_.resize(getJacobianStateNonZeroCountIntermediate());
+ evalJacSparseStateIntermediate_.setZero();
+ evalJacSparseInputIntermediate_.resize(getJacobianInputNonZeroCountIntermediate());
+ evalJacSparseInputIntermediate_.setZero();
+ evalJacDenseStateIntermediate_.resize(getIntermediateConstraintsCount(), STATE_DIM);
+ evalJacDenseStateIntermediate_.setZero();
+ evalJacDenseInputIntermediate_.resize(getIntermediateConstraintsCount(), CONTROL_DIM);
+ evalJacDenseInputIntermediate_.setZero();
+
+ size_t count = 0;
+
+ this->lowerBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+ this->lowerBoundsIntermediate_.setZero();
+ this->upperBoundsIntermediate_.resize(getIntermediateConstraintsCount());
+ this->upperBoundsIntermediate_.setZero();
+
+ for (auto constraint : constraintsIntermediate_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsIntermediate_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsIntermediate_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+ }
+
+ return true;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::initializeTerminal()
+{
+ if (getTerminalConstraintsCount() > 0)
+ {
+ evalTerminal_.resize(getTerminalConstraintsCount());
+ evalTerminal_.setZero();
+ evalJacSparseStateTerminal_.resize(getJacobianStateNonZeroCountTerminal());
+ evalJacSparseStateTerminal_.setZero();
+ evalJacSparseInputTerminal_.resize(getJacobianInputNonZeroCountTerminal());
+ evalJacSparseInputTerminal_.setZero();
+ evalJacDenseStateTerminal_.resize(getTerminalConstraintsCount(), STATE_DIM);
+ evalJacDenseStateTerminal_.setZero();
+ evalJacDenseInputTerminal_.resize(getTerminalConstraintsCount(), CONTROL_DIM);
+ evalJacDenseInputTerminal_.setZero();
+
+ size_t count = 0;
+
+ this->lowerBoundsTerminal_.resize(getTerminalConstraintsCount());
+ this->lowerBoundsTerminal_.setZero();
+ this->upperBoundsTerminal_.resize(getTerminalConstraintsCount());
+ this->upperBoundsTerminal_.setZero();
+
+ for (auto constraint : constraintsTerminal_)
+ {
+ size_t constraintSize = constraint->getConstraintSize();
+ this->lowerBoundsTerminal_.segment(count, constraintSize) = constraint->getLowerBound();
+ this->upperBoundsTerminal_.segment(count, constraintSize) = constraint->getUpperBound();
+ count += constraintSize;
+ }
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::update()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::checkIntermediateConstraints()
+{
+ if (!this->initializedIntermediate_)
+ throw std::runtime_error("Error: Intermediate constraints are or not initialized yet. ");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::checkTerminalConstraints()
+{
+ if (!this->initializedTerminal_)
+ throw std::runtime_error("Error: Terminal constraints are either not initialized yet. ");
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical.h
new file mode 100644
index 0000000..ddefee6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical.h
@@ -0,0 +1,167 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cppad/example/cppad_eigen.hpp>
+
+#include "LinearConstraintContainer.h"
+#include "term/ConstraintBase.h"
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Contains all the constraints using analytically calculated
+ * jacobians
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintContainerAnalytical : public LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef ConstraintContainerAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>* ConstraintContainerAnalytical_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ ConstraintContainerAnalytical();
+
+ /**
+ * @brief Constructor using state, control and time
+ *
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ ConstraintContainerAnalytical(const state_vector_t& x, const input_vector_t& u, const SCALAR& t = 0.0);
+
+ /**
+ * @brief Copy constructor
+ *
+ * @param arg constraint class to copy
+ */
+ ConstraintContainerAnalytical(const ConstraintContainerAnalytical& arg);
+
+ /**
+ * @brief Deep-cloning of Constraint
+ *
+ * @return Copy of this object.
+ */
+ virtual ConstraintContainerAnalytical_Raw_Ptr_t clone() const override;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintContainerAnalytical();
+
+ /**
+ * @brief Adds an intermedaite constraint.
+ *
+ * @param[in] constraint The constraint to be added
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addIntermediateConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ /**
+ * @brief Adds a terminal constraint.
+ *
+ * @param[in] constraint The constraint to be added
+ * @param[in] verbose Flag indicating whether verbosity is on or off
+ */
+ void addTerminalConstraint(std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>> constraint,
+ bool verbose);
+
+ virtual VectorXs evaluateIntermediate() override;
+
+ virtual VectorXs evaluateTerminal() override;
+
+ virtual size_t getIntermediateConstraintsCount() override;
+
+ virtual size_t getTerminalConstraintsCount() override;
+
+ virtual VectorXs jacobianStateSparseIntermediate() override;
+
+ virtual MatrixXs jacobianStateIntermediate() override;
+
+ virtual VectorXs jacobianStateSparseTerminal() override;
+
+ virtual MatrixXs jacobianStateTerminal() override;
+
+ virtual VectorXs jacobianInputSparseIntermediate() override;
+
+ virtual MatrixXs jacobianInputIntermediate() override;
+
+ virtual VectorXs jacobianInputSparseTerminal() override;
+
+ virtual MatrixXs jacobianInputTerminal() override;
+
+ virtual void sparsityPatternStateIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternStateTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual void sparsityPatternInputTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) override;
+
+ virtual size_t getJacobianStateNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianStateNonZeroCountTerminal() override;
+
+ virtual size_t getJacobianInputNonZeroCountIntermediate() override;
+
+ virtual size_t getJacobianInputNonZeroCountTerminal() override;
+
+ virtual bool initializeIntermediate() override;
+
+ virtual bool initializeTerminal() override;
+
+
+private:
+ virtual void update() override;
+
+ /**
+ * @brief Checks whether the intermediate constraints are initialized.
+ * Throws a runtime error if not.
+ */
+ void checkIntermediateConstraints();
+
+ /**
+ * @brief Checks whether the terminal constraints are initialized.
+ * Throws a runtime error if not.
+ */
+ void checkTerminalConstraints();
+
+
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsIntermediate_;
+ std::vector<std::shared_ptr<ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>>> constraintsTerminal_;
+
+ VectorXs evalIntermediate_;
+ VectorXs evalJacSparseStateIntermediate_;
+ VectorXs evalJacSparseInputIntermediate_;
+ MatrixXs evalJacDenseStateIntermediate_;
+ MatrixXs evalJacDenseInputIntermediate_;
+
+ VectorXs evalTerminal_;
+ VectorXs evalJacSparseStateTerminal_;
+ VectorXs evalJacSparseInputTerminal_;
+ MatrixXs evalJacDenseStateTerminal_;
+ MatrixXs evalJacDenseInputTerminal_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase-impl.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase-impl.h
new file mode 100644
index 0000000..c58e22a
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase-impl.h
@@ -0,0 +1,133 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerBase()
+ : x_(state_vector_t::Zero()), u_(input_vector_t::Zero()), t_(SCALAR(0.0))
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintContainerBase(const ConstraintContainerBase& arg)
+ : x_(arg.x_),
+ u_(arg.u_),
+ t_(arg.t_),
+ lowerBoundsIntermediate_(arg.lowerBoundsIntermediate_),
+ lowerBoundsTerminal_(arg.lowerBoundsTerminal_),
+ upperBoundsIntermediate_(arg.upperBoundsIntermediate_),
+ upperBoundsTerminal_(arg.upperBoundsTerminal_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintContainerBase()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR t)
+{
+ t_ = t;
+ x_ = x;
+ u_ = u;
+ update();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintsCount()
+{
+ return getIntermediateConstraintsCount() + getTerminalConstraintsCount();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsIntermediate() const
+{
+ return lowerBoundsIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsTerminal() const
+{
+ return lowerBoundsTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsIntermediate() const
+{
+ return upperBoundsIntermediate_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsTerminal() const
+{
+ return upperBoundsTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsViolationIntermediate()
+{
+ const VectorXs vZero = VectorXs::Zero(upperBoundsIntermediate_.rows());
+ return (evaluateIntermediate() - upperBoundsIntermediate_).array().max(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsViolationIntermediate()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsIntermediate_.rows());
+ return (evaluateIntermediate() - lowerBoundsIntermediate_).array().min(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBoundsViolationTerminal()
+{
+ const VectorXs vZero = VectorXs::Zero(upperBoundsTerminal_.rows());
+ return (evaluateTerminal() - upperBoundsTerminal_).array().max(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBoundsViolationTerminal()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsTerminal_.rows());
+ return (evaluateTerminal() - lowerBoundsTerminal_).array().min(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getTotalBoundsViolationIntermediate()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsIntermediate_.rows());
+ VectorXs eval = evaluateIntermediate();
+ return (eval - lowerBoundsIntermediate_).array().min(vZero.array()) +
+ (eval - upperBoundsIntermediate_).array().max(vZero.array());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>::getTotalBoundsViolationTerminal()
+{
+ const VectorXs vZero = VectorXs::Zero(lowerBoundsTerminal_.rows());
+ VectorXs eval = evaluateTerminal();
+ return (eval - lowerBoundsTerminal_).array().min(vZero.array()) +
+ (eval - upperBoundsTerminal_).array().max(vZero.array());
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase.h b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase.h
new file mode 100644
index 0000000..293c342
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase.h
@@ -0,0 +1,210 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/** \defgroup Constraint Constraint
+ *
+ * \brief Constraint Module used in Optimal Control
+ */
+
+/**
+ * @ingroup Constraint
+ * + { list_item_description }
+ *
+ * @brief The ConstraintBase Class is the base class for defining the
+ * non-linear optimization constraints.
+ *
+ * @tparam STATE_DIM Dimension of the state vector
+ * @tparam CONTROL_DIM Dimension of the control input vector
+ *
+ * An example for usage is given in the unit test @ref
+ * ConstraintTest.h
+ */
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintContainerBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>* ConstraintBase_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * \brief Default constructor
+ *
+ * Default constructor, sets state, control and time to zero
+ */
+ ConstraintContainerBase();
+
+ /**
+ * \brief copy constructor
+ *
+ * Copy constructor
+ */
+ ConstraintContainerBase(const ConstraintContainerBase& arg);
+
+ /**
+ * \brief Destructor
+ *
+ * Destructor
+ */
+ virtual ~ConstraintContainerBase();
+
+ /**
+ * Clones the constraint.
+ * @return Base pointer to the clone
+ */
+ virtual ConstraintBase_Raw_Ptr_t clone() const = 0;
+
+ /**
+ * This methods updates the current time, state, and input in the class. It also call the user defined update() method,
+ * which can be used to make the implementation more efficient by executing shared calculations directly at the time of
+ * updating time, state and control.
+ *
+ * @param[in] t current time
+ * @param[in] x state vector
+ * @param[in] x input vector
+ */
+ virtual void setCurrentStateAndControl(const state_vector_t& x,
+ const input_vector_t& u,
+ const SCALAR t = SCALAR(0.0));
+
+ /**
+ * @brief Evaluates the intermediate constraints
+ *
+ * @return The evaluation of the intermediate constraints
+ */
+ virtual VectorXs evaluateIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the terminal constraints
+ *
+ * @return The evaluation of the terminal constraints
+ */
+ virtual VectorXs evaluateTerminal() = 0;
+
+ /**
+ * @brief Retrieves the number of intermediate constraints
+ *
+ * @return The number of intermediate constraints
+ */
+ virtual size_t getIntermediateConstraintsCount() = 0;
+
+ /**
+ * @brief Retrieves the number of final constraints
+ *
+ * @return The number of final constraints
+ */
+ virtual size_t getTerminalConstraintsCount() = 0;
+
+ /**
+ * @brief Retrieves the total number of constraints
+ *
+ * @return The total number of constraints
+ */
+ size_t getConstraintsCount();
+ /**
+ * @brief Retrieves the lower constraint bound on the intermediate
+ * constraints
+ *
+ * @return The lower bound on the intermediate constraints
+ */
+ VectorXs getLowerBoundsIntermediate() const;
+
+ /**
+ * @brief Retrieves the lower constraint bound on the terminal
+ * constraints
+ *
+ * @return The lower bound on the terminal constraints
+ */
+ VectorXs getLowerBoundsTerminal() const;
+
+ /**
+ * @brief Retrieves the upper constraint bound on the intermediate
+ * constraints
+ *
+ * @return The upper bound on the intermediate constraints
+ */
+ VectorXs getUpperBoundsIntermediate() const;
+
+ /**
+ * @brief Retrieves the upper constraint bound on the terminal
+ * constraints
+ *
+ * @return The upper bound on the terminal constraints
+ */
+ VectorXs getUpperBoundsTerminal() const;
+
+ /**
+ * @brief Retrieves the violation of the upper constraint bound on the intermediate constraints
+ *
+ * @return The upper bound violation on intermediate constraints
+ */
+ VectorXs getUpperBoundsViolationIntermediate();
+
+ /**
+ * @brief Retrieves the violation of the lower constraint bound on the intermediate constraints
+ *
+ * @return The lower bound violation on intermediate constraints
+ */
+ VectorXs getLowerBoundsViolationIntermediate();
+
+ /**
+ * @brief Retrieves the violation of the upper constraint bound on the terminal constraints
+ *
+ * @return The upper bound violation on terminal constraints
+ */
+ VectorXs getUpperBoundsViolationTerminal();
+
+ /**
+ * @brief Retrieves the violation of the lower constraint bound on the terminal constraints
+ *
+ * @return The lower bound violation on terminal constraints
+ */
+ VectorXs getLowerBoundsViolationTerminal();
+
+ /**
+ * @brief Retrieves the total violation of the constraints bounds on the intermediate constraints
+ *
+ * @return The total bound violation on intermediate constraints
+ */
+ VectorXs getTotalBoundsViolationIntermediate();
+
+ /**
+ * @brief Retrieves the total violation of the constraints bounds on the terminal constraints
+ *
+ * @return The total bound violation on terminal constraints
+ */
+ VectorXs getTotalBoundsViolationTerminal();
+
+protected:
+ /**
+ * @brief Gets called by the setCurrentStateAndControl method. Can be
+ * used to update container properties
+ */
+ virtual void update() = 0;
+
+ state_vector_t x_; /** state vector */
+ input_vector_t u_; /** control vector */
+ SCALAR t_; /** time */
+
+ VectorXs lowerBoundsIntermediate_;
+ VectorXs lowerBoundsTerminal_;
+ VectorXs upperBoundsIntermediate_;
+ VectorXs upperBoundsTerminal_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer-impl.h b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer-impl.h
new file mode 100644
index 0000000..fe2063b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer-impl.h
@@ -0,0 +1,138 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::LinearConstraintContainer()
+ : initializedIntermediate_(false), initializedTerminal_(false)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::LinearConstraintContainer(
+ const LinearConstraintContainer& arg)
+ : ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ initializedIntermediate_(arg.initializedIntermediate_),
+ initializedTerminal_(arg.initializedTerminal_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::~LinearConstraintContainer()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::getJacNonZeroCount()
+{
+ return getJacobianStateNonZeroCountIntermediate() + getJacobianStateNonZeroCountTerminal() +
+ getJacobianInputNonZeroCountIntermediate() + getJacobianInputNonZeroCountTerminal();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::initialize()
+{
+ initializedIntermediate_ = initializeIntermediate();
+ initializedTerminal_ = initializeTerminal();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::isInitialized()
+{
+ return initializedIntermediate_ && initializedTerminal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>::printout()
+{
+ Eigen::VectorXi iRows, jCols; // local var
+
+ std::cout << std::endl;
+
+ std::cout << "LinearConstraintContainer - Printout INTERMEDIATE STAGE" << std::endl;
+
+ std::cout << "getLowerBoundsIntermediate().transpose() :" << std::endl;
+ std::cout << this->getLowerBoundsIntermediate().transpose() << std::endl;
+
+ std::cout << "getUpperBoundsIntermediate().transpose() :" << std::endl;
+ std::cout << this->getUpperBoundsIntermediate().transpose() << std::endl;
+
+ std::cout << "getJacobianStateNonZeroCountIntermediate() :" << std::endl;
+ std::cout << getJacobianStateNonZeroCountIntermediate() << std::endl;
+
+ std::cout << "getJacobianInputNonZeroCountIntermediate() :" << std::endl;
+ std::cout << getJacobianInputNonZeroCountIntermediate() << std::endl;
+
+ std::cout << "jacobianStateIntermediate() :" << std::endl;
+ std::cout << jacobianStateIntermediate() << std::endl;
+
+ std::cout << "jacobianStateSparseIntermediate() :" << std::endl;
+ std::cout << jacobianStateSparseIntermediate() << std::endl;
+
+ std::cout << "jacobianInputIntermediate() :" << std::endl;
+ std::cout << jacobianInputIntermediate() << std::endl;
+
+ std::cout << "jacobianInputSparseIntermediate() :" << std::endl;
+ std::cout << jacobianInputSparseIntermediate() << std::endl;
+
+ std::cout << "sparsityPatternStateIntermediate(iRows, jCols) :" << std::endl;
+ sparsityPatternStateIntermediate(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+ std::cout << "sparsityPatternInputIntermediate(iRows, jCols) :" << std::endl;
+ sparsityPatternInputIntermediate(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+
+ std::cout << "LinearConstraintContainer - Printout TERMINAL STAGE" << std::endl;
+
+ std::cout << "getLowerBoundsTerminal().transpose() :" << std::endl;
+ std::cout << this->getLowerBoundsTerminal().transpose() << std::endl;
+
+ std::cout << "getUpperBoundsTerminal().transpose() :" << std::endl;
+ std::cout << this->getUpperBoundsTerminal().transpose() << std::endl;
+
+ std::cout << "getJacobianStateNonZeroCountTerminal() :" << std::endl;
+ std::cout << getJacobianStateNonZeroCountTerminal() << std::endl;
+
+ std::cout << "getJacobianInputNonZeroCountTerminal() :" << std::endl;
+ std::cout << getJacobianInputNonZeroCountTerminal() << std::endl;
+
+ std::cout << "jacobianStateTerminal() :" << std::endl;
+ std::cout << jacobianStateTerminal() << std::endl;
+
+ std::cout << "jacobianStateSparseTerminal() :" << std::endl;
+ std::cout << jacobianStateSparseTerminal() << std::endl;
+
+ std::cout << "jacobianInputTerminal() :" << std::endl;
+ std::cout << jacobianInputTerminal() << std::endl;
+
+ std::cout << "jacobianInputSparseTerminal() :" << std::endl;
+ std::cout << jacobianInputSparseTerminal() << std::endl;
+
+ std::cout << "sparsityPatternStateTerminal(iRows, jCols) :" << std::endl;
+ sparsityPatternStateTerminal(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+ std::cout << "sparsityPatternInputTerminal(iRows, jCols) :" << std::endl;
+ sparsityPatternInputTerminal(iRows, jCols);
+ std::cout << "iRows: " << iRows.transpose() << std::endl;
+ std::cout << "jCols: " << jCols.transpose() << std::endl;
+
+ std::cout << std::endl;
+ std::cout << std::endl;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer.h b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer.h
new file mode 100644
index 0000000..74d560c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer.h
@@ -0,0 +1,244 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ConstraintContainerBase.h"
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief A base function for linear constraint functions which have a
+ * first derivative
+ *
+ * * The LinearConstraintBase Class is the base class for defining the
+ * non-linear optimization constraints.
+ *
+ * @tparam STATE_DIM Dimension of the state vector
+ * @tparam CONTROL_DIM Dimension of the control input vector
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class LinearConstraintContainer : public ConstraintContainerBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> input_vector_t;
+
+ typedef LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>* LinearConstraintContainer_Raw_Ptr_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Default constructor
+ */
+ LinearConstraintContainer();
+
+
+ /**
+ * @brief Copy constructor
+ *
+ * @param[in] arg The object to be copied
+ */
+ LinearConstraintContainer(const LinearConstraintContainer& arg);
+ /**
+ * @brief Destructor
+ *
+ */
+ virtual ~LinearConstraintContainer();
+
+ /**
+ * Clones the linear constraint class
+ * @return pointer to the clone
+ */
+ virtual LinearConstraintContainer_Raw_Ptr_t clone() const = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianStateSparseIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state
+ *
+ * @return The jacobian wrt state
+ */
+ virtual MatrixXs jacobianStateIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianStateSparseTerminal() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the state
+ *
+ * @return The jacobian wrt state
+ */
+ virtual MatrixXs jacobianStateTerminal() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianInputSparseIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input
+ *
+ * @return The jacobian wrt control
+ */
+ virtual MatrixXs jacobianInputIntermediate() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input using sparse representation
+ *
+ * @param jacVec The sparse jacobian vector
+ * @param count The size of jacVec
+ */
+ virtual VectorXs jacobianInputSparseTerminal() = 0;
+
+ /**
+ * @brief Evaluates the constraint jacobian wrt the control input
+ *
+ * @return The jacobian wrt control
+ */
+ virtual MatrixXs jacobianInputTerminal() = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt state
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternStateIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt state
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternStateTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt control
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternInputIntermediate(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the sparsity pattern for the jacobian wrt control
+ *
+ * @param iRows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param jCols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ *
+ * @return The size of iRow and jCols
+ */
+ virtual void sparsityPatternInputTerminal(Eigen::VectorXi& iRows, Eigen::VectorXi& jCols) = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt state
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianStateNonZeroCountIntermediate() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt state
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianStateNonZeroCountTerminal() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt input
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianInputNonZeroCountIntermediate() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements in the constraint jacobian wrt input
+ *
+ * @return The number of the non zeros
+ */
+ virtual size_t getJacobianInputNonZeroCountTerminal() = 0;
+
+ /**
+ * @brief Returns the number of non zeros in the constraint jacobian
+ * wrt to state and input
+ *
+ * @return The number of the non zeros
+ */
+ size_t getJacNonZeroCount();
+ /**
+ * @brief Initializes the constraint container
+ */
+ void initialize();
+
+ /**
+ * @brief Initializes the intermediate constraints
+ *
+ * @return Returns true if the initialization was successful
+ */
+ virtual bool initializeIntermediate() = 0;
+
+ /**
+ * @brief Initializes the terminal constraints
+ *
+ * @return Returns true if the initialization was successful
+ */
+ virtual bool initializeTerminal() = 0;
+
+ /**
+ * @brief Checks if the constraint container is initialized
+ *
+ * @return Returns true if initialized
+ */
+ bool isInitialized();
+
+ /**
+ * @brief Print out sparsity patterns, jacobians, etc. Serves for quick visual inspection
+ */
+ void printout();
+
+protected:
+ bool initializedIntermediate_;
+ bool initializedTerminal_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/constraint-impl.h b/ct_optcon/include/ct/optcon/constraint/constraint-impl.h
new file mode 100644
index 0000000..6cd775d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/constraint-impl.h
@@ -0,0 +1,18 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "term/ConstraintBase-impl.h"
+#include "term/ControlInputConstraint-impl.h"
+#include "term/ObstacleConstraint-impl.h"
+#include "term/StateConstraint-impl.h"
+#include "term/TerminalConstraint-impl.h"
+
+#include "ConstraintContainerBase-impl.h"
+#include "LinearConstraintContainer-impl.h"
+#include "ConstraintContainerAnalytical-impl.h"
+#include "ConstraintContainerAD-impl.h"
diff --git a/ct_optcon/include/ct/optcon/constraint/constraint.h b/ct_optcon/include/ct/optcon/constraint/constraint.h
new file mode 100644
index 0000000..f354d14
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/constraint.h
@@ -0,0 +1,19 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "term/ConstraintBase.h"
+#include "term/BoxConstraintBase.h"
+#include "term/ControlInputConstraint.h"
+#include "term/ObstacleConstraint.h"
+#include "term/StateConstraint.h"
+#include "term/TerminalConstraint.h"
+
+#include "ConstraintContainerBase.h"
+#include "LinearConstraintContainer.h"
+#include "ConstraintContainerAD.h"
+#include "ConstraintContainerAnalytical.h"
diff --git a/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h
new file mode 100644
index 0000000..c143490
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h
@@ -0,0 +1,135 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::BoxConstraintBase(const decision_vector_t& lb,
+ const decision_vector_t& ub)
+{
+ // check if box constraints are meaningful
+ sanityCheck(DERIVED_DIM, lb, ub);
+
+ // this box constraint is 'dense' (one dense jacobian diagonal, one zero jacobian)
+ constrSize_ = DERIVED_DIM;
+ Base::lb_.resize(DERIVED_DIM);
+ Base::ub_.resize(DERIVED_DIM);
+ sparsity_ = Eigen::Matrix<int, DERIVED_DIM, 1>::Ones();
+ sparsity_J_.resize(DERIVED_DIM, DERIVED_DIM);
+ sparsity_J_.setIdentity();
+ Base::lb_ = lb.template cast<SCALAR>();
+ Base::ub_ = ub.template cast<SCALAR>();
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::BoxConstraintBase(const VectorXs& lb,
+ const VectorXs& ub,
+ const Eigen::VectorXi& sparsity_vec)
+{
+ // make sure the provided sparsity pattern is correct and consists only of ones and zeros
+ assert(sparsity_vec.maxCoeff() <= 1);
+ assert(sparsity_vec.minCoeff() >= 0);
+
+ constrSize_ = (size_t)sparsity_vec.sum();
+
+ // make sure the provided bounds are consistent
+ sanityCheck(constrSize_, lb, ub);
+
+ Base::lb_.resize(constrSize_);
+ Base::ub_.resize(constrSize_);
+ sparsity_ = sparsity_vec;
+ sparsity_J_.resize(constrSize_, DERIVED_DIM);
+ sparsity_J_ = diagSparsityVecToSparsityMat(sparsity_vec, constrSize_);
+ Base::lb_ = lb.template cast<SCALAR>();
+ Base::ub_ = ub.template cast<SCALAR>();
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::BoxConstraintBase(const BoxConstraintBase& arg)
+ : Base(arg), sparsity_(arg.sparsity_), sparsity_J_(arg.sparsity_J_), constrSize_(arg.constrSize_)
+{
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::~BoxConstraintBase()
+{
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintSize() const
+{
+ return constrSize_;
+}
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::sparsity_matrix_t
+BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::diagSparsityVecToSparsityMat(const VectorXi& spVec,
+ const size_t& nConstr)
+{
+ // set up sparsity matrix all zero
+ sparsity_matrix_t mat(nConstr, DERIVED_DIM);
+ mat.setZero();
+
+ // set selected elements to one
+ size_t count = 0;
+ assert(spVec.rows() == DERIVED_DIM);
+ for (size_t i = 0; i < DERIVED_DIM; i++)
+ {
+ if (spVec(i) == 1)
+ {
+ mat(count, i) = 1;
+ count++;
+ }
+ }
+ return mat;
+}
+
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::sanityCheck(const size_t& nCon,
+ const VectorXs& lb,
+ const VectorXs& ub) const
+{
+ // assert that the size of constraint vectors is equal to the computed/given number of constraints
+ if (lb.rows() != nCon | ub.rows() != nCon)
+ {
+ std::cout << "no. Constraints: " << nCon << std::endl;
+ std::cout << "BoxConstraintBase: lb " << lb.transpose() << std::endl;
+ std::cout << "BoxConstraintBase: ub " << ub.transpose() << std::endl;
+ throw std::runtime_error("BoxConstraintBase: wrong constraint sizes in StateConstraint");
+ }
+
+ // assert that the boundaries are meaningful
+ for (size_t i = 0; i < nCon; i++)
+ {
+ if (lb(i) > ub(i))
+ {
+ std::cout << "BoxConstraintBase: lb " << lb.transpose() << std::endl;
+ std::cout << "BoxConstraintBase: ub " << ub.transpose() << std::endl;
+ throw std::runtime_error("BoxConstraintBase: wrong boundaries: lb > ub");
+ }
+ }
+}
+
+
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternSparseJacobian(
+ const VectorXi& sparsity_vec,
+ const size_t& constrSize,
+ VectorXi& rows,
+ VectorXi& cols)
+{
+ Base::genSparseDiagonalIndices(sparsity_vec, rows, cols);
+ for (size_t i = 0; i < constrSize; i++)
+ rows(i) = i;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase.h b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase.h
new file mode 100644
index 0000000..ea38774
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase.h
@@ -0,0 +1,125 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @brief Base for box constraint, templated on dimension of the decision vector of the derived class
+ *
+ * @tparam DERIVED_DIM Dimension of the decision vector of the derived class
+ * @tparam STATE_DIM The state dimension
+ * @tparam INPUT_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t DERIVED_DIM, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class BoxConstraintBase : public ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using Trait = typename ct::core::tpl::TraitSelector<SCALAR>::Trait;
+ using Base = ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+ using decision_vector_t = core::StateVector<DERIVED_DIM, SCALAR>;
+
+ using VectorXi = Eigen::Matrix<int, Eigen::Dynamic, 1>;
+ using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;
+ using MatrixXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic>;
+
+ using sparsity_matrix_t = Eigen::Matrix<SCALAR, Eigen::Dynamic, DERIVED_DIM>;
+
+ //! generate sparsity pattern for sparse box constraint
+ static void sparsityPatternSparseJacobian(const VectorXi& sparsity_vec,
+ const size_t& constrSize,
+ VectorXi& rows,
+ VectorXi& cols);
+
+ /**
+ * @brief Constructor taking lower and upper state bounds directly. Assumes the box constraint is dense.
+ *
+ * @param[in] vLow The full lower bound
+ * @param[in] vHigh The full upper bound
+ */
+ BoxConstraintBase(const decision_vector_t& vLow, const decision_vector_t& vHigh);
+
+ /**
+ * @brief Constructor for sparse box constraint. Takes bounds and sparsity pattern.
+ * @param lb Lower boundary values
+ * @param ub Upper boundary values
+ * @param sparsity_vec Box constraint sparsity pattern as a vector
+ */
+ BoxConstraintBase(const VectorXs& lb, const VectorXs& ub, const Eigen::VectorXi& sparsity_vec);
+
+ BoxConstraintBase(const BoxConstraintBase& arg);
+
+ virtual ~BoxConstraintBase();
+
+ virtual BoxConstraintBase<DERIVED_DIM, STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override = 0;
+
+ virtual size_t getConstraintSize() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override = 0;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override = 0;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override = 0;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override = 0;
+
+ virtual size_t getNumNonZerosJacobianState() const override = 0;
+
+ virtual size_t getNumNonZerosJacobianInput() const override = 0;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t) override = 0;
+
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t) override = 0;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override = 0;
+
+ virtual void sparsityPatternInput(VectorXi& rows, VectorXi& cols) override = 0;
+
+protected:
+ /*!
+ * @brief transform a sparsity vector (giving the sparsity pattern on the diagonal) in to a sparsity matrix
+ * @param spVec diagonal sparsity pattern, e.g. [0 0 1 0 1 0]
+ * @param nConstr number of constraints
+ * @return the sparsity matrix
+ */
+ sparsity_matrix_t diagSparsityVecToSparsityMat(const VectorXi& spVec, const size_t& nConstr);
+
+ //! sparsity in vector form
+ VectorXi sparsity_;
+
+ //! sparsity matrix
+ sparsity_matrix_t sparsity_J_;
+
+ //! size of the constraint
+ size_t constrSize_;
+
+private:
+ void sanityCheck(const size_t& nCon, const VectorXs& lb, const VectorXs& ub) const;
+};
+
+} // namespace optcon
+} // namepace ct
+
+/*
+ * For this class, we can include the implementation here, as it is virtual,
+ * and only used by few derived members who all get compiled in prespec themselves.
+ */
+#include "BoxConstraintBase-impl.h"
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase-impl.h b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase-impl.h
new file mode 100644
index 0000000..4226bbd
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase-impl.h
@@ -0,0 +1,206 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintBase(std::string name) : name_(name)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintBase(const ConstraintBase& arg)
+ : lb_(arg.lb_), ub_(arg.ub_), name_(arg.name_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::~ConstraintBase()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ throw std::runtime_error("Term " + name_ + " has no Implementation of evaluateCppaCg.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ throw std::runtime_error(
+ "This constraint function element is not implemented for the given term."
+ "Please use either auto-diff cost function or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ throw std::runtime_error(
+ "This constraint function element is not implemented for the given term."
+ "Please use either auto-diff cost function or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getLowerBound() const
+{
+ return lb_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getUpperBound() const
+{
+ return ub_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getName(std::string& constraintName) const
+{
+ constraintName = name_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::setName(const std::string constraintName)
+{
+ name_ = constraintName;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return STATE_DIM * getConstraintSize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return CONTROL_DIM * getConstraintSize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jacState = jacobianState(x, u, t);
+
+ VectorXs jac(Eigen::Map<VectorXs>(jacState.data(), jacState.rows() * jacState.cols()));
+
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jacInput = jacobianInput(x, u, t);
+
+ VectorXs jac(Eigen::Map<VectorXs>(jacInput.data(), jacInput.rows() * jacInput.cols()));
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(Eigen::VectorXi& rows, Eigen::VectorXi& cols)
+{
+ genBlockIndices(getConstraintSize(), STATE_DIM, rows, cols);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInput(Eigen::VectorXi& rows, Eigen::VectorXi& cols)
+{
+ genBlockIndices(getConstraintSize(), CONTROL_DIM, rows, cols);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::genDiagonalIndices(const size_t num_elements,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec)
+{
+ iRow_vec.resize(num_elements);
+ jCol_vec.resize(num_elements);
+
+ size_t count = 0;
+
+ for (size_t i = 0; i < num_elements; ++i)
+ {
+ iRow_vec(count) = i;
+ jCol_vec(count) = i;
+ count++;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::genSparseDiagonalIndices(const Eigen::VectorXi& diag_sparsity,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec)
+{
+ // make sure the sparsity pattern is correct and consists only of ones and zeros
+ assert(diag_sparsity.maxCoeff() <= 1);
+ assert(diag_sparsity.minCoeff() >= 0);
+
+ const int num_elements = diag_sparsity.sum();
+
+ iRow_vec.resize(num_elements);
+ jCol_vec.resize(num_elements);
+
+ size_t count = 0;
+
+ for (int i = 0; i < diag_sparsity.rows(); ++i)
+ {
+ if (diag_sparsity(i) == 1)
+ {
+ iRow_vec(count) = i;
+ jCol_vec(count) = i;
+ count++;
+ }
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>::genBlockIndices(const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec)
+{
+ size_t num_gen_indices = num_rows * num_cols;
+
+ iRow_vec.resize(num_gen_indices);
+ jCol_vec.resize(num_gen_indices);
+
+ size_t count = 0;
+
+ for (size_t row = 0; row < num_rows; ++row)
+ {
+ for (size_t col = 0; col < num_cols; ++col)
+ {
+ iRow_vec(count) = row;
+ jCol_vec(count) = col;
+ count++;
+ }
+ }
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase.h b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase.h
new file mode 100644
index 0000000..6dff79b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ConstraintBase.h
@@ -0,0 +1,243 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Base class for the constraints used in this toolbox
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] name The name of the constraint
+ */
+ ConstraintBase(std::string name = "Unnamed");
+
+ /**
+ * @brief Copy constructor
+ *
+ * @param[in] arg The object to be copied
+ */
+ ConstraintBase(const ConstraintBase& arg);
+
+ /**
+ * @brief Creates a new instance of the object with same properties than original.
+ *
+ * @return Copy of this object.
+ */
+ virtual ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintBase();
+
+ /**
+ * @brief The evaluation of the constraint violation. Note this method
+ * is SCALAR typed
+ *
+ * @param[in] x The state vector
+ * @param[in] u The control vector
+ * @param[in] t The time
+ *
+ * @return The constraint violation
+ */
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) = 0;
+
+ /**
+ * @brief The evaluate method used for jit compilation in constraint
+ * container ad
+ *
+ * @param[in] x The state vector
+ * @param[in] u The control vector
+ * @param[in] t The time
+ *
+ * @return The constraint violation
+ */
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t);
+
+
+ /**
+ * @brief Returns the number of constraints
+ *
+ * @return The number of constraints
+ */
+ virtual size_t getConstraintSize() const = 0;
+
+
+ /**
+ * @brief Returns the constraint jacobian wrt state
+ *
+ * @return The constraint jacobian
+ */
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+ /**
+ * @brief Returns the constraint jacobian wrt input
+ *
+ * @return The constraint jacobian
+ */
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+ /**
+ * @brief Returns the lower constraint bound
+ *
+ * @return The lower constraint bound
+ */
+ virtual VectorXs getLowerBound() const;
+
+ /**
+ * @brief Returns the upper constraint bound
+ *
+ * @return The upper constraint bound
+ */
+ virtual VectorXs getUpperBound() const;
+
+ /**
+ * @brief Returns the constraint name
+ *
+ * @param[out] constraintName The constraint name
+ */
+ void getName(std::string& constraintName) const;
+
+ /**
+ * @brief Sets the constraint name.
+ *
+ * @param[in] constraintName The constraint name
+ */
+ void setName(const std::string constraintName);
+
+ /**
+ * @brief Returns the number of nonzeros in the jacobian wrt state. The
+ * default implementation assumes a dense matrix with only
+ * nonzero elements.
+ *
+ * @return The number of non zeros
+ */
+ virtual size_t getNumNonZerosJacobianState() const;
+
+ /**
+ * @brief Returns the number of nonzeros in the jacobian wrt control
+ * input. The default implementation assumes a dense matrix with
+ * only nonzero elements
+ *
+ * @return The number of non zeros
+ */
+ virtual size_t getNumNonZerosJacobianInput() const;
+
+ /**
+ * @brief Returns the constraint jacobian wrt state in sparse
+ * structure. The default implementation maps the JacobianState
+ * matrix to a vector
+ *
+ * @return The sparse constraint jacobian
+ */
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+ /**
+ * @brief Returns the constraint jacobian wrt control input in sparse
+ * structure. The default implementation maps the JacobianState
+ * matrix to a vector
+ *
+ * @return The sparse constraint jacobian
+ */
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t);
+
+
+ /**
+ * @brief Generates the sparsity pattern of the jacobian wrt state. The
+ * default implementation returns a vector of ones corresponding
+ * to the dense jacobianState
+ *
+ * @param rows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param cols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ */
+ virtual void sparsityPatternState(Eigen::VectorXi& rows, Eigen::VectorXi& cols);
+
+ /**
+ * @brief Generates the sparsity pattern of the jacobian wrt control
+ * input. The default implementation returns a vector of ones
+ * corresponding to the dense jacobianInput
+ *
+ * @param rows The vector of the row indices containing non zero
+ * elements in the constraint jacobian
+ * @param cols The vector of the column indices containing non zero
+ * elements in the constraint jacobian
+ */
+ virtual void sparsityPatternInput(Eigen::VectorXi& rows, Eigen::VectorXi& cols);
+
+
+protected:
+ VectorXs lb_; //! lower bound on the constraints
+ VectorXs ub_; //! upper bound on the constraints
+
+ /**
+ * @brief Generates indices of a diagonal square matrix
+ *
+ * @param[in] num_elements The number of elements
+ * @param[out] iRow_vec The row vector
+ * @param[out] jCol_vec The column vector
+ */
+ static void genDiagonalIndices(const size_t num_elements, Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec);
+
+ /**
+ * @brief Generates indices of a sparse diagonal square matrix
+ *
+ * @param[in] diag_sparsity Sparsity pattern for the diagonal (Example: [0 1 0 0 1 1])
+ * @param[out] iRow_vec The row vector
+ * @param[out] jCol_vec The column vector
+ */
+ static void genSparseDiagonalIndices(const Eigen::VectorXi& diag_sparsity,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec);
+
+ /**
+ * @brief Generates indices of a full matrix
+ *
+ * @param[in] num_rows The number of rows of the matrix
+ * @param[in] num_cols The number columns of the matrix
+ * @param[out] iRow_vec The row vector
+ * @param[out] jCol_vec The col vector
+ */
+ static void genBlockIndices(const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec);
+
+private:
+ std::string name_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint-impl.h
new file mode 100644
index 0000000..4fb8dd1
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint-impl.h
@@ -0,0 +1,130 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ControlInputConstraint(const control_vector_t& lb,
+ const control_vector_t& ub)
+ : Base(lb, ub)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ControlInputConstraint(const VectorXs& lb,
+ const VectorXs& ub,
+ const Eigen::VectorXi& control_sparsity)
+ : Base(lb, ub, control_sparsity)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ControlInputConstraint(const ControlInputConstraint& arg)
+ : Base(arg)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~ControlInputConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone()
+ const
+{
+ return new ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(
+ const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_ * u;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1>
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return this->sparsity_J_.template cast<ct::core::ADCGScalar>() * u;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jac(this->constrSize_, STATE_DIM);
+ jac.setZero();
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return 0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return this->constrSize_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return VectorXs();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(VectorXi& rows, VectorXi& cols)
+{
+ // do nothing
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ VectorXs jac(this->constrSize_);
+ jac.setConstant(1.0);
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInput(VectorXi& rows, VectorXi& cols)
+{
+ this->sparsityPatternSparseJacobian(this->sparsity_, this->constrSize_, rows, cols);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint.h
new file mode 100644
index 0000000..07cda6d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint.h
@@ -0,0 +1,86 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Class for control input box constraint term
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ControlInputConstraint : public BoxConstraintBase<CONTROL_DIM, STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using Trait = typename ct::core::tpl::TraitSelector<SCALAR>::Trait;
+ using Base = BoxConstraintBase<CONTROL_DIM, STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+
+ using VectorXi = Eigen::Matrix<int, Eigen::Dynamic, 1>;
+ using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;
+ using MatrixXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic>;
+
+ using sparsity_matrix_t = Eigen::Matrix<SCALAR, Eigen::Dynamic, CONTROL_DIM>;
+
+ /**
+ * @brief Constructor taking lower and upper state bounds directly. Assumes state box constraint is dense.
+ *
+ * @param[in] uLow The upper control input bound
+ * @param[in] uHigh The lower control input bound
+ */
+ ControlInputConstraint(const control_vector_t& uLow, const control_vector_t& uHigh);
+
+ /**
+ * @brief Constructor for sparse control input box constraint. Takes bounds and sparsity pattern.
+ * @param lb Lower boundary values
+ * @param ub Upper boundary values
+ * @param control_sparsity Control input constraint sparsity pattern
+ */
+ ControlInputConstraint(const VectorXs& lb, const VectorXs& ub, const Eigen::VectorXi& control_sparsity);
+
+ ControlInputConstraint(const ControlInputConstraint& arg);
+
+ virtual ~ControlInputConstraint();
+
+ virtual ControlInputConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual size_t getNumNonZerosJacobianState() const override;
+
+ virtual size_t getNumNonZerosJacobianInput() const override;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override;
+
+ virtual void sparsityPatternInput(VectorXi& rows, VectorXi& cols) override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint-impl.h
new file mode 100644
index 0000000..b07247e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint-impl.h
@@ -0,0 +1,91 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef CT_OPTCON_CONSTRAINT_TERM_CONSTRAINT_OBSTACLE_IMPL_HPP_
+#define CT_OPTCON_CONSTRAINT_TERM_CONSTRAINT_OBSTACLE_IMPL_HPP_
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ObstacleConstraint(
+ std::shared_ptr<ct::core::tpl::Ellipsoid<SCALAR>> obstacle,
+ std::function<void(const state_vector_t&, Vector3s&)> getPosition,
+ std::function<void(const state_vector_t&, Eigen::Matrix<SCALAR, 3, STATE_DIM>&)> getJacobian)
+ : obstacle_(obstacle), xFun_(getPosition), dXFun_(getJacobian)
+{
+ this->lb_.resize(1);
+ this->ub_.resize(1);
+ this->lb_(0) = SCALAR(0.0);
+ this->ub_(0) = std::numeric_limits<SCALAR>::max();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~ObstacleConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::ObstacleConstraint(const ObstacleConstraint& arg)
+ : Base(arg), obstacle_(arg.obstacle_), xFun_(arg.xFun_), dXFun_(arg.dXFun_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintSize() const
+{
+ return 1;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(
+ const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ Vector3s xRef;
+ xFun_(x, xRef);
+ val_(0) = obstacle_->insideEllipsoid(xRef);
+ return val_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ Vector3s xRef;
+ Eigen::Matrix<SCALAR, 3, STATE_DIM> dXRef;
+ xFun_(x, xRef);
+ dXFun_(x, dXRef);
+ VectorXs dist = xRef - obstacle_->x0();
+ jac_ = 2 * dist.transpose() * obstacle_->S() * obstacle_->A().transpose() * obstacle_->A() *
+ obstacle_->S().transpose() * dXRef;
+ return jac_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return Eigen::Matrix<SCALAR, 1, CONTROL_DIM>::Zero();
+}
+
+
+} // namespace optcon
+} // namespace core
+
+#endif //CT_OPTCON_CONSTRAINT_TERM_CONSTRAINT_OBSTACLE_IMPL_HPP_
diff --git a/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint.h
new file mode 100644
index 0000000..ce4773f
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint.h
@@ -0,0 +1,67 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @brief Class for obstacle constraint.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam INPUT_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ObstacleConstraint : public ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ typedef Eigen::Matrix<SCALAR, 3, 1> Vector3s;
+
+ ObstacleConstraint(std::shared_ptr<ct::core::tpl::Ellipsoid<SCALAR>> obstacle,
+ std::function<void(const state_vector_t&, Vector3s&)> getPosition,
+ std::function<void(const state_vector_t&, Eigen::Matrix<SCALAR, 3, STATE_DIM>&)> getJacobian);
+
+ virtual ObstacleConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual ~ObstacleConstraint();
+
+ ObstacleConstraint(const ObstacleConstraint& arg);
+
+ virtual size_t getConstraintSize() const override;
+
+ virtual Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> evaluate(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+private:
+ std::shared_ptr<ct::core::tpl::Ellipsoid<SCALAR>> obstacle_;
+
+ std::function<void(const core::StateVector<STATE_DIM, SCALAR>&, Vector3s&)> xFun_;
+ std::function<void(const core::StateVector<STATE_DIM, SCALAR>&, Eigen::Matrix<SCALAR, 3, STATE_DIM>&)> dXFun_;
+
+ core::StateVector<1, SCALAR> val_;
+ Eigen::Matrix<SCALAR, 1, STATE_DIM> jac_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/StateConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint-impl.h
new file mode 100644
index 0000000..6f9a6e4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint-impl.h
@@ -0,0 +1,126 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::StateConstraint(const state_vector_t& xLow,
+ const state_vector_t& xHigh)
+ : Base(xLow, xHigh)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::StateConstraint(const VectorXs& lb,
+ const VectorXs& ub,
+ const Eigen::VectorXi& state_sparsity)
+ : Base(lb, ub, state_sparsity)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::StateConstraint(const StateConstraint& arg) : Base(arg)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~StateConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_ * x;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return this->sparsity_J_.template cast<ct::core::ADCGScalar>() * x;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return this->sparsity_J_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ MatrixXs jac(this->constrSize_, CONTROL_DIM);
+ jac.setZero();
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return this->constrSize_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return 0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ VectorXs jac(this->constrSize_);
+ jac.setConstant(1.0);
+ return jac;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(VectorXi& rows, VectorXi& cols)
+{
+ this->sparsityPatternSparseJacobian(this->sparsity_, this->constrSize_, rows, cols);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInputSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return VectorXs();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternInput(VectorXi& rows, VectorXi& cols)
+{
+ //do nothing
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/StateConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint.h
new file mode 100644
index 0000000..fff7aff
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/StateConstraint.h
@@ -0,0 +1,87 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Class for state box constraint.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam INPUT_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class StateConstraint : public BoxConstraintBase<STATE_DIM, STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using Trait = typename ct::core::tpl::TraitSelector<SCALAR>::Trait;
+ using Base = BoxConstraintBase<STATE_DIM, STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+
+ using VectorXi = Eigen::Matrix<int, Eigen::Dynamic, 1>;
+ using VectorXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>;
+ using MatrixXs = Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic>;
+
+ using sparsity_matrix_t = Eigen::Matrix<SCALAR, Eigen::Dynamic, STATE_DIM>;
+
+ /**
+ * @brief Constructor taking lower and upper state bounds directly. Assumes state box constraint is dense.
+ *
+ * @param[in] xLow The lower state bound
+ * @param[in] xHigh The upper state bound
+ */
+ StateConstraint(const state_vector_t& xLow, const state_vector_t& xHigh);
+
+ /**
+ * @brief Constructor for sparse state box constraint. Takes bounds and sparsity pattern.
+ * @param lb Lower boundary values
+ * @param ub Upper boundary values
+ * @param state_sparsity State constraint sparsity pattern
+ */
+ StateConstraint(const VectorXs& lb, const VectorXs& ub, const Eigen::VectorXi& state_sparsity);
+
+ StateConstraint(const StateConstraint& arg);
+
+ virtual ~StateConstraint();
+
+ virtual StateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual size_t getNumNonZerosJacobianState() const override;
+
+ virtual size_t getNumNonZerosJacobianInput() const override;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual VectorXs jacobianInputSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override;
+
+ virtual void sparsityPatternInput(VectorXi& rows, VectorXi& cols) override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint-impl.h b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint-impl.h
new file mode 100644
index 0000000..2f79335
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint-impl.h
@@ -0,0 +1,110 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::TerminalConstraint(const core::StateVector<STATE_DIM, SCALAR> xf)
+{
+ xF_ = xf;
+ Base::lb_.resize(STATE_DIM);
+ Base::ub_.resize(STATE_DIM);
+ // The terminal state constraint is treated as equality constraint, therefore, ub = lb
+ Base::lb_.setConstant(SCALAR(0.0));
+ Base::ub_.setConstant(SCALAR(0.0));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::TerminalConstraint(const TerminalConstraint& arg)
+ : Base(arg), xF_(arg.xF_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::~TerminalConstraint()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getConstraintSize() const
+{
+ return STATE_DIM;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluate(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return x - xF_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1>
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return x - xF_.template cast<ct::core::ADCGScalar>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianState(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM>::Identity();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianInput(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM>::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianState() const
+{
+ return STATE_DIM;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::getNumNonZerosJacobianInput() const
+{
+ return 0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::VectorXs
+TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::jacobianStateSparse(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR t)
+{
+ return core::StateVector<STATE_DIM, SCALAR>::Ones();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::sparsityPatternState(VectorXi& rows, VectorXi& cols)
+{
+ this->genDiagonalIndices(STATE_DIM, rows, cols);
+}
+}
+}
diff --git a/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint.h b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint.h
new file mode 100644
index 0000000..4343ed7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint.h
@@ -0,0 +1,74 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup Constraint
+ *
+ * @brief Class for terminal constraint.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The Scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class TerminalConstraint : public ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] xf The desired terminal state
+ */
+ TerminalConstraint(const core::StateVector<STATE_DIM, SCALAR> xf);
+
+ virtual TerminalConstraint<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ TerminalConstraint(const TerminalConstraint& arg);
+
+ virtual ~TerminalConstraint();
+
+ virtual size_t getConstraintSize() const override;
+
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual size_t getNumNonZerosJacobianState() const;
+
+ virtual size_t getNumNonZerosJacobianInput() const;
+
+ virtual VectorXs jacobianStateSparse(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override;
+
+ virtual void sparsityPatternState(VectorXi& rows, VectorXi& cols) override;
+
+private:
+ core::StateVector<STATE_DIM, SCALAR> xF_;
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
new file mode 100644
index 0000000..e56067e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp
@@ -0,0 +1,55 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunction() : t_(0.0), t_shift_(0.0)
+{
+ x_.setZero();
+ u_.setZero();
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunction(){};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunction(const CostFunction& arg)
+ : x_(arg.x_), u_(arg.u_), t_(arg.t_), t_shift_(arg.t_shift_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t)
+{
+ x_ = x;
+ u_ = u;
+ t_ = t + t_shift_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::getCurrentStateAndControl(Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ SCALAR& t) const
+{
+ x = this->x_;
+ u = this->u_;
+ t = this->t_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>::shiftTime(const SCALAR t)
+{
+ t_shift_ = t;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunction.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunction.hpp
new file mode 100644
index 0000000..99b9f08
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunction.hpp
@@ -0,0 +1,112 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <vector>
+
+namespace ct {
+namespace optcon {
+
+/** \defgroup CostFunction CostFunction
+ *
+ * \brief Cost Function Module used in Optimal Control
+ */
+
+/**
+ * \ingroup CostFunction
+ *+
+ * \brief A base function for cost functions. All cost functions should derive from this.
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunction
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ /**
+ * \brief Default constructor
+ *
+ * Default constructor, sets state, control and time to zero
+ */
+ CostFunction();
+
+ /**
+ * \brief Destructor
+ *
+ * Destructor
+ */
+ virtual ~CostFunction();
+
+ /**
+ * \brief Copy constructor
+ * @param arg other cost function
+ */
+ CostFunction(const CostFunction& arg);
+
+ /**
+ * Clones the cost function.
+ * @return Base pointer to the clone
+ */
+ virtual CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;
+
+ /**
+ * Set the current state, control and time of the cost function. In this function, the user can add pre-computations
+ * shared between different calls to the derivative functions.
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ virtual void setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t = SCALAR(0.0));
+
+ /**
+ * \brief sets current state, control and time
+ *
+ * Get the current state, control and time of the cost function. In this function, the user can add pre-computations
+ * shared between different calls to the derivative functions.
+ * @param x state vector
+ * @param u control vector
+ * @param t time
+ */
+ virtual void getCurrentStateAndControl(Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ SCALAR& t) const;
+
+ /**
+ * \brief evaluate intermediate costs
+ *
+ * Evaluates the running/intermediate cost function for the control, state and time set in setCurrentStateAndControl()
+ * @return costs
+ */
+ virtual SCALAR evaluateIntermediate() = 0;
+
+ /**
+ * \brief evaluate terminal costs
+ *
+ * Evaluates the terminal cost for a given state and control set in setCurrentStateAndControl(). This usually ignores time.
+ * @return costs
+ */
+ virtual SCALAR evaluateTerminal() = 0;
+
+ virtual void shiftTime(const SCALAR t);
+
+
+protected:
+ state_vector_t x_; /** state vector */
+ control_vector_t u_; /** control vector */
+ SCALAR t_; /** time */
+
+ SCALAR t_shift_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
new file mode 100644
index 0000000..810985b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp
@@ -0,0 +1,362 @@
+/**********************************************************************************************************************
+ This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+ Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+ Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAD()
+ : CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>(),
+ stateControlTime_(Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM + 1, 1>::Zero())
+{
+ intermediateFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateIntermediateCg(stateInputTime);
+ };
+
+ finalFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateTerminalCg(stateInputTime);
+ };
+
+ intermediateCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(intermediateFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+ finalCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(finalFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+
+ setCurrentStateAndControl(this->x_, this->u_, this->t_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAD(const CostFunctionAD& arg)
+ : CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ stateControlTime_(arg.stateControlTime_),
+ intermediateFun_(arg.intermediateFun_),
+ finalFun_(arg.finalFun_)
+{
+ intermediateTerms_.resize(arg.intermediateTerms_.size());
+ finalTerms_.resize(arg.finalTerms_.size());
+
+ for (size_t i = 0; i < intermediateTerms_.size(); ++i)
+ intermediateTerms_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>(arg.intermediateTerms_[i]->clone());
+
+ for (size_t i = 0; i < finalTerms_.size(); ++i)
+ finalTerms_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>(arg.finalTerms_[i]->clone());
+
+ intermediateCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(intermediateFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+ finalCostCodegen_ = std::shared_ptr<JacCG>(new JacCG(finalFun_, STATE_DIM + CONTROL_DIM + 1, 1));
+
+ initialize(); //when cloning, we directly call initialize()
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAD(const std::string& filename, bool verbose)
+ : CostFunctionAD() //! @warning the delegating constructor in the initializer list is required to call the initial routine in CostFunctionAD()
+{
+ loadFromConfigFile(filename, verbose);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionAD(){};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>* CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new CostFunctionAD(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::initialize()
+{
+ intermediateFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateIntermediateCg(stateInputTime);
+ };
+
+ finalFun_ = [&](const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime) {
+ return this->evaluateTerminalCg(stateInputTime);
+ };
+
+ intermediateCostCodegen_->update(intermediateFun_, STATE_DIM + CONTROL_DIM + 1, 1);
+ finalCostCodegen_->update(finalFun_, STATE_DIM + CONTROL_DIM + 1, 1);
+
+ //! @ todo: this should probably become an option (eg. IPOPT can work without cost Hessians)
+ ct::core::DerivativesCppadSettings settings;
+ settings.createForwardZero_ = true;
+ settings.createJacobian_ = true;
+ settings.createHessian_ = true;
+
+ finalCostCodegen_->compileJIT(settings, "finalCosts");
+ intermediateCostCodegen_->compileJIT(settings, "intermediateCosts");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose)
+{
+ intermediateTerms_.push_back(term);
+
+ if (verbose)
+ {
+ std::cout << term->getName() + " added as intermediate AD term" << std::endl;
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::addFinalADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose)
+{
+ finalTerms_.push_back(term);
+
+ if (verbose)
+ {
+ std::cout << term->getName() + " added as final AD term" << std::endl;
+ }
+}
+
+// set state and control
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t)
+{
+ this->x_ = x;
+ this->u_ = u;
+ this->t_ = t;
+
+ stateControlTime_ << x, u, t;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::loadFromConfigFile(const std::string& filename, bool verbose)
+{
+ this->intermediateCostAnalytical_.clear();
+ this->finalCostAnalytical_.clear();
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+ int i = 0;
+ std::string currentTerm;
+ do
+ {
+ currentTerm = "term" + std::to_string(i);
+ std::string termKind = pt.get<std::string>(currentTerm + ".kind");
+ boost::algorithm::to_lower(termKind);
+ int currentTermType = pt.get<int>(currentTerm + ".type");
+ std::string termName;
+ try
+ {
+ termName = pt.get<std::string>(currentTerm + ".name");
+ } catch (boost::property_tree::ptree_bad_path err)
+ {
+ termName = "Unnamed";
+ if (verbose)
+ {
+ std::cout << "Name field for " + currentTerm + " does not exist" << std::endl;
+ }
+ }
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term;
+
+ CT_LOADABLE_TERMS(SCALAR, CGScalar);
+
+ if (!term)
+ {
+ throw std::runtime_error("Term type \"" + termKind + "\" not supported");
+ }
+ else
+ {
+ if (term)
+ addADTerm(filename, currentTerm, currentTermType, term, this, verbose);
+ else
+ throw std::runtime_error("Term type \"" + termKind + "\" loaded but unsupported.");
+ }
+ currentTerm = "term" + std::to_string(++i);
+ } while (pt.find(currentTerm) != pt.not_found());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixCg
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediateCg(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime)
+{
+ CGScalar y = CGScalar(0.0);
+
+ for (auto it : intermediateTerms_)
+ y += it->evaluateCppadCg(stateInputTime.segment(0, STATE_DIM), stateInputTime.segment(STATE_DIM, CONTROL_DIM),
+ stateInputTime(STATE_DIM + CONTROL_DIM));
+
+ Eigen::Matrix<CGScalar, 1, 1> out;
+ out << y;
+ return out;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::MatrixCg
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminalCg(
+ const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime)
+{
+ CGScalar y = CGScalar(0.0);
+
+ for (auto it : finalTerms_)
+ y += it->evaluateCppadCg(stateInputTime.segment(0, STATE_DIM), stateInputTime.segment(STATE_DIM, CONTROL_DIM),
+ stateInputTime(STATE_DIM + CONTROL_DIM));
+
+ Eigen::Matrix<CGScalar, 1, 1> out;
+ out << y;
+ return out;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ return this->evaluateIntermediateBase() + intermediateCostCodegen_->forwardZero(stateControlTime_)(0);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ return this->evaluateTerminalBase() + finalCostCodegen_->forwardZero(stateControlTime_)(0);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot =
+ intermediateCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template leftCols<STATE_DIM>().transpose() + this->stateDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot = finalCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template leftCols<STATE_DIM>().transpose() + this->stateDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot =
+ intermediateCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template block<1, CONTROL_DIM>(0, STATE_DIM).transpose() + this->controlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, STATE_DIM + CONTROL_DIM + 1> jacTot = finalCostCodegen_->jacobian(stateControlTime_);
+ return jacTot.template block<1, CONTROL_DIM>(0, STATE_DIM).transpose() + this->controlDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = intermediateCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<STATE_DIM, STATE_DIM>(0, 0) + this->stateSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = finalCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<STATE_DIM, STATE_DIM>(0, 0) + this->stateSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = intermediateCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, CONTROL_DIM>(STATE_DIM, STATE_DIM) +
+ this->controlSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = finalCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, CONTROL_DIM>(STATE_DIM, STATE_DIM) +
+ this->controlSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediate()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = intermediateCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, STATE_DIM>(STATE_DIM, 0) + this->stateControlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminal()
+{
+ Eigen::Matrix<SCALAR, 1, 1> w;
+ w << SCALAR(1.0);
+ MatrixXs hesTot = finalCostCodegen_->hessian(stateControlTime_, w);
+ return hesTot.template block<CONTROL_DIM, STATE_DIM>(STATE_DIM, 0) + this->stateControlDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<ct::optcon::
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateADTermById(const size_t id)
+{
+ return intermediateTerms_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<ct::optcon::
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalADTermById(const size_t id)
+{
+ return finalTerms_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateADTermByName(const std::string& name)
+{
+ for (auto term : intermediateTerms_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionAD");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<
+ TermBase<STATE_DIM, CONTROL_DIM, SCALAR, typename CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::CGScalar>>
+CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalADTermByName(const std::string& name)
+{
+ for (auto term : finalTerms_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionAD");
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
new file mode 100644
index 0000000..833e03c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp
@@ -0,0 +1,169 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/core/core.h>
+#include <memory>
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+#include <boost/algorithm/string.hpp>
+
+#include "CostFunctionQuadratic.hpp"
+#include "utility/utilities.hpp"
+
+#include "term/TermLoadMacros.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief Cost Function with Auto-Diff support
+ *
+ * This cost function can work with both, analytical terms as well as
+ * auto-diff terms. For analytical terms it will use provided derivatives
+ * and for auto-diff terms derivatives will be computed using auto-diff.
+ *
+ * Unit test \ref ADTest.cpp illustrates the use of a CostFunctionAD.
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionAD : public CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::DerivativesCppadJIT<STATE_DIM + CONTROL_DIM + 1, 1> JacCG;
+ typedef typename JacCG::CG_SCALAR CGScalar;
+ typedef Eigen::Matrix<CGScalar, 1, 1> MatrixCg;
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * \brief Basic constructor
+ */
+ CostFunctionAD();
+
+ /**
+ * \brief Constructor loading function from file
+ * @param filename config file location
+ * @param verbose flag enabling printouts
+ */
+ CostFunctionAD(const std::string& filename, bool verbose = false);
+
+ /**
+ * Deep-cloning of cost function
+ * @return base pointer to clone
+ */
+ CostFunctionAD<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const;
+
+ /**
+ * \brief Copy constructor
+ * @param arg cost function to copy
+ */
+ CostFunctionAD(const CostFunctionAD& arg);
+
+
+ /**
+ * \brief Destructor
+ */
+ virtual ~CostFunctionAD();
+
+
+ /**
+ * @brief Initializes the AD costfunction, generates and compiles
+ * source code
+ */
+ virtual void initialize() override;
+
+ /**
+ * \brief Add an intermediate, auto-differentiable term
+ *
+ * Use this function to add an auto-differentiable, intermediate term to the cost function.
+ *
+ * @param term The term to be added
+ * @param verbose Flag enabling printouts
+ * @return
+ */
+ void addIntermediateADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose = false) override;
+
+ /**
+ * \brief Add a final, auto-differentiable term
+ *
+ * Use this function to add an auto-differentiable, final term to the cost function.
+ *
+ * @param term The term to be added
+ * @param verbose Flag enabling printouts
+ * @return
+ */
+ void addFinalADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> term,
+ bool verbose = false) override;
+
+ void setCurrentStateAndControl(const state_vector_t& x, const control_vector_t& u, const SCALAR& t = 0.0) override;
+
+ void loadFromConfigFile(const std::string& filename, bool verbose = false) override;
+
+ SCALAR evaluateIntermediate() override;
+ SCALAR evaluateTerminal() override;
+
+ state_vector_t stateDerivativeIntermediate() override;
+ state_vector_t stateDerivativeTerminal() override;
+
+ control_vector_t controlDerivativeIntermediate() override;
+ control_vector_t controlDerivativeTerminal() override;
+
+ state_matrix_t stateSecondDerivativeIntermediate() override;
+ state_matrix_t stateSecondDerivativeTerminal() override;
+
+ control_matrix_t controlSecondDerivativeIntermediate() override;
+ control_matrix_t controlSecondDerivativeTerminal() override;
+
+ control_state_matrix_t stateControlDerivativeIntermediate() override;
+ control_state_matrix_t stateControlDerivativeTerminal() override;
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getIntermediateADTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getFinalADTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getIntermediateADTermByName(
+ const std::string& name);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>> getFinalADTermByName(const std::string& name);
+
+
+private:
+ MatrixCg evaluateIntermediateCg(const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime);
+ MatrixCg evaluateTerminalCg(const Eigen::Matrix<CGScalar, STATE_DIM + CONTROL_DIM + 1, 1>& stateInputTime);
+
+ //! combined state, control and time vector
+ Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM + 1, 1> stateControlTime_;
+
+ //! intermediate AD terms
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>> intermediateTerms_;
+ //! final AD terms
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, CGScalar>>> finalTerms_;
+
+ //! generated jacobians
+ std::shared_ptr<JacCG> intermediateCostCodegen_;
+ std::shared_ptr<JacCG> finalCostCodegen_;
+
+ //! cppad functions
+ typename JacCG::FUN_TYPE_CG intermediateFun_;
+ typename JacCG::FUN_TYPE_CG finalFun_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
new file mode 100644
index 0000000..e0d8a22
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp
@@ -0,0 +1,174 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAnalytical(){};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAnalytical(const CostFunctionAnalytical& arg)
+ : CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>(arg)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionAnalytical(const std::string& filename,
+ bool verbose)
+{
+ loadFromConfigFile(filename, verbose);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>* CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::clone()
+ const
+{
+ return new CostFunctionAnalytical(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionAnalytical(){};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::loadFromConfigFile(const std::string& filename,
+ bool verbose)
+{
+ if (verbose)
+ std::cout << "Starting to load analytical cost function from file " << filename << std::endl;
+
+ this->intermediateCostAnalytical_.clear();
+ this->finalCostAnalytical_.clear();
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+ int i = 0;
+ std::string currentTerm;
+ do
+ {
+ std::cout << "=============================================" << std::endl; //indicating new term
+ currentTerm = "term" + std::to_string(i);
+ std::string termKind = pt.get<std::string>(currentTerm + ".kind");
+ boost::algorithm::to_lower(termKind);
+ int currentTermType = pt.get<int>(currentTerm + ".type");
+ std::string termName;
+ try
+ {
+ termName = pt.get<std::string>(currentTerm + ".name");
+ if (verbose)
+ std::cout << "Trying to add " + termName + " as term" << std::endl;
+ } catch (boost::property_tree::ptree_bad_path err)
+ {
+ termName = "Unnamed";
+ if (verbose)
+ {
+ std::cout << "Name field for " + currentTerm + " does not exist" << std::endl;
+ }
+ }
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term;
+
+ CT_LOADABLE_TERMS(SCALAR, SCALAR);
+
+ if (!term)
+ {
+ throw std::runtime_error("Term type \"" + termKind + "\" not supported");
+ }
+ else
+ {
+ addTerm(filename, currentTerm, currentTermType, term, this, verbose);
+ }
+ currentTerm = "term" + std::to_string(++i);
+ } while (pt.find(currentTerm) != pt.not_found());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ return this->evaluateIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ return this->evaluateTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediate()
+{
+ return this->stateDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminal()
+{
+ return this->stateDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediate()
+{
+ return this->stateSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminal()
+{
+ return this->stateSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediate()
+{
+ return this->controlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminal()
+{
+ return this->controlDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediate()
+{
+ return this->controlSecondDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminal()
+{
+ return this->controlSecondDerivativeTerminalBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediate()
+{
+ return this->stateControlDerivativeIntermediateBase();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminal()
+{
+ return this->stateControlDerivativeTerminalBase();
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
new file mode 100644
index 0000000..2338fcf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp
@@ -0,0 +1,96 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+#include <boost/algorithm/string.hpp>
+
+#include "CostFunctionQuadratic.hpp"
+#include "utility/utilities.hpp"
+
+#include "term/TermLoadMacros.hpp"
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A cost function which contains only terms that have analytical derivatives
+ *
+ * This class provides functions to evaluate a cost function and computes its first
+ * and second order derivatives. This cost function assumes that analytical derivatives
+ * for all terms are available.
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionAnalytical : public CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ /**
+ * \brief Basic constructor
+ */
+ CostFunctionAnalytical();
+
+ /**
+ * \brief Copy constructor
+ * @param arg cost function to copy
+ */
+ CostFunctionAnalytical(const CostFunctionAnalytical& arg);
+ /**
+ * \brief Constructor loading function from file
+ * @param filename config file location
+ * @param verbose flag enabling printouts
+ */
+ CostFunctionAnalytical(const std::string& filename, bool verbose = false);
+
+ /**
+ * Deep-cloning of cost function
+ * @return base pointer to clone
+ */
+ CostFunctionAnalytical<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const;
+
+ /**
+ * Destructor
+ */
+ ~CostFunctionAnalytical();
+
+ SCALAR evaluateIntermediate() override;
+ SCALAR evaluateTerminal() override;
+
+ state_vector_t stateDerivativeIntermediate() override;
+ state_vector_t stateDerivativeTerminal() override;
+
+ state_matrix_t stateSecondDerivativeIntermediate() override;
+ state_matrix_t stateSecondDerivativeTerminal() override;
+
+ control_vector_t controlDerivativeIntermediate() override;
+ control_vector_t controlDerivativeTerminal() override;
+
+ control_matrix_t controlSecondDerivativeIntermediate() override;
+ control_matrix_t controlSecondDerivativeTerminal() override;
+
+ control_state_matrix_t stateControlDerivativeIntermediate() override;
+ control_state_matrix_t stateControlDerivativeTerminal() override;
+
+ void loadFromConfigFile(const std::string& filename, bool verbose = false) override;
+
+private:
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
new file mode 100644
index 0000000..46caf1c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp
@@ -0,0 +1,493 @@
+/**********************************************************************************************************************
+ This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+ Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+ Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadratic()
+{
+ eps_ = sqrt(Eigen::NumTraits<SCALAR>::epsilon());
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadratic(const CostFunctionQuadratic& arg)
+ : CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>(arg),
+ eps_(arg.eps_),
+ doubleSidedDerivative_(arg.doubleSidedDerivative_)
+{
+ intermediateCostAnalytical_.resize(arg.intermediateCostAnalytical_.size());
+ finalCostAnalytical_.resize(arg.finalCostAnalytical_.size());
+
+ for (size_t i = 0; i < arg.intermediateCostAnalytical_.size(); i++)
+ {
+ intermediateCostAnalytical_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.intermediateCostAnalytical_[i]->clone());
+ }
+
+ for (size_t i = 0; i < arg.finalCostAnalytical_.size(); i++)
+ {
+ finalCostAnalytical_[i] =
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>(arg.finalCostAnalytical_[i]->clone());
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionQuadratic()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose)
+{
+ throw std::runtime_error("not implemented");
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addFinalADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose)
+{
+ throw std::runtime_error("not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::loadFromConfigFile(const std::string& filename,
+ bool verbose)
+{
+ throw std::runtime_error("not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminal()
+{
+ throw std::runtime_error("controlDerivativeTerminal() not implemented in CostFunctionQuadratic");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminal()
+{
+ throw std::runtime_error("controlSecondDerivativeTerminal() not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminal()
+{
+ throw std::runtime_error("stateControlDerivativeTerminal() not implemented");
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::updateReferenceState(const state_vector_t& x_ref)
+{
+ for (auto costIntermediate : intermediateCostAnalytical_)
+ costIntermediate->updateReferenceState(x_ref);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::updateFinalState(const state_vector_t& x_final)
+{
+ for (auto costFinal : finalCostAnalytical_)
+ costFinal->updateReferenceState(x_final);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediateTest()
+{
+ state_vector_t derivative = stateDerivativeIntermediate();
+ state_vector_t derivativeNd = stateDerivativeIntermediateNumDiff();
+ std::cout << "norm error between derivative/numdiff state : " << std::endl
+ << (derivative - derivativeNd).norm() << std::endl;
+
+ return (derivative.isApprox(derivativeNd, 1e-6));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+bool CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediateTest()
+{
+ control_vector_t derivative = controlDerivativeIntermediate();
+ control_vector_t derivativeNd = controlDerivativeIntermediateNumDiff();
+ std::cout << "norm error between derivative/numdiff control : " << std::endl
+ << (derivative - derivativeNd).norm() << std::endl;
+
+ return (derivative.isApprox(derivativeNd, 1e-6));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateTermById(const size_t id)
+{
+ return intermediateCostAnalytical_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalTermById(const size_t id)
+{
+ return finalCostAnalytical_[id];
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getIntermediateTermByName(const std::string& name)
+{
+ for (auto term : intermediateCostAnalytical_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionQuadratic");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::getFinalTermByName(const std::string& name)
+{
+ for (auto term : finalCostAnalytical_)
+ if (term->getName() == name)
+ return term;
+
+ throw std::runtime_error("Term " + name + " not found in the CostFunctionQuadratic");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediateNumDiff()
+{
+ state_vector_t dFdx = state_vector_t::Zero();
+ state_vector_t x_local;
+ control_vector_t u_local;
+ SCALAR t_local;
+ this->getCurrentStateAndControl(x_local, u_local, t_local);
+ SCALAR dxdt_ref = this->evaluateIntermediate();
+
+ for (size_t i = 0; i < STATE_DIM; ++i)
+ {
+ // inspired from http://en.wikipedia.org/wiki/Numerical_differentiation#Practical_considerations_using_floating_point_arithmetic
+ SCALAR h = eps_ * std::max(std::abs(x_local(i)), 1.0);
+ volatile SCALAR x_ph = x_local(i) + h;
+ SCALAR dxp = x_ph - x_local(i);
+
+ state_vector_t x_perturbed = x_local;
+ x_perturbed(i) = x_ph;
+
+ // get evaluation of f(x,u)
+ this->setCurrentStateAndControl(x_perturbed, u_local, t_local);
+ SCALAR dxdt = this->evaluateIntermediate();
+
+ if (doubleSidedDerivative_)
+ {
+ SCALAR dxdt_low;
+
+ volatile SCALAR x_mh = x_local(i) - h;
+ SCALAR dxm = x_local(i) - x_mh;
+
+ x_perturbed = x_local;
+ x_perturbed(i) = x_mh;
+ this->setCurrentStateAndControl(x_perturbed, u_local, t_local);
+ dxdt_low = this->evaluateIntermediate();
+ dFdx(i, 0) = (dxdt - dxdt_low) / (dxp + dxm);
+ }
+ else
+ {
+ dFdx(i, 0) = (dxdt - dxdt_ref) / dxp;
+ }
+ }
+
+ return dFdx;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediateNumDiff()
+{
+ control_vector_t dFdu = control_vector_t::Zero();
+ state_vector_t x_local;
+ control_vector_t u_local;
+ SCALAR t_local;
+ this->getCurrentStateAndControl(x_local, u_local, t_local);
+ SCALAR dxdt_ref = this->evaluateIntermediate();
+
+ for (size_t i = 0; i < CONTROL_DIM; ++i)
+ {
+ // inspired from http://en.wikipedia.org/wiki/Numerical_differentiation#Practical_considerations_using_floating_point_arithmetic
+ SCALAR h = eps_ * std::max(std::abs(u_local(i)), 1.0);
+ volatile SCALAR u_ph = u_local(i) + h;
+ SCALAR dup = u_ph - u_local(i);
+
+ control_vector_t u_perturbed = u_local;
+ u_perturbed(i) = u_ph;
+
+ // get evaluation of f(x,u)
+ this->setCurrentStateAndControl(x_local, u_perturbed, t_local);
+ SCALAR dxdt = this->evaluateIntermediate();
+
+ if (doubleSidedDerivative_)
+ {
+ SCALAR dxdt_low;
+
+ volatile SCALAR u_mh = u_local(i) - h;
+ SCALAR dum = u_local(i) - u_mh;
+
+ u_perturbed = u_local;
+ u_perturbed(i) = u_mh;
+ this->setCurrentStateAndControl(x_local, u_perturbed, t_local);
+ dxdt_low = this->evaluateIntermediate();
+
+ dFdu(i, 0) = (dxdt - dxdt_low) / (dup + dum);
+ }
+ else
+ {
+ dFdu(i, 0) = (dxdt - dxdt_ref) / dup;
+ }
+ }
+
+ return dFdu;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::initialize()
+{
+ /*!
+ * do nothing at all
+ */
+}
+
+
+// add terms
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addIntermediateTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term,
+ bool verbose)
+{
+ intermediateCostAnalytical_.push_back(term);
+ if (verbose)
+ {
+ std::string name = term->getName();
+ std::cout << "Trying to add term as intermediate" << std::endl;
+ }
+
+ return intermediateCostAnalytical_.size() - 1;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::addFinalTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term,
+ bool verbose)
+{
+ finalCostAnalytical_.push_back(term);
+ if (verbose)
+ {
+ std::string name = term->getName();
+ std::cout << "Trying to add term as final" << std::endl;
+ }
+
+ return finalCostAnalytical_.size() - 1;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediateBase()
+{
+ SCALAR y = SCALAR(0.0);
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ y += it->computeActivation(this->t_) * it->eval(this->x_, this->u_, this->t_);
+ }
+
+ return y;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminalBase()
+{
+ SCALAR y = SCALAR(0.0);
+
+ for (auto it : this->finalCostAnalytical_)
+ y += it->evaluate(this->x_, this->u_, this->t_);
+
+ return y;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediateBase()
+{
+ state_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->stateDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminalBase()
+{
+ state_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->stateDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediateBase()
+{
+ state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->stateSecondDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminalBase()
+{
+ state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->stateSecondDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediateBase()
+{
+ control_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->controlDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeTerminalBase()
+{
+ control_vector_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->controlDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+// get control second derivatives
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediateBase()
+{
+ control_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->controlSecondDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeTerminalBase()
+{
+ control_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->controlSecondDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+// get state-control derivatives
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediateBase()
+{
+ control_state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->intermediateCostAnalytical_)
+ {
+ if (!it->isActiveAtTime(this->t_))
+ {
+ continue;
+ }
+ derivative += it->computeActivation(this->t_) * it->stateControlDerivative(this->x_, this->u_, this->t_);
+ }
+
+ return derivative;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeTerminalBase()
+{
+ control_state_matrix_t derivative;
+ derivative.setZero();
+
+ for (auto it : this->finalCostAnalytical_)
+ derivative += it->stateControlDerivative(this->x_, this->u_, this->t_);
+
+ return derivative;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp
new file mode 100644
index 0000000..51ba632
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp
@@ -0,0 +1,239 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "CostFunction.hpp"
+#include "term/TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief Describes a cost function with a quadratic approximation, i.e. one that
+ * can compute first and second order derivatives with respect to state and
+ * control input. **This does NOT mean it has to be a purely quadratic cost
+ * function**. If you are looking for a purely quadratic cost function, check
+ * CostFunctionQuadraticSimple.
+ *
+ * A cost function is assumed to be a sum of intermediate and final terms, i.e.
+ * \f$ J(x,u,t) = \sum_{n=0}^{N_i} T_{i,n}(x,u,t) + \sum_{n=0}^{N_f} T_{i,f}(x,u,t) \f$
+ * These terms can have arbitrary form.
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionQuadratic : public CostFunction<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ typedef CostFunction<STATE_DIM, CONTROL_DIM, SCALAR> BASE;
+
+ /**
+ * Constructor
+ */
+ CostFunctionQuadratic();
+
+ /**
+ * Copy constructor
+ * @param arg other cost function
+ */
+ CostFunctionQuadratic(const CostFunctionQuadratic& arg);
+
+ /**
+ * Clones the cost function.
+ * @return
+ */
+ virtual CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const = 0;
+
+ /**
+ * Destructor
+ */
+ virtual ~CostFunctionQuadratic();
+
+ /**
+ * \brief Adds an intermediate term
+ * @param term intermediate term
+ * @param verbose verbosity flag which enables printout
+ * @return
+ */
+ virtual size_t addIntermediateTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term,
+ bool verbose = false);
+
+ virtual void addIntermediateADTerm(
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose = false);
+
+ /**
+ * \brief Adds a final term
+ * @param term final term
+ * @param verbose verbosity flag which enables printout
+ * @return
+ */
+ virtual size_t addFinalTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> term, bool verbose = false);
+
+ virtual void addFinalADTerm(std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR, ct::core::ADCGScalar>> term,
+ bool verbose = false);
+
+ /**
+ * \brief Loads cost function from config file
+ * @param filename config file location
+ * @param verbose verbosity flag which enables printout
+ */
+ virtual void loadFromConfigFile(const std::string& filename, bool verbose = false);
+
+ /**
+ * \brief Computes intermediate-cost first-order derivative with respect to state
+ * @return derivative vector (Jacobian)
+ */
+ virtual state_vector_t stateDerivativeIntermediate() = 0;
+
+ /**
+ * Computes terminal-cost first-order derivative with respect to state
+ * @return derivative vector (Jacobian)
+ */
+ virtual state_vector_t stateDerivativeTerminal() = 0;
+
+ /**
+ * \brief Computes intermediate-cost second-order derivative with respect to state
+ * @return derivative matrix (Jacobian)
+ */
+ virtual state_matrix_t stateSecondDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes final-cost second-order derivative with respect to state
+ * @return derivative matrix (Jacobian)
+ */
+ virtual state_matrix_t stateSecondDerivativeTerminal() = 0;
+
+ /**
+ * \brief Computes intermediate-cost first-order derivative with respect to control
+ * @return derivative vector (Jacobian)
+ */
+ virtual control_vector_t controlDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes terminal-cost first-order derivative with respect to control
+ *
+ * Not available for all cost functions. Throws an exception if not available.
+ * @return derivative vector (Jacobian)
+ */
+ virtual control_vector_t controlDerivativeTerminal();
+ /**
+ * \brief Computes intermediate-cost second-order derivative with respect to input
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_matrix_t controlSecondDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes final-cost second-order derivative with respect to input
+ *
+ * Not available for all cost functions. Throws an exception if not available.
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_matrix_t controlSecondDerivativeTerminal();
+
+ /**
+ * \brief Computes intermediate-cost derivative with respect to state and control
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_state_matrix_t stateControlDerivativeIntermediate() = 0;
+
+ /**
+ * \brief Computes final-cost derivative with respect to state and control
+ * @return derivative matrix (Jacobian)
+ */
+ virtual control_state_matrix_t stateControlDerivativeTerminal();
+
+ virtual void updateReferenceState(const state_vector_t& x_ref);
+
+ virtual void updateFinalState(const state_vector_t& x_final);
+
+ //! compare the state derivative against numerical differentiation
+ bool stateDerivativeIntermediateTest();
+
+ //! compare the control derivative against numerical differentiation
+ bool controlDerivativeIntermediateTest();
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getIntermediateTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getFinalTermById(const size_t id);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getIntermediateTermByName(const std::string& name);
+
+ std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>> getFinalTermByName(const std::string& name);
+
+ //! initialize the cost function (e.g. to be used in CostFunctionAD)
+ virtual void initialize();
+
+protected:
+ //! evaluate intermediate analytical cost terms
+ SCALAR evaluateIntermediateBase();
+
+ //! evaluate terminal analytical cost terms
+ SCALAR evaluateTerminalBase();
+
+ //! evaluate intermediate analytical state derivatives
+ state_vector_t stateDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical state derivatives
+ state_vector_t stateDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical state second derivatives
+ state_matrix_t stateSecondDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical state second derivatives
+ state_matrix_t stateSecondDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical control derivatives
+ control_vector_t controlDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical control derivatives
+ control_vector_t controlDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical control second derivatives
+ control_matrix_t controlSecondDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical control second derivatives
+ control_matrix_t controlSecondDerivativeTerminalBase();
+
+ //! evaluate intermediate analytical control mixed state control derivatives
+ control_state_matrix_t stateControlDerivativeIntermediateBase();
+
+ //! evaluate terminal analytical control mixed state control derivatives
+ control_state_matrix_t stateControlDerivativeTerminalBase();
+
+ //! compute the state derivative by numerical differentiation (can be used for testing)
+ state_vector_t stateDerivativeIntermediateNumDiff();
+
+ //! compute the control derivative by numerical differentiation (can be used for testing)
+ control_vector_t controlDerivativeIntermediateNumDiff();
+
+ //! stepsize for numerical differentiation
+ SCALAR eps_;
+
+ //! use double sided derivatives in numerical differentiation
+ bool doubleSidedDerivative_ = true;
+
+ /** list of intermediate cost terms for which analytic derivatives are available */
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>> intermediateCostAnalytical_;
+
+ /** list of final cost terms for which analytic derivatives are available */
+ std::vector<std::shared_ptr<TermBase<STATE_DIM, CONTROL_DIM, SCALAR>>> finalCostAnalytical_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
new file mode 100644
index 0000000..90a107d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp
@@ -0,0 +1,157 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple()
+{
+ x_nominal_.setZero();
+ Q_.setZero();
+ u_nominal_.setZero();
+ R_.setZero();
+ x_final_.setZero();
+ Q_final_.setZero();
+ x_deviation_.setZero();
+ u_deviation_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_vector_t& x_nominal,
+ const control_vector_t& u_nominal,
+ const state_vector_t& x_final,
+ const state_matrix_t& Q_final)
+ : x_nominal_(x_nominal), Q_(Q), u_nominal_(u_nominal), R_(R), x_final_(x_final), Q_final_(Q_final)
+{
+ x_deviation_.setZero();
+ u_deviation_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::~CostFunctionQuadraticSimple()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionQuadraticSimple(
+ const CostFunctionQuadraticSimple& arg)
+ : x_deviation_(arg.x_deviation_),
+ x_nominal_(arg.x_nominal_),
+ Q_(arg.Q_),
+ u_deviation_(arg.u_deviation_),
+ u_nominal_(arg.u_nominal_),
+ R_(arg.R_),
+ x_final_(arg.x_final_),
+ Q_final_(arg.Q_final_)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>*
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::clone() const
+{
+ return new CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t)
+{
+ this->x_ = x;
+ this->u_ = u;
+ this->t_ = t;
+
+ this->x_deviation_ = x - x_nominal_;
+ this->u_deviation_ = u - u_nominal_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateIntermediate()
+{
+ SCALAR costQ = SCALAR(0.5) * (x_deviation_.transpose() * Q_ * x_deviation_)(0);
+ SCALAR costR = SCALAR(0.5) * (u_deviation_.transpose() * R_ * u_deviation_)(0);
+ return costQ + costR;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeIntermediate()
+{
+ return Q_ * x_deviation_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeIntermediate()
+{
+ return Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::controlDerivativeIntermediate()
+{
+ return R_ * u_deviation_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::control_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::controlSecondDerivativeIntermediate()
+{
+ return R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::control_state_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateControlDerivativeIntermediate()
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::evaluateTerminal()
+{
+ state_vector_t x_deviation_final = this->x_ - x_final_;
+ return SCALAR(0.5) * x_deviation_final.transpose() * Q_final_ * x_deviation_final;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateDerivativeTerminal()
+{
+ state_vector_t x_deviation_final = this->x_ - x_final_;
+ return Q_final_ * x_deviation_final;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::stateSecondDerivativeTerminal()
+{
+ return Q_final_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::updateReferenceState(const state_vector_t& x_ref)
+{
+ x_nominal_ = x_ref;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>::updateFinalState(const state_vector_t& x_final)
+{
+ x_final_ = x_final;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
new file mode 100644
index 0000000..05c1bc8
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp
@@ -0,0 +1,106 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "CostFunctionQuadratic.hpp"
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup CostFunction
+ *
+ * \brief A simple quadratic cost function
+ *
+ * A simple, purely-quadratic cost function of the form
+ * \f$ J(x,u,t) = \bar{x}^T Q \bar{x} + \bar{u}^T R \bar{u} + \bar{x}^T_f Q_f \bar{x}^T_f \f$
+ * where \f$ \bar{x}, \bar{u} \f$ indicate deviations from a nominal (desired) state and control
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostFunctionQuadraticSimple : public CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+
+ /**
+ * Constructs a simple, purely quadratic cost function with all zero elements.
+ */
+ CostFunctionQuadraticSimple();
+ /**
+ * Constructs a simple, purely quadratic cost function
+ * @param Q intermediate state cost weighting
+ * @param R intermediate control cost weighting
+ * @param x_nominal nominal (desired) state
+ * @param u_nominal nominal (desired) control
+ * @param x_final nominal (desired) final state
+ * @param Q_final final state cost weighting
+ */
+ CostFunctionQuadraticSimple(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_vector_t& x_nominal,
+ const control_vector_t& u_nominal,
+ const state_vector_t& x_final,
+ const state_matrix_t& Q_final);
+
+ virtual ~CostFunctionQuadraticSimple();
+
+ CostFunctionQuadraticSimple(const CostFunctionQuadraticSimple& arg);
+
+ /**
+ * Clones the cost function.
+ * @return
+ */
+ CostFunctionQuadraticSimple<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override;
+
+ virtual void setCurrentStateAndControl(const state_vector_t& x,
+ const control_vector_t& u,
+ const SCALAR& t) override;
+
+ virtual SCALAR evaluateIntermediate() override;
+
+ virtual state_vector_t stateDerivativeIntermediate() override;
+
+ virtual state_matrix_t stateSecondDerivativeIntermediate() override;
+
+ virtual control_vector_t controlDerivativeIntermediate() override;
+
+ virtual control_matrix_t controlSecondDerivativeIntermediate() override;
+
+ virtual control_state_matrix_t stateControlDerivativeIntermediate() override;
+
+ virtual SCALAR evaluateTerminal() override;
+
+ virtual state_vector_t stateDerivativeTerminal() override;
+
+ virtual state_matrix_t stateSecondDerivativeTerminal() override;
+
+ virtual void updateReferenceState(const state_vector_t& x_ref) override;
+
+ virtual void updateFinalState(const state_vector_t& x_final) override;
+
+protected:
+ state_vector_t x_deviation_;
+ state_vector_t x_nominal_;
+ state_matrix_t Q_;
+
+ control_vector_t u_deviation_;
+ control_vector_t u_nominal_;
+ control_matrix_t R_;
+
+ state_vector_t x_final_;
+ state_matrix_t Q_final_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/costfunction-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/costfunction-impl.hpp
new file mode 100644
index 0000000..5bdead8
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/costfunction-impl.hpp
@@ -0,0 +1,21 @@
+/*!
+ * a convenience include file collecting all class implementations related to cost functions
+ */
+
+#pragma once
+
+// terms
+#include "term/TermBase-impl.hpp"
+#include "term/TermLinear-impl.hpp"
+#include "term/TermMixed-impl.hpp"
+#include "term/TermQuadratic-impl.hpp"
+#include "term/TermQuadMult-impl.hpp"
+#include "term/TermQuadTracking-impl.hpp"
+#include "term/TermStateBarrier-impl.hpp"
+
+// costfunctions
+#include "CostFunction-impl.hpp"
+#include "CostFunctionAD-impl.hpp"
+#include "CostFunctionAnalytical-impl.hpp"
+#include "CostFunctionQuadratic-impl.hpp"
+#include "CostFunctionQuadraticSimple-impl.hpp"
diff --git a/ct_optcon/include/ct/optcon/costfunction/costfunction.hpp b/ct_optcon/include/ct/optcon/costfunction/costfunction.hpp
new file mode 100644
index 0000000..1c8a194
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/costfunction.hpp
@@ -0,0 +1,21 @@
+/*!
+ * a convenience include file collecting all class declarations related to cost functions
+ */
+
+#pragma once
+
+// time activations
+#include "term/TermBase.hpp"
+#include "term/TermLinear.hpp"
+#include "term/TermMixed.hpp"
+#include "term/TermQuadratic.hpp"
+#include "term/TermQuadMult.hpp"
+#include "term/TermQuadTracking.hpp"
+#include "term/TermStateBarrier.hpp"
+
+// costfunctions
+#include "CostFunction.hpp"
+#include "CostFunctionQuadratic.hpp"
+#include "CostFunctionAD.hpp"
+#include "CostFunctionAnalytical.hpp"
+#include "CostFunctionQuadraticSimple.hpp"
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermBase-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermBase-impl.hpp
new file mode 100644
index 0000000..b87cbb7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermBase-impl.hpp
@@ -0,0 +1,213 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermBase(std::string name)
+ : name_(name),
+ c_i_(
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>>(new ct::core::tpl::ActivationBase<SCALAR_EVAL>()))
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermBase(const TermBase& arg) : name_(arg.name_), c_i_(arg.c_i_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermBase()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR_EVAL TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::eval(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& u,
+ const SCALAR_EVAL& t)
+{
+ return computeActivation(t) * evaluate(x, u, t);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ throw std::runtime_error("The cost function term term " + name_ + " does not implement evaluate CppadCg.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+bool TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::isActiveAtTime(SCALAR_EVAL t)
+{
+ return c_i_->isActive(t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR_EVAL TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::computeActivation(SCALAR_EVAL t)
+{
+ return c_i_->computeActivation(t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::StateVector<STATE_DIM, SCALAR_EVAL> TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function or "
+ "implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL> TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented "
+ "for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ throw std::runtime_error(
+ "This cost function element is not implemented for the given term. Please use either auto-diff cost function "
+ "or implement the analytical derivatives manually.");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setTimeActivation(
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i,
+ bool verbose)
+{
+ c_i_ = c_i;
+ if (verbose)
+ c_i_->printInfo();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadTimeActivation(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ if (verbose)
+ std::cout << "TermBase: loading TimeActivation ..." << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ try
+ {
+ std::string activationKind = pt.get<std::string>(termName + ".time_activation" + ".kind");
+ boost::algorithm::to_lower(activationKind);
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i;
+ CT_LOADABLE_ACTIVATIONS(SCALAR_EVAL);
+ c_i->loadConfigFile(filename, termName + ".time_activation", verbose);
+ if (!c_i)
+ {
+ throw std::runtime_error("Activation type \"" + activationKind + "\" not supported");
+ }
+ else
+ {
+ c_i_ = c_i;
+ if (verbose)
+ c_i_->printInfo();
+ }
+ } catch (std::exception& e)
+ {
+ return;
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+const std::string& TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getName() const
+{
+ return name_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setName(const std::string& termName)
+{
+ name_ = termName;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceState(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceControl(
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getReferenceState()
+ const
+{
+ throw std::runtime_error("getReferenceState is not implemented for the current term!");
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermBase.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermBase.hpp
new file mode 100644
index 0000000..8dd6bbf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermBase.hpp
@@ -0,0 +1,181 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <boost/algorithm/string.hpp>
+
+#include <ct/core/common/activations/Activations.h>
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup CostFunction
+ *
+ * \brief An interface for a term, supporting both analytical and auto-diff terms
+ *
+ * Derive from this term to implement your own term. You only have to implement
+ * evaluateCppadCg() if you want to use auto-diff. Otherwise, you need to implement
+ * evaluate() as well as the derivatives. In case you want to go for the most general
+ * implementation, you can implement a local, templated evalLocal() method in derived terms,
+ * which gets called by evaluate() and evaluateCppadCg(), see e.g. TermLinear.
+ *
+ * An example for an implementation of a custom term is given in \ref TermQuadratic.hpp
+ **/
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermBase
+{
+protected:
+ //! a name identifier for this term
+ std::string name_;
+ //! time activations for this term
+ std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i_;
+
+public:
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ /**
+ * \brief Default constructor
+ * @param name Name of the term
+ */
+ TermBase(std::string name = "Unnamed");
+
+ /**
+ * \brief Copy Cunstructor
+ * @param arg The term to copy
+ */
+ TermBase(const TermBase& arg);
+
+ /**
+ * \brief Deep-copy term
+ * @return
+ */
+ virtual TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const = 0;
+
+ /**
+ * \brief Destructor
+ */
+ virtual ~TermBase();
+
+ /**
+ * @brief Evaluates the term at x, u, t
+ *
+ * @param[in] x The current state
+ * @param[in] u The current control
+ * @param[in] t The current time
+ *
+ * @return The evaluatated cost term
+ */
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) = 0;
+
+ /**
+ * @brief The evaluate method used for jit compilation in CostfunctionAD
+ *
+ * @param[in] x The state vector
+ * @param[in] u The control vector
+ * @param[in] t The time
+ *
+ * @return The evaluated cost
+ */
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t);
+
+ /**
+ * @brief Gets called by the analytical costfunction. Adds time
+ * dependent activations on top of the term
+ *
+ * @param[in] x The current state
+ * @param[in] u The current control
+ * @param[in] t The current time
+ *
+ * @return The evaluatated cost term
+ */
+ SCALAR_EVAL eval(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& u,
+ const SCALAR_EVAL& t);
+
+ /**
+ * \brief Returns if term is non-zero at a specific time
+ * By default, all terms are evaluated at all times. However, if a term is not active at a certain time, you can overload this
+ * function to spare evaluations of the term and its derivatives
+ * @param t time
+ * @return true if term is active at t
+ */
+ virtual bool isActiveAtTime(SCALAR_EVAL t);
+
+ //! compute time activation
+ SCALAR_EVAL computeActivation(SCALAR_EVAL t);
+
+ //! compute derivative of this cost term w.r.t. the state
+ virtual core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute second order derivative of this cost term w.r.t. the state
+ virtual state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute derivative of this cost term w.r.t. the control input
+ virtual core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute second order derivative of this cost term w.r.t. the control input
+ virtual control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! compute the cross-term derivative (state-control) of this cost function term
+ virtual control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t);
+
+ //! load this term from a configuration file
+ virtual void loadConfigFile(const std::string& filename, const std::string& termName, bool verbose = false);
+
+ //! set the time activation functions for this term
+ void setTimeActivation(std::shared_ptr<ct::core::tpl::ActivationBase<SCALAR_EVAL>> c_i, bool verbose = false);
+
+ //! load the time activation functions for this term from file
+ void loadTimeActivation(const std::string& filename, const std::string& termName, bool verbose = false);
+
+ /**
+ * \brief Returns the name of the term
+ * @param termName name of the term
+ */
+ const std::string& getName() const;
+
+ /**
+ * \brief Sets the name of the term
+ * @param termName
+ */
+ void setName(const std::string& termName);
+
+ //! updates the reference state for this term
+ virtual void updateReferenceState(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState);
+
+ //! updates the reference control for this term
+ virtual void updateReferenceControl(const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl);
+
+ //! retrieve this term's current reference state
+ virtual Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> getReferenceState() const;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermLinear-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear-impl.hpp
new file mode 100644
index 0000000..aaa540e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear-impl.hpp
@@ -0,0 +1,125 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermLinear(const core::StateVector<STATE_DIM, SCALAR_EVAL> a,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> b,
+ const SCALAR_EVAL c)
+ : a_(a), b_(b), c_(c)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermLinear(const TermLinear &arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg), a_(arg.a_), b_(arg.b_), c_(arg.c_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+ *TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermLinear()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermLinear()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> &u,
+ const SCALAR &t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar> &x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar> &u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::StateVector<STATE_DIM, SCALAR_EVAL> TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return a_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return b_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return control_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t)
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string &filename,
+ const std::string &termName,
+ bool verbose)
+{
+ // read in the file and put the valus in a_ and b_
+ loadMatrixCF(filename, "a", a_, termName);
+ loadMatrixCF(filename, "b", b_, termName);
+ if (verbose)
+ {
+ std::cout << "Read a as a= \n" << a_ << std::endl;
+ std::cout << "Read b as b= \n" << b_ << std::endl;
+ }
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermLinear.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear.hpp
new file mode 100644
index 0000000..5011ab5
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermLinear.hpp
@@ -0,0 +1,100 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A linear term of type \f$ J = a x + b u + c \f$
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermLinear : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermLinear(const core::StateVector<STATE_DIM, SCALAR_EVAL> a,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> b,
+ const SCALAR_EVAL c = 0.);
+
+ TermLinear();
+
+ TermLinear(const TermLinear &arg);
+
+ TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR> *clone() const override;
+
+ virtual ~TermLinear();
+
+ SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> &u,
+ const SCALAR &t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar> &x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar> &u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override;
+
+ void loadConfigFile(const std::string &filename,
+ const std::string &termName,
+ bool verbose = false) override; // virtual function for data loading
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1> &x, const Eigen::Matrix<SC, CONTROL_DIM, 1> &u, const SC &t);
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> a_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> b_;
+ SCALAR_EVAL c_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermLinear<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1> &u,
+ const SC &t)
+{
+ Eigen::Matrix<SC, 1, 1> y_eigen = a_.template cast<SC>().transpose() * x + b_.template cast<SC>().transpose() * u;
+ SC y = y_eigen(0, 0) + SC(c_);
+ return y;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermLoadMacros.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermLoadMacros.hpp
new file mode 100644
index 0000000..a637d75
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermLoadMacros.hpp
@@ -0,0 +1,19 @@
+
+#pragma once
+
+#include "TermLinear.hpp"
+#include "TermQuadratic.hpp"
+#include "TermQuadMult.hpp"
+
+#define CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TERM, TERMNAME) \
+ if (termKind == TERMNAME) \
+ { \
+ term = std::shared_ptr<TERM<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>>( \
+ new TERM<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>()); \
+ term->setName(TERMNAME); \
+ }
+
+#define CT_LOADABLE_TERMS(SCALAR_EVAL, SCALAR) \
+ CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TermLinear, "linear") \
+ CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TermQuadratic, "quadratic") \
+ CT_LOADABLE_TERM(SCALAR_EVAL, SCALAR, TermQuadMult, "quadratic-multiplicative")
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermMixed-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed-impl.hpp
new file mode 100644
index 0000000..98d6093
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed-impl.hpp
@@ -0,0 +1,161 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed(const control_state_matrix_t& P) : P_(P)
+{
+ x_ref_.setZero(); // default values
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed()
+{
+ P_.setZero(); // default values
+ x_ref_.setZero();
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed(const control_state_matrix_t& P,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+ : P_(P), x_ref_(x_ref), u_ref_(u_ref)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermMixed(
+ const TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg), P_(arg.P_), x_ref_(arg.x_ref_), u_ref_(arg.u_ref_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone()
+ const
+{
+ return new TermMixed(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermMixed()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(const control_state_matrix_double_t& P)
+{
+ P_ = P.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateVector<STATE_DIM>& x_ref,
+ const core::ControlVector<CONTROL_DIM>& u_ref)
+{
+ x_ref_ = x_ref.template cast<SCALAR_EVAL>();
+ u_ref_ = u_ref.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::StateVector<STATE_DIM, SCALAR_EVAL> TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ return P_.transpose() * uDiff;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL> TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+
+ return P_ * xDiff;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return control_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return P_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ loadMatrixCF(filename, "P", P_, termName);
+ loadMatrixCF(filename, "x_des", x_ref_, termName);
+ loadMatrixCF(filename, "u_des", u_ref_, termName);
+ if (verbose)
+ {
+ std::cout << "Read P as P = \n" << P_ << std::endl;
+ std::cout << "Read x_ref as x_ref = \n" << x_ref_.transpose() << std::endl;
+ std::cout << "Read u_ref as u_ref = \n" << u_ref_.transpose() << std::endl;
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceState(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState)
+{
+ x_ref_ = newRefState;
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermMixed.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed.hpp
new file mode 100644
index 0000000..4f44eca
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermMixed.hpp
@@ -0,0 +1,112 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A basic quadratic term of type \f$ J = u^T P x \f$
+ *
+ * An example for using this term is given in \ref CostFunctionTest.cpp
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermMixed : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermMixed();
+
+ TermMixed(const control_state_matrix_t& P);
+
+ TermMixed(const control_state_matrix_t& P,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ TermMixed(const TermMixed& arg);
+
+ virtual ~TermMixed();
+
+ TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const control_state_matrix_double_t& P);
+
+ void setStateAndControlReference(const core::StateVector<STATE_DIM>& x_ref,
+ const core::ControlVector<CONTROL_DIM>& u_ref);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+ void updateReferenceState(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState) override;
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ control_state_matrix_t P_;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> x_ref_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> u_ref_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermMixed<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = (x - x_ref_.template cast<SC>());
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff = (u - u_ref_.template cast<SC>());
+
+ return (uDiff.transpose() * P_.template cast<SC>() * xDiff)(0, 0);
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult-impl.hpp
new file mode 100644
index 0000000..c2bb31d
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult-impl.hpp
@@ -0,0 +1,188 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult(const state_matrix_t& Q,
+ const control_matrix_t& R)
+ : Q_(Q), R_(R)
+{
+ x_ref_.setZero(); // default values
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult()
+{
+ Q_.setIdentity(); // default values
+ R_.setIdentity();
+ x_ref_.setZero();
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+ : Q_(Q), R_(R), x_ref_(x_ref), u_ref_(u_ref)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadMult(const TermQuadMult& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg),
+ Q_(arg.Q_),
+ R_(arg.R_),
+ x_ref_(arg.x_ref_),
+ u_ref_(arg.u_ref_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermQuadMult(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermQuadMult()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(const state_matrix_double_t& Q,
+ const control_matrix_double_t& R)
+{
+ Q_ = Q.template cast<SCALAR_EVAL>();
+ R_ = R.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateVector<STATE_DIM>& x_ref,
+ core::ControlVector<CONTROL_DIM>& u_ref)
+{
+ x_ref_ = x_ref.template cast<SCALAR_EVAL>();
+ ;
+ u_ref_ = u_ref.template cast<SCALAR_EVAL>();
+ ;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::StateVector<STATE_DIM, SCALAR_EVAL> TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ SCALAR_EVAL r = (uDiff.transpose() * R_ * uDiff)(0, 0);
+
+ return (xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_) * r;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ SCALAR_EVAL r = (uDiff.transpose() * R_ * uDiff)(0, 0);
+
+ return (Q_ + Q_.transpose()) * r;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ SCALAR_EVAL q = (xDiff.transpose() * Q_ * xDiff)(0, 0);
+
+ return (uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_) * q;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+
+ SCALAR_EVAL q = (xDiff.transpose() * Q_ * xDiff)(0, 0);
+
+ return (R_ + R_.transpose()) * q;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ return (uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_).transpose() *
+ (xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ loadMatrixCF(filename, "Q", Q_, termName);
+ loadMatrixCF(filename, "R", R_, termName);
+ loadMatrixCF(filename, "x_des", x_ref_, termName);
+ loadMatrixCF(filename, "u_des", u_ref_, termName);
+ if (verbose)
+ {
+ std::cout << "Read Q as Q = \n" << Q_ << std::endl;
+ std::cout << "Read R as R = \n" << R_ << std::endl;
+ std::cout << "Read x_ref as x_ref = \n" << x_ref_.transpose() << std::endl;
+ std::cout << "Read u_ref as u_ref = \n" << u_ref_.transpose() << std::endl;
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult.hpp
new file mode 100644
index 0000000..f7f68a0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult.hpp
@@ -0,0 +1,114 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A multiplicative term of type \f$ J = (x^T Q x) (u^T R u) \f$
+ *
+ * Probably this term is not very useful but we use it for testing.
+ *
+ * An example for using this term is given in \ref CostFunctionTest.cpp
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermQuadMult : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermQuadMult();
+
+ TermQuadMult(const state_matrix_t& Q, const control_matrix_t& R);
+
+ TermQuadMult(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ TermQuadMult(const TermQuadMult& arg);
+
+ virtual ~TermQuadMult();
+
+ TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const state_matrix_double_t& Q, const control_matrix_double_t& R);
+
+ void setStateAndControlReference(const core::StateVector<STATE_DIM>& x_ref,
+ core::ControlVector<CONTROL_DIM>& u_ref);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ void loadConfigFile(const std::string& filename, const std::string& termName, bool verbose = false);
+
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ state_matrix_t Q_;
+ control_matrix_t R_;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> x_ref_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> u_ref_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermQuadMult<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = (x - x_ref_.template cast<SC>());
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff = (u - u_ref_.template cast<SC>());
+
+ return (xDiff.transpose() * Q_.template cast<SC>() * xDiff)(0, 0) *
+ (uDiff.transpose() * R_.template cast<SC>() * uDiff)(0, 0);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking-impl.hpp
new file mode 100644
index 0000000..c9cdeb7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking-impl.hpp
@@ -0,0 +1,160 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadTracking(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::InterpolationType& stateSplineType,
+ const core::InterpolationType& controlSplineType,
+ const bool trackControlTrajectory)
+ : Q_(Q),
+ R_(R),
+ x_traj_ref_(stateSplineType),
+ u_traj_ref_(controlSplineType),
+ trackControlTrajectory_(trackControlTrajectory)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadTracking()
+{
+ // default values
+ Q_.setIdentity();
+ R_.setIdentity();
+ trackControlTrajectory_ = false;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadTracking(
+ const TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg),
+ Q_(arg.Q_),
+ R_(arg.R_),
+ x_traj_ref_(arg.x_traj_ref_),
+ u_traj_ref_(arg.u_traj_ref_),
+ trackControlTrajectory_(arg.trackControlTrajectory_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermQuadTracking()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermQuadTracking(*this);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(const state_matrix_double_t& Q,
+ const control_matrix_double_t& R)
+{
+ Q_ = Q.template cast<SCALAR_EVAL>();
+ R_ = R.template cast<SCALAR_EVAL>();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateTrajectory<STATE_DIM>& xTraj,
+ const core::ControlTrajectory<CONTROL_DIM>& uTraj)
+{
+ x_traj_ref_ = xTraj;
+ u_traj_ref_ = uTraj;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(
+ const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> xDiff = x - x_traj_ref_.eval(t);
+
+ return xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return Q_ + Q_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1> uDiff;
+
+ if (trackControlTrajectory_)
+ uDiff = u - u_traj_ref_.eval(t);
+ else
+ uDiff = u;
+
+ return uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return R_ + R_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ loadMatrixCF(filename, "Q", Q_, termName);
+ loadMatrixCF(filename, "R", R_, termName);
+ if (verbose)
+ {
+ std::cout << "Read Q as Q = \n" << Q_ << std::endl;
+ std::cout << "Read R as R = \n" << R_ << std::endl;
+ }
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking.hpp
new file mode 100644
index 0000000..2a2fb5c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking.hpp
@@ -0,0 +1,116 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A quadratic tracking term of type \f$ J(t) = (x_{ref}(t) - x)^T Q (x_{ref}(t) - x) + (u_{ref}(t) - u)^T R (u_{ref}(t) - u) \f$
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermQuadTracking : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermQuadTracking();
+
+ TermQuadTracking(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::InterpolationType& stateSplineType,
+ const core::InterpolationType& controlSplineType,
+ const bool trackControlTrajectory = false);
+
+ TermQuadTracking(const TermQuadTracking& arg);
+
+ virtual ~TermQuadTracking();
+
+ TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const state_matrix_double_t& Q, const control_matrix_double_t& R);
+
+ void setStateAndControlReference(const core::StateTrajectory<STATE_DIM>& xTraj,
+ const core::ControlTrajectory<CONTROL_DIM>& uTraj);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ state_matrix_t Q_;
+ control_matrix_t R_;
+
+ // the reference trajectories to be tracked
+ ct::core::StateTrajectory<STATE_DIM, SCALAR_EVAL> x_traj_ref_;
+ ct::core::ControlTrajectory<CONTROL_DIM, SCALAR_EVAL> u_traj_ref_;
+
+ // Option whether the control trajectory deviation shall be penalized or not
+ bool trackControlTrajectory_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermQuadTracking<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = x - x_traj_ref_.eval((SCALAR_EVAL)t).template cast<SC>();
+
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff;
+
+ if (trackControlTrajectory_)
+ uDiff = u - u_traj_ref_.eval((SCALAR_EVAL)t).template cast<SC>();
+ else
+ uDiff = u;
+
+ return (xDiff.transpose() * Q_.template cast<SC>() * xDiff + uDiff.transpose() * R_.template cast<SC>() * uDiff)(
+ 0, 0);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic-impl.hpp
new file mode 100644
index 0000000..072459c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic-impl.hpp
@@ -0,0 +1,235 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic(const state_matrix_t& Q,
+ const control_matrix_t& R)
+ : Q_(Q), R_(R)
+{
+ x_ref_.setZero(); // default values
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic()
+{
+ Q_.setConstant(9999); // default values
+ R_.setConstant(9999);
+ x_ref_.setZero();
+ u_ref_.setZero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+ : Q_(Q), R_(R), x_ref_(x_ref), u_ref_(u_ref)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermQuadratic(
+ const TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg),
+ Q_(arg.Q_),
+ R_(arg.R_),
+ x_ref_(arg.x_ref_),
+ u_ref_(arg.u_ref_)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermQuadratic()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermQuadratic(*this);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setWeights(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM>& Q,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM>& R)
+{
+ Q_ = Q;
+ R_ = R;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+const typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getStateWeight() const
+{
+ return Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getStateWeight()
+{
+ return Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+const typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getControlWeight() const
+{
+ return R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t&
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getControlWeight()
+{
+ return R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::setStateAndControlReference(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+{
+ x_ref_ = x_ref;
+ u_ref_ = u_ref;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(
+ const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ return evalLocal<SCALAR>(x, u, t);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::StateVector<STATE_DIM, SCALAR_EVAL>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> xDiff = (x - x_ref_);
+
+ return xDiff.transpose() * Q_.transpose() + xDiff.transpose() * Q_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::state_matrix_t
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return Q_ + Q_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+core::ControlVector<CONTROL_DIM, SCALAR_EVAL>
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> uDiff = (u - u_ref_);
+
+ return uDiff.transpose() * R_.transpose() + uDiff.transpose() * R_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_matrix_t
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::controlSecondDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return R_ + R_.transpose();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+typename TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::control_state_matrix_t
+TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::stateControlDerivative(
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t)
+{
+ return control_state_matrix_t::Zero();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ boost::property_tree::ptree pt;
+ try
+ {
+ boost::property_tree::read_info(filename, pt);
+ } catch (...)
+ {
+ }
+ this->name_ = pt.get<std::string>(termName + ".name.", termName);
+
+ loadMatrixCF(filename, "Q", Q_, termName);
+ loadMatrixCF(filename, "R", R_, termName);
+ loadMatrixCF(filename, "x_des", x_ref_, termName);
+ loadMatrixCF(filename, "u_des", u_ref_, termName);
+ if (verbose)
+ {
+ std::cout << "Read Q as Q = \n" << Q_ << std::endl;
+ std::cout << "Read R as R = \n" << R_ << std::endl;
+ std::cout << "Read x_des as x_des = \n" << x_ref_.transpose() << std::endl;
+ std::cout << "Read u_des as u_des = \n" << u_ref_.transpose() << std::endl;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceState(
+ const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState)
+{
+ x_ref_ = newRefState;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::updateReferenceControl(
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl)
+{
+ u_ref_ = newRefControl;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::getReferenceState()
+ const
+{
+ return x_ref_;
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic.hpp
new file mode 100644
index 0000000..9607f47
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic.hpp
@@ -0,0 +1,128 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A basic quadratic term of type \f$ J = x^T Q x + u^T R u \f$
+ *
+ * An example for using this term is given in \ref CostFunctionTest.cpp
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermQuadratic : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermQuadratic();
+
+ TermQuadratic(const state_matrix_t& Q, const control_matrix_t& R);
+
+ TermQuadratic(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ TermQuadratic(const TermQuadratic& arg);
+
+ virtual ~TermQuadratic();
+
+ virtual TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ void setWeights(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM>& Q,
+ const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM>& R);
+
+ const state_matrix_t& getStateWeight() const;
+
+ state_matrix_t& getStateWeight();
+
+ const control_matrix_t& getControlWeight() const;
+
+ control_matrix_t& getControlWeight();
+
+ void setStateAndControlReference(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref);
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ state_matrix_t stateSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_matrix_t controlSecondDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ control_state_matrix_t stateControlDerivative(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x,
+ const core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u,
+ const SCALAR_EVAL& t) override;
+
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+ virtual void updateReferenceState(const Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1>& newRefState) override;
+
+ virtual void updateReferenceControl(const Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, 1>& newRefControl) override;
+
+ virtual Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> getReferenceState() const override;
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t);
+
+ state_matrix_t Q_;
+ control_matrix_t R_;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> x_ref_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> u_ref_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+template <typename SC>
+SC TermQuadratic<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SC, CONTROL_DIM, 1>& u,
+ const SC& t)
+{
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = (x - x_ref_.template cast<SC>());
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff = (u - u_ref_.template cast<SC>());
+
+ return (xDiff.transpose() * Q_.template cast<SC>() * xDiff + uDiff.transpose() * R_.template cast<SC>() * uDiff)(
+ 0, 0);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier-impl.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier-impl.hpp
new file mode 100644
index 0000000..577ef59
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier-impl.hpp
@@ -0,0 +1,104 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermStateBarrier(const state_vector_t& ub,
+ const state_vector_t& lb,
+ const state_vector_t& alpha)
+ : ub_(ub), lb_(lb), alpha_(alpha)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermStateBarrier()
+ : ub_(state_vector_t::Zero()), lb_(state_vector_t::Zero()), alpha_(state_vector_t::Zero())
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::TermStateBarrier(const TermStateBarrier& arg)
+ : ub_(arg.ub_), lb_(arg.lb_), alpha_(arg.alpha_)
+{
+ initialize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::~TermStateBarrier()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>*
+TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::clone() const
+{
+ return new TermStateBarrier(*this);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::initialize()
+{
+ barriers_.clear();
+ for (size_t i = 0; i < STATE_DIM; i++)
+ {
+ barriers_.push_back(ct::core::tpl::BarrierActivation<SCALAR>(ub_(i), lb_(i), alpha_(i)));
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+SCALAR TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluate(
+ const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t)
+{
+ SCALAR c = SCALAR(0.0);
+ for (size_t i = 0; i < STATE_DIM; i++)
+ c += barriers_[i].computeActivation(x(i));
+ return c;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+ct::core::ADCGScalar TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t)
+{
+ ct::core::ADCGScalar c = ct::core::ADCGScalar(0.0);
+ for (size_t i = 0; i < STATE_DIM; i++)
+ c += barriers_[i].computeActivation(x(i));
+ return c;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL, typename SCALAR>
+void TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>::loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose)
+{
+ state_matrix_t Alpha;
+ state_matrix_t Ub;
+ state_matrix_t Lb;
+
+ loadMatrixCF(filename, "alpha", Alpha, termName);
+ loadMatrixCF(filename, "upper_bound", Ub, termName);
+ loadMatrixCF(filename, "lower_bound", Lb, termName);
+
+ alpha_ = Alpha.diagonal();
+ ub_ = Ub.diagonal();
+ lb_ = Lb.diagonal();
+
+ if (verbose)
+ {
+ std::cout << "Read alpha as = \n" << alpha_.transpose() << std::endl;
+ std::cout << "Read upper_bound as = \n" << ub_.transpose() << std::endl;
+ std::cout << "Read lower_bound as = \n" << lb_.transpose() << std::endl;
+ }
+}
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier.hpp b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier.hpp
new file mode 100644
index 0000000..ff9cff2
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier.hpp
@@ -0,0 +1,75 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "TermBase.hpp"
+#include <ct/core/common/activations/Activations.h>
+#include <ct/core/internal/traits/TraitSelectorSpecs.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * \ingroup CostFunction
+ *
+ * \brief A state barrier term (could also be considered a soft constraint)
+ * Note that this term explicitly excludes controls, as there are better
+ * ways to limit control effort in a "soft" way, e.g. through the use
+ * of sigmoid functions.
+ *
+ * @todo implement sigmoid barriers
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TermStateBarrier : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> state_vector_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+ TermStateBarrier();
+
+ TermStateBarrier(const state_vector_t& ub, const state_vector_t& lb, const state_vector_t& alpha);
+
+ TermStateBarrier(const TermStateBarrier& arg);
+
+ virtual ~TermStateBarrier();
+
+ TermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override;
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override;
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override;
+
+ //! load the term from config file, where the bounds are stored as matrices
+ virtual void loadConfigFile(const std::string& filename,
+ const std::string& termName,
+ bool verbose = false) override;
+
+protected:
+ void initialize();
+
+ state_vector_t alpha_;
+ state_vector_t ub_;
+ state_vector_t lb_;
+
+ std::vector<ct::core::tpl::BarrierActivation<SCALAR, ct::core::tpl::TraitSelector<SCALAR>>> barriers_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/costfunction/utility/utilities.hpp b/ct_optcon/include/ct/optcon/costfunction/utility/utilities.hpp
new file mode 100644
index 0000000..0147b31
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/costfunction/utility/utilities.hpp
@@ -0,0 +1,146 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <iostream>
+
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+
+#include <ct/optcon/costfunction/term/TermBase.hpp>
+
+#include "../CostFunction.hpp"
+
+namespace ct {
+namespace optcon {
+
+template <typename SCALAR>
+void loadScalarCF(const std::string& filename,
+ const std::string& scalarName,
+ SCALAR& scalar,
+ const std::string& termName = "")
+{
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ scalar = pt.get<SCALAR>(termName + ".weights." + scalarName);
+}
+
+template <typename SCALAR>
+void loadScalarOptionalCF(const std::string& filename,
+ const std::string& scalarName,
+ SCALAR& scalar,
+ const std::string& termName,
+ const SCALAR& defaultValue)
+{
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ scalar = pt.get<SCALAR>(termName + ".weights." + scalarName, defaultValue);
+}
+
+template <typename SCALAR, int ROW, int COL>
+void loadMatrixCF(const std::string& filename,
+ const std::string& matrixName,
+ Eigen::Matrix<SCALAR, ROW, COL>& matrix,
+ const std::string& termName = "")
+{
+ size_t rows = matrix.rows();
+ size_t cols = matrix.cols();
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ double scaling = pt.get<double>(termName + ".weights." + matrixName + ".scaling", 1);
+
+ for (size_t i = 0; i < rows; i++)
+ {
+ for (size_t j = 0; j < cols; j++)
+ {
+ if (termName == "")
+ {
+ matrix(i, j) =
+ scaling *
+ pt.get<double>(matrixName + "." + "(" + std::to_string(i) + "," + std::to_string(j) + ")", 0.0);
+ }
+ else
+ {
+ matrix(i, j) = scaling *
+ pt.get<double>(termName + ".weights." + matrixName + "." + "(" + std::to_string(i) +
+ "," + std::to_string(j) + ")",
+ 0.0);
+ }
+ }
+ }
+}
+
+template <typename TERM_PTR, typename costFuncType>
+void addTerm(const std::string& filename,
+ std::string& currentTerm,
+ int currentTermType,
+ TERM_PTR term,
+ costFuncType* costFunc,
+ bool verbose = false)
+{
+ switch (currentTermType)
+ {
+ case 0:
+ costFunc->addIntermediateTerm(term, verbose);
+ break;
+ case 1:
+ costFunc->addFinalTerm(term, verbose);
+ break;
+ default:
+ if (verbose)
+ {
+ std::cout << "error code 1 => term type other than term0 and term1 encountered" << std::endl;
+ }
+ BOOST_PROPERTY_TREE_THROW(boost::property_tree::info_parser::info_parser_error(
+ "read error code = ", "", 1)); //error code 1 => term type otherthan term0 and term1 encountered
+ break;
+ }
+ term->loadTimeActivation(filename, currentTerm, verbose);
+ term->loadConfigFile(filename, currentTerm, verbose);
+
+ if (verbose)
+ std::cout << "Successfully loaded term " + currentTerm << std::endl;
+}
+
+template <typename TERM_PTR, typename costFuncType>
+void addADTerm(const std::string& filename,
+ std::string& currentTerm,
+ int currentTermType,
+ TERM_PTR term,
+ costFuncType* costFunc,
+ bool verbose = false)
+{
+ switch (currentTermType)
+ {
+ case 0:
+ costFunc->addIntermediateADTerm(term, verbose);
+ break;
+ case 1:
+ costFunc->addFinalADTerm(term, verbose);
+ break;
+ default:
+ if (verbose)
+ {
+ std::cout << "error code 1 => term type other than term0 and term1 encountered" << std::endl;
+ }
+ BOOST_PROPERTY_TREE_THROW(boost::property_tree::info_parser::info_parser_error(
+ "read error code = ", "", 1)); //error code 1 => term type otherthan term0 and term1 encountered
+ break;
+ }
+ term->loadTimeActivation(filename, currentTerm, verbose);
+ term->loadConfigFile(filename, currentTerm, verbose);
+
+ if (verbose)
+ std::cout << "Successfully loaded term " + currentTerm << std::endl;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/Constraints b/ct_optcon/include/ct/optcon/dms/Constraints
new file mode 100644
index 0000000..825e219
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/Constraints
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "constraints/ConstraintDiscretizer.h"
+#include "constraints/ConstraintsContainerDms.h"
+#include "constraints/ContinuityConstraint.h"
+#include "constraints/InitStateConstraint.h"
+#include "constraints/TimeHorizonEqualityConstraint.h"
diff --git a/ct_optcon/include/ct/optcon/dms/Dms b/ct_optcon/include/ct/optcon/dms/Dms
new file mode 100644
index 0000000..9913e2c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/Dms
@@ -0,0 +1,18 @@
+
+#pragma once
+
+#include "dms_core/DmsSettings.h"
+#include "dms_core/DmsSolver.h"
+
+#include "dms_core/cost_evaluator/CostEvaluatorFull.h"
+#include "dms_core/cost_evaluator/CostEvaluatorSimple.h"
+
+#include "dms_core/spline/Linear/LinearSpliner.h"
+#include "dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h"
+
+#include "dms_core/ControllerDms.h"
+#include "dms_core/DmsDimensions.h"
+#include "dms_core/OptVectorDms.h"
+#include "dms_core/RKnDerivatives.h"
+#include "dms_core/ShotContainer.h"
+#include "dms_core/TimeGrid.h"
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/ConstraintDiscretizer.h b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintDiscretizer.h
new file mode 100644
index 0000000..dd154b4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintDiscretizer.h
@@ -0,0 +1,362 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/constraint/ConstraintContainerAD.h>
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/TimeGrid.h>
+#include <ct/optcon/dms/dms_core/spline/SplinerBase.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The class takes continuous constraints defined with the
+ * constraint toolbox and discretizes them over the DMS shots. These
+ * discretized constraints can then be used in the NLP module
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintDiscretizer : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef tpl::DiscreteConstraintBase<SCALAR> BASE;
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
+
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
+
+ typedef ct::core::tpl::TimeArray<SCALAR> time_array_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ ConstraintDiscretizer() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintDiscretizer() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] w The optimization variables
+ * @param[in] c_continuous The continuous constraints
+ * @param[in] activeInd A vector defining at which shots the constraint
+ * is active
+ */
+ ConstraintDiscretizer(std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ size_t N)
+ : w_(w),
+ controlSpliner_(controlSpliner),
+ timeGrid_(timeGrid),
+ N_(N),
+ constraintsCount_(0),
+ constraintsIntermediateCount_(0),
+ constraintsTerminalCount_(0),
+ nonZeroJacCount_(0),
+ nonZeroJacCountIntermediate_(0),
+ nonZeroJacCountTerminal_(0)
+ {
+ }
+
+
+ void setBoxConstraints(std::shared_ptr<LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> boxConstraints)
+ {
+ constraints_.push_back(boxConstraints);
+ constraintsIntermediateCount_ += (N_ + 1) * boxConstraints->getIntermediateConstraintsCount();
+ constraintsTerminalCount_ += boxConstraints->getTerminalConstraintsCount();
+ constraintsCount_ = constraintsIntermediateCount_ + constraintsTerminalCount_;
+
+ discreteConstraints_.resize(constraintsCount_);
+ discreteLowerBound_.resize(constraintsCount_);
+ discreteUpperBound_.resize(constraintsCount_);
+
+ nonZeroJacCountIntermediate_ += (N_ + 1) * (boxConstraints->getJacobianStateNonZeroCountIntermediate() +
+ boxConstraints->getJacobianInputNonZeroCountIntermediate());
+ nonZeroJacCountTerminal_ += boxConstraints->getJacobianStateNonZeroCountTerminal() +
+ boxConstraints->getJacobianInputNonZeroCountTerminal();
+ nonZeroJacCount_ = nonZeroJacCountIntermediate_ + nonZeroJacCountTerminal_;
+
+ discreteJac_.resize(nonZeroJacCount_);
+ discreteIRow_.resize(nonZeroJacCount_);
+ discreteJCol_.resize(nonZeroJacCount_);
+ }
+
+ void setGeneralConstraints(
+ std::shared_ptr<LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> generalConstraints)
+ {
+ constraints_.push_back(generalConstraints);
+ constraintsIntermediateCount_ += (N_ + 1) * generalConstraints->getIntermediateConstraintsCount();
+ constraintsTerminalCount_ += generalConstraints->getTerminalConstraintsCount();
+ constraintsCount_ = constraintsIntermediateCount_ + constraintsTerminalCount_;
+
+ discreteConstraints_.resize(constraintsCount_);
+ discreteLowerBound_.resize(constraintsCount_);
+ discreteUpperBound_.resize(constraintsCount_);
+
+ nonZeroJacCountIntermediate_ += (N_ + 1) * generalConstraints->getJacobianStateNonZeroCountIntermediate();
+ nonZeroJacCountTerminal_ += generalConstraints->getJacobianStateNonZeroCountTerminal();
+ nonZeroJacCount_ = nonZeroJacCountIntermediate_ + nonZeroJacCountTerminal_;
+
+ discreteJac_.resize(nonZeroJacCount_);
+ discreteIRow_.resize(nonZeroJacCount_);
+ discreteJCol_.resize(nonZeroJacCount_);
+ }
+
+ virtual VectorXs eval() override
+ {
+ size_t constraintSize = 0;
+ size_t discreteInd = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ SCALAR tShot = timeGrid_->getShotStartTime(n);
+ for (auto constraint : constraints_)
+ {
+ constraint->setCurrentStateAndControl(
+ w_->getOptimizedState(n), controlSpliner_->evalSpline(tShot, n), tShot);
+ constraintSize = constraint->getIntermediateConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteConstraints_.segment(discreteInd, constraintSize) = constraint->evaluateIntermediate();
+ discreteInd += constraintSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getTerminalConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteConstraints_.segment(discreteInd, constraintSize) = constraint->evaluateTerminal();
+ discreteInd += constraintSize;
+ }
+ }
+
+ return discreteConstraints_;
+ }
+
+ virtual VectorXs evalSparseJacobian() override
+ {
+ size_t jacSize = 0;
+ size_t discreteInd = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ SCALAR tShot = timeGrid_->getShotStartTime(n);
+
+ for (auto constraint : constraints_)
+ {
+ constraint->setCurrentStateAndControl(
+ w_->getOptimizedState(n), controlSpliner_->evalSpline(tShot, n), tShot);
+ jacSize = constraint->getJacobianStateNonZeroCountIntermediate();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianStateSparseIntermediate();
+ discreteInd += jacSize;
+ }
+
+ jacSize = constraint->getJacobianInputNonZeroCountIntermediate();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianInputSparseIntermediate();
+ discreteInd += jacSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ jacSize = constraint->getJacobianStateNonZeroCountTerminal();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianStateSparseTerminal();
+ discreteInd += jacSize;
+ }
+ jacSize = constraint->getJacobianInputNonZeroCountTerminal();
+ if (jacSize > 0)
+ {
+ discreteJac_.segment(discreteInd, jacSize) = constraint->jacobianInputSparseTerminal();
+ discreteInd += jacSize;
+ }
+ }
+
+ return discreteJac_;
+ }
+
+ virtual size_t getNumNonZerosJacobian() override { return nonZeroJacCount_; }
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ size_t discreteInd = 0;
+ size_t rowOffset = 0;
+ size_t nnEle = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ for (auto constraint : constraints_)
+ {
+ nnEle = constraint->getJacobianStateNonZeroCountIntermediate();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowStateIntermediate(constraint->getJacobianStateNonZeroCountIntermediate());
+ Eigen::VectorXi jColStateIntermediate(constraint->getJacobianStateNonZeroCountIntermediate());
+ constraint->sparsityPatternStateIntermediate(iRowStateIntermediate, jColStateIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowStateIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColStateIntermediate.array() + w_->getStateIndex(n);
+ discreteInd += nnEle;
+ }
+
+ nnEle = constraint->getJacobianInputNonZeroCountIntermediate();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowInputIntermediate(constraint->getJacobianInputNonZeroCountIntermediate());
+ Eigen::VectorXi jColInputIntermediate(constraint->getJacobianInputNonZeroCountIntermediate());
+ constraint->sparsityPatternInputIntermediate(iRowInputIntermediate, jColInputIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowInputIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColInputIntermediate.array() + w_->getControlIndex(n);
+ discreteInd += nnEle;
+ }
+
+ rowOffset += constraint->getJacobianStateNonZeroCountIntermediate() +
+ constraint->getJacobianInputNonZeroCountIntermediate();
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ nnEle = constraint->getJacobianStateNonZeroCountTerminal();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowStateIntermediate(constraint->getJacobianStateNonZeroCountTerminal());
+ Eigen::VectorXi jColStateIntermediate(constraint->getJacobianStateNonZeroCountTerminal());
+ constraint->sparsityPatternStateTerminal(iRowStateIntermediate, jColStateIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowStateIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColStateIntermediate.array() + w_->getStateIndex(N_);
+ discreteInd += nnEle;
+ }
+
+ nnEle = constraint->getJacobianInputNonZeroCountTerminal();
+ if (nnEle > 0)
+ {
+ Eigen::VectorXi iRowInputIntermediate(constraint->getJacobianInputNonZeroCountTerminal());
+ Eigen::VectorXi jColInputIntermediate(constraint->getJacobianInputNonZeroCountTerminal());
+ constraint->sparsityPatternInputTerminal(iRowInputIntermediate, jColInputIntermediate);
+ discreteIRow_.segment(discreteInd, nnEle) = iRowInputIntermediate.array() + rowOffset;
+ discreteJCol_.segment(discreteInd, nnEle) = jColInputIntermediate.array() + w_->getControlIndex(N_);
+ discreteInd += nnEle;
+ }
+ rowOffset +=
+ constraint->getJacobianStateNonZeroCountTerminal() + constraint->getJacobianInputNonZeroCountTerminal();
+ }
+
+ iRow_vec = discreteIRow_;
+ jCol_vec = discreteJCol_;
+ }
+
+ virtual VectorXs getLowerBound() override
+ {
+ size_t discreteInd = 0;
+ size_t constraintSize = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getIntermediateConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteLowerBound_.segment(discreteInd, constraintSize) = constraint->getLowerBoundsIntermediate();
+ discreteInd += constraintSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getTerminalConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteLowerBound_.segment(discreteInd, constraintSize) = constraint->getLowerBoundsTerminal();
+ discreteInd += constraintSize;
+ }
+ }
+
+ return discreteLowerBound_;
+ }
+
+ virtual VectorXs getUpperBound() override
+ {
+ size_t discreteInd = 0;
+ size_t constraintSize = 0;
+
+ for (size_t n = 0; n < N_ + 1; ++n)
+ {
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getIntermediateConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteUpperBound_.segment(discreteInd, constraintSize) = constraint->getUpperBoundsIntermediate();
+ discreteInd += constraintSize;
+ }
+ }
+ }
+
+ for (auto constraint : constraints_)
+ {
+ constraintSize = constraint->getTerminalConstraintsCount();
+ if (constraintSize > 0)
+ {
+ discreteUpperBound_.segment(discreteInd, constraintSize) = constraint->getUpperBoundsTerminal();
+ discreteInd += constraintSize;
+ }
+ }
+
+ return discreteUpperBound_;
+ }
+
+ virtual size_t getConstraintSize() override { return constraintsCount_; }
+private:
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+ size_t N_;
+
+ std::vector<std::shared_ptr<LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> constraints_;
+
+ size_t constraintsCount_;
+ size_t constraintsIntermediateCount_;
+ size_t constraintsTerminalCount_;
+
+ VectorXs discreteConstraints_;
+ VectorXs discreteLowerBound_;
+ VectorXs discreteUpperBound_;
+
+ VectorXs discreteJac_;
+ Eigen::VectorXi discreteIRow_;
+ Eigen::VectorXi discreteJCol_;
+
+ size_t nonZeroJacCount_;
+ size_t nonZeroJacCountIntermediate_;
+ size_t nonZeroJacCountTerminal_;
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/ConstraintsContainerDms.h b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintsContainerDms.h
new file mode 100644
index 0000000..031f53e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/ConstraintsContainerDms.h
@@ -0,0 +1,97 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <Eigen/Dense>
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ShotContainer.h>
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+#include <ct/optcon/nlp/DiscreteConstraintContainerBase.h>
+#include <ct/optcon/dms/constraints/InitStateConstraint.h>
+#include <ct/optcon/dms/constraints/ContinuityConstraint.h>
+#include <ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+#include <ct/optcon/dms/constraints/ConstraintDiscretizer.h>
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Container class for the constraints used in DMS
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintsContainerDms : public tpl::DiscreteConstraintContainerBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
+ typedef typename DIMENSIONS::state_control_matrix_t state_control_matrix_t;
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] w The optimization variables
+ * @param[in] timeGrid The time grid
+ * @param[in] shotContainers The shot containers
+ * @param[in] constraintsIntermediate The intermediate constraints
+ * @param[in] constraintsFinal The final constraints
+ * @param[in] x0 The initial state
+ * @param[in] settings The dms settings
+ */
+ ConstraintsContainerDms(std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers,
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>> discretizedConstraints,
+ const state_vector_t& x0,
+ const DmsSettings settings);
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~ConstraintsContainerDms(){};
+
+ virtual void prepareEvaluation() override;
+
+ virtual void prepareJacobianEvaluation() override;
+
+ /**
+ * @brief Updates the initial constraint
+ *
+ * @param[in] x0 The new initial state
+ */
+ void changeInitialConstraint(const state_vector_t& x0);
+
+private:
+ const DmsSettings settings_;
+
+ std::shared_ptr<InitStateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>> c_init_;
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers_;
+};
+
+#include "implementation/ConstraintsContainerDms-impl.h"
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/ContinuityConstraint.h b/ct_optcon/include/ct/optcon/dms/constraints/ContinuityConstraint.h
new file mode 100644
index 0000000..0084915
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/ContinuityConstraint.h
@@ -0,0 +1,351 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/ShotContainer.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Implementation of the DMS continuity constraints
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ContinuityConstraint : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef tpl::DiscreteConstraintBase<SCALAR> BASE;
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ /**
+ * @brief Default constructor
+ */
+ ContinuityConstraint() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] shotContainer The shot container
+ * @param[in] w The optimization variables
+ * @param[in] shotIndex The shot number
+ * @param[in] settings The dms settings
+ */
+ ContinuityConstraint(std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>> shotContainer,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ size_t shotIndex,
+ const DmsSettings settings)
+ : shotContainer_(shotContainer), w_(w), shotIndex_(shotIndex), settings_(settings)
+ {
+ lb_.setConstant(SCALAR(0.0));
+ ub_.setConstant(SCALAR(0.0));
+
+ size_t nr = 0;
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ nr = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM;
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ nr = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM + STATE_DIM * CONTROL_DIM;
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // nr += STATE_DIM;
+ jacLocal_.resize(nr);
+ }
+
+
+ virtual VectorXs eval() override
+ {
+ stateNext_ = shotContainer_->getStateIntegrated();
+ assert(stateNext_ == stateNext_);
+ assert(w_->getOptimizedState(shotIndex_ + 1) == w_->getOptimizedState(shotIndex_ + 1));
+ return w_->getOptimizedState(shotIndex_ + 1) - stateNext_;
+ }
+
+ virtual VectorXs evalSparseJacobian() override
+ {
+ count_local_ = 0;
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ computeXblock(); // add the big block (derivative w.r.t. state s_i)
+ computeUblock(); // add the smaller block (derivative w.r.t. control q_i)
+ computeIblock(); // add the diagonal (derivative w.r.t. state s_(i+1))
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // computeHblock();
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ computeXblock(); // add the big block (derivative w.r.t. state s_i)
+ computeUblock(); // add the smaller block (derivative w.r.t. control q_i)
+ computeIblock(); // add the diagonal (derivative w.r.t. state s_(i+1))
+ computeUblock_2(); // add the smaller block (derivative w.r.t. control q_(i+1))
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // computeHblock();
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+ return jacLocal_;
+ }
+
+ virtual size_t getNumNonZerosJacobian() override
+ {
+ size_t no = 0;
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ no = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM * 1;
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ no = STATE_DIM * STATE_DIM + STATE_DIM * CONTROL_DIM + STATE_DIM * 1 + STATE_DIM * CONTROL_DIM;
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+
+ /* the derivatives w.r.t. the time optimization variable (h_i) */
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // no += STATE_DIM;
+ // }
+
+ return no;
+ }
+
+
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ size_t indexNumber = 0;
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ // add the big block (derivative w.r.t. state)
+ indexNumber += BASE::genBlockIndices(
+ w_->getStateIndex(shotIndex_), STATE_DIM, STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the smaller block (derivative w.r.t. control)
+ indexNumber += BASE::genBlockIndices(
+ w_->getControlIndex(shotIndex_), STATE_DIM, CONTROL_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the diagonal
+ indexNumber += BASE::genDiagonalIndices(
+ w_->getStateIndex(shotIndex_ + 1), STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ // add the big block (derivative w.r.t. state)
+ indexNumber += BASE::genBlockIndices(
+ w_->getStateIndex(shotIndex_), STATE_DIM, STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the smaller block (derivative w.r.t. control)
+ indexNumber += BASE::genBlockIndices(
+ w_->getControlIndex(shotIndex_), STATE_DIM, CONTROL_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the diagonal
+ indexNumber += BASE::genDiagonalIndices(
+ w_->getStateIndex(shotIndex_ + 1), STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+
+ // add the fourth block (derivative w.r.t. control)
+ indexNumber += BASE::genBlockIndices(
+ w_->getControlIndex(shotIndex_ + 1), STATE_DIM, CONTROL_DIM, iRow_vec, jCol_vec, indexNumber);
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+ }
+ }
+
+ /* for the derivatives w.r.t. the time optimization variables (t_i) */
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // indexNumber += BASE::genBlockIndices(w_->getTimeSegmentIndex(shotIndex_),
+ // STATE_DIM, 1, iRow_vec, jCol_vec, indexNumber);
+ // }
+ }
+
+ virtual VectorXs getLowerBound() override { return lb_; }
+ virtual VectorXs getUpperBound() override { return ub_; }
+ virtual size_t getConstraintSize() override { return STATE_DIM; }
+private:
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * states s_i
+ */
+ void computeXblock();
+
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * inputs q_i
+ */
+ void computeUblock();
+
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * inputs q_{i+1}
+ */
+ void computeUblock_2();
+
+ /**
+ * @brief Evaluates an identity matrix in sparse form
+ */
+ void computeIblock();
+
+ /**
+ * @brief Evaluates the sparse jacobian with respect to the discretized
+ * time segments h_i
+ */
+ void computeHblock();
+
+ std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>> shotContainer_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ size_t shotIndex_;
+ const DmsSettings settings_;
+
+ VectorXs jacLocal_;
+ size_t count_local_;
+ state_vector_t stateNext_;
+
+ state_vector_t lb_;
+ state_vector_t ub_;
+};
+
+
+// template <size_t STATE_DIM, size_t CONTROL_DIM>
+// void ContinuityConstraint<STATE_DIM, CONTROL_DIM>::computeHblock()
+// {
+// // take last elements of state and time
+// state_vector_t state = shotContainer_->getStateIntegrated();
+// ct::core::Time time = shotContainer_->getIntegrationTimeFinal();
+
+// // compute derivative
+// state_vector_t mat;
+// state_vector_t dynamics;
+// shotContainer_->getControlledSystemPtr()->computeDynamics(state, time, dynamics);
+
+// switch (settings_.splineType_)
+// {
+// case DmsSettings::ZERO_ORDER_HOLD:
+// {
+// mat = - dynamics;
+// break;
+// }
+// case DmsSettings::PIECEWISE_LINEAR:
+// {
+// mat = -dynamics - shotContainer_->getdXdHiIntegrated();
+// break;
+// }
+// default:
+// {
+// throw(std::runtime_error("specified invalid spliner type in ContinuityConstraint-class"));
+// }
+// }
+
+// jacLocal_.segment(count_local_, STATE_DIM) = mat;
+// count_local_ += STATE_DIM;
+// }
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeXblock()
+{
+ state_matrix_t mat = -shotContainer_->getdXdSiIntegrated();
+ mat.transposeInPlace();
+ VectorXs dXdSiVec = (Eigen::Map<VectorXs>(mat.data(), STATE_DIM * STATE_DIM));
+
+ // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM * STATE_DIM) = dXdSiVec;
+
+ count_local_ += STATE_DIM * STATE_DIM;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeUblock()
+{
+ MatrixXs mat = -shotContainer_->getdXdQiIntegrated();
+ mat.transposeInPlace();
+ VectorXs dXdQiVec = Eigen::Map<VectorXs>(mat.data(), STATE_DIM * CONTROL_DIM);
+
+ // // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM * CONTROL_DIM) = dXdQiVec;
+ count_local_ += STATE_DIM * CONTROL_DIM;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeUblock_2()
+{
+ MatrixXs mat = -shotContainer_->getdXdQip1Integrated();
+ mat.transposeInPlace();
+ VectorXs dXdU1Vec = Eigen::Map<VectorXs>(mat.data(), STATE_DIM * CONTROL_DIM);
+
+ // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM * CONTROL_DIM) = dXdU1Vec;
+ count_local_ += STATE_DIM * CONTROL_DIM;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>::computeIblock()
+{
+ // fill into value vector with correct indexing
+ jacLocal_.segment(count_local_, STATE_DIM) = VectorXs::Ones(STATE_DIM);
+ count_local_ += STATE_DIM;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/InitStateConstraint.h b/ct_optcon/include/ct/optcon/dms/constraints/InitStateConstraint.h
new file mode 100644
index 0000000..fbe9821
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/InitStateConstraint.h
@@ -0,0 +1,79 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The implementation of the DMS initial state constraint
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class InitStateConstraint : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef tpl::DiscreteConstraintBase<SCALAR> BASE;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ InitStateConstraint() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] x0 The initial state
+ * @param[in] w The optimization variables
+ */
+ InitStateConstraint(const state_vector_t& x0, std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w)
+ : w_(w), x_0_(x0)
+ {
+ lb_.setConstant(SCALAR(0.0));
+ ub_.setConstant(SCALAR(0.0));
+ }
+
+ /**
+ * @brief Updates the constraint
+ *
+ * @param[in] x0 The new initial state
+ */
+ void updateConstraint(const state_vector_t& x0) { x_0_ = x0; }
+ virtual VectorXs eval() override { return w_->getOptimizedState(0) - x_0_; }
+ virtual VectorXs evalSparseJacobian() override { return state_vector_t::Ones(); }
+ virtual size_t getNumNonZerosJacobian() override { return (size_t)STATE_DIM; }
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ size_t indexNumber = 0;
+ indexNumber += BASE::genDiagonalIndices(w_->getStateIndex(0), STATE_DIM, iRow_vec, jCol_vec, indexNumber);
+ }
+
+ virtual VectorXs getLowerBound() override { return lb_; }
+ virtual VectorXs getUpperBound() override { return ub_; }
+ virtual size_t getConstraintSize() override { return STATE_DIM; }
+private:
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ state_vector_t x_0_;
+
+ //Constraint bounds
+ state_vector_t lb_; // lower bound
+ state_vector_t ub_; // upper bound
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h b/ct_optcon/include/ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h
new file mode 100644
index 0000000..a968093
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h
@@ -0,0 +1,95 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <ct/optcon/nlp/DiscreteConstraintBase.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This is the implementation of the time horizon constraint when
+ * using time grid optimization.
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class TimeHorizonEqualityConstraint : public tpl::DiscreteConstraintBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef tpl::DiscreteConstraintBase<double> BASE;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ TimeHorizonEqualityConstraint() {}
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] w The optimization variables
+ * @param[in] timeGrid The dms time grid
+ * @param[in] settings The dms settings
+ */
+ TimeHorizonEqualityConstraint(std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ DmsSettings settings)
+ : w_(w), timeGrid_(timeGrid), settings_(settings)
+ {
+ // lower bound is number of shot times the lower bound for each interval h
+ lb_ << SCALAR(settings_.N_ * settings_.h_min_ - settings_.T_);
+ ub_ << SCALAR(0.0);
+
+ std::cout << " ... time horizon lower bound: " << SCALAR(settings_.T_ + lb_(0)) << std::endl;
+ std::cout << " ... time horizon upper bound: " << SCALAR(settings_.T_ + ub_(0)) << std::endl;
+ }
+
+ virtual VectorXs eval() override
+ {
+ Eigen::Matrix<SCALAR, 1, 1> mat;
+ mat << SCALAR(timeGrid_->getOptimizedTimeHorizon() - settings_.T_);
+ return mat;
+ }
+
+ virtual VectorXs evalSparseJacobian() override
+ {
+ VectorXs one(settings_.N_);
+ one.setConstant(SCALAR(1.0));
+ return one;
+ }
+
+ virtual size_t getNumNonZerosJacobian() override { return settings_.N_; }
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) override
+ {
+ for (size_t i = 0; i < settings_.N_; ++i)
+ {
+ iRow_vec(i) = 0;
+ jCol_vec(i) = w_->getTimeSegmentIndex(i);
+ }
+ }
+
+ virtual VectorXs getLowerBound() override { return lb_; }
+ virtual VectorXs getUpperBound() override { return ub_; }
+ virtual size_t getConstraintSize() override { return 1; }
+private:
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+ DmsSettings settings_;
+
+ //Constraint bounds
+ Eigen::Matrix<SCALAR, 1, 1> lb_;
+ Eigen::Matrix<SCALAR, 1, 1> ub_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/constraints/implementation/ConstraintsContainerDms-impl.h b/ct_optcon/include/ct/optcon/dms/constraints/implementation/ConstraintsContainerDms-impl.h
new file mode 100644
index 0000000..0238b43
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/constraints/implementation/ConstraintsContainerDms-impl.h
@@ -0,0 +1,71 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintsContainerDms(
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers,
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>> discretizedConstraints,
+ const state_vector_t& x0,
+ const DmsSettings settings)
+ : settings_(settings), shotContainers_(shotContainers)
+{
+ c_init_ = std::shared_ptr<InitStateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new InitStateConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(x0, w));
+
+ this->constraints_.push_back(c_init_);
+
+ for (size_t shotNr = 0; shotNr < settings_.N_; shotNr++)
+ {
+ std::shared_ptr<ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>> c_i =
+ std::shared_ptr<ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ContinuityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(shotContainers[shotNr], w, shotNr, settings));
+
+ this->constraints_.push_back(c_i);
+ }
+
+ if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ {
+ std::shared_ptr<TimeHorizonEqualityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>> c_horizon_equal =
+ std::shared_ptr<TimeHorizonEqualityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new TimeHorizonEqualityConstraint<STATE_DIM, CONTROL_DIM, SCALAR>(w, timeGrid, settings));
+
+ this->constraints_.push_back(c_horizon_equal);
+ }
+
+ if (discretizedConstraints)
+ {
+ std::cout << "Adding discretized constraints" << std::endl;
+ this->constraints_.push_back(discretizedConstraints);
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::prepareEvaluation()
+{
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateShot();
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::prepareJacobianEvaluation()
+{
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateSensitivities();
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>::changeInitialConstraint(const state_vector_t& x0)
+{
+ c_init_->updateConstraint(x0);
+}
diff --git a/ct_optcon/include/ct/optcon/dms/dms.h b/ct_optcon/include/ct/optcon/dms/dms.h
new file mode 100644
index 0000000..55f3e65
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms.h
@@ -0,0 +1,7 @@
+
+#pragma once
+
+// Include file for convenience
+
+#include "Constraints"
+#include "Dms"
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/ControllerDms.h b/ct_optcon/include/ct/optcon/dms/dms_core/ControllerDms.h
new file mode 100644
index 0000000..297e538
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/ControllerDms.h
@@ -0,0 +1,84 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <Eigen/Dense>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * \ingroup DMS
+ *
+ * \brief DMS controller class
+ *
+ * Implements a controller to be handed over to a system of type "ControlledSystem". This controller applies the nominal input trajectory designed by the algorithm,
+ * which is either piecewise constant or piecewise linear between the nodes.
+ *
+ * @tparam STATE_DIM: Dimension of the state vector
+ * @tparam INPUT_DIM: Dimension of the control input vector
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ControllerDms : public ct::core::Controller<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+
+
+ ControllerDms() = delete;
+
+ ControllerDms(std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner, size_t shotIdx)
+ : controlSpliner_(controlSpliner), shotIdx_(shotIdx)
+ {
+ }
+
+
+ ControllerDms(const ControllerDms& arg) : controlSpliner_(arg.controlSpliner_), shotIdx_(arg.shotIdx_) {}
+ virtual ~ControllerDms() {}
+ virtual ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
+ {
+ return new ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>(*this);
+ }
+
+
+ void computeControl(const state_vector_t& state, const SCALAR& t, control_vector_t& controlAction)
+ {
+ controlAction = controlSpliner_->evalSpline(t, shotIdx_);
+ assert(controlAction == controlAction);
+ }
+
+ virtual core::ControlMatrix<CONTROL_DIM, SCALAR> getDerivativeU0(const state_vector_t& state,
+ const SCALAR time) override
+ {
+ return controlSpliner_->splineDerivative_q_i(time, shotIdx_);
+ }
+
+ virtual core::ControlMatrix<CONTROL_DIM, SCALAR> getDerivativeUf(const state_vector_t& state,
+ const SCALAR time) override
+ {
+ return controlSpliner_->splineDerivative_q_iplus1(time, shotIdx_);
+ }
+
+
+private:
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+
+ /* index of the shot to which this controller belongs */
+ size_t shotIdx_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsDimensions.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsDimensions.h
new file mode 100644
index 0000000..380f591
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsDimensions.h
@@ -0,0 +1,46 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Defines basic types used in the DMS algorithm
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DmsDimensions
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> state_matrix_array_t;
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> state_control_matrix_t;
+ typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> state_control_matrix_array_t;
+
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
+
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef ct::core::ControlMatrixTrajectory<CONTROL_DIM, SCALAR> control_matrix_array_t;
+
+ typedef SCALAR time_t;
+ typedef ct::core::tpl::TimeArray<SCALAR> time_array_t;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsProblem.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsProblem.h
new file mode 100644
index 0000000..a657a09
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsProblem.h
@@ -0,0 +1,477 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+
+#include <ct/optcon/problem/OptConProblem.h>
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ControllerDms.h>
+#include <ct/optcon/dms/constraints/ConstraintsContainerDms.h>
+#include <ct/optcon/dms/constraints/ConstraintDiscretizer.h>
+
+#include <ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h>
+#include <ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+
+#include <ct/optcon/nlp/Nlp.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class sets up the DMS problem
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DmsProblem : public tpl::Nlp<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+
+ /**
+ * @brief Custom constructor, sets up the objects needed for the Dms
+ * algorithm depending on the settings
+ *
+ * @param[in] settings The Dms settings
+ * @param[in] systemPtrs The non linear systems
+ * @param[in] linearPtrs The linearized systems
+ * @param[in] costPtrs The cost function
+ * @param[in] boxConstraints The box constraints
+ * @param[in] generaConstraints The general constraints
+ * @param[in] x0 The initial state
+ */
+ DmsProblem(DmsSettings settings,
+ std::vector<typename OptConProblem_t::DynamicsPtr_t> systemPtrs,
+ std::vector<typename OptConProblem_t::LinearPtr_t> linearPtrs,
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t> costPtrs,
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> boxConstraints,
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> generalConstraints,
+ const state_vector_t& x0)
+ : settings_(settings)
+ {
+ assert(systemPtrs.size() == settings_.N_);
+ assert(linearPtrs.size() == settings_.N_);
+ assert(costPtrs.size() == settings_.N_);
+ settings_.parametersOk();
+
+ timeGrid_ = std::shared_ptr<tpl::TimeGrid<SCALAR>>(new tpl::TimeGrid<SCALAR>(settings.N_, settings.T_));
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ controlSpliner_ = std::shared_ptr<ZeroOrderHoldSpliner<control_vector_t, SCALAR>>(
+ new ZeroOrderHoldSpliner<control_vector_t, SCALAR>(timeGrid_));
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ controlSpliner_ = std::shared_ptr<LinearSpliner<control_vector_t, SCALAR>>(
+ new LinearSpliner<control_vector_t, SCALAR>(timeGrid_));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown spline type"));
+ }
+
+
+ size_t wLength = (settings.N_ + 1) * (STATE_DIM + CONTROL_DIM);
+ if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ {
+ throw std::runtime_error("Currently we do not support adaptive time gridding");
+ // wLength += settings_.N_;
+ }
+
+ this->optVariables_ = std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>(wLength, settings));
+
+ optVariablesDms_ = std::static_pointer_cast<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>>(this->optVariables_);
+
+ if (boxConstraints.size() > 0 || generalConstraints.size() > 0)
+ discretizedConstraints_ = std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>(
+ optVariablesDms_, controlSpliner_, timeGrid_, settings_.N_));
+
+ if (boxConstraints.size() > 0)
+ discretizedConstraints_->setBoxConstraints(boxConstraints.front());
+
+ if (generalConstraints.size() > 0)
+ discretizedConstraints_->setGeneralConstraints(generalConstraints.front());
+
+
+ for (size_t shotIdx = 0; shotIdx < settings_.N_; shotIdx++)
+ {
+ std::shared_ptr<ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>> newController(
+ new ControllerDms<STATE_DIM, CONTROL_DIM, SCALAR>(controlSpliner_, shotIdx));
+ systemPtrs[shotIdx]->setController(newController);
+ linearPtrs[shotIdx]->setController(newController);
+
+ size_t nIntegrationSteps =
+ (timeGrid_->getShotEndTime(shotIdx) - timeGrid_->getShotStartTime(shotIdx)) / settings_.dt_sim_ + 0.5;
+
+ shotContainers_.push_back(std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>(systemPtrs[shotIdx], linearPtrs[shotIdx],
+ costPtrs[shotIdx], optVariablesDms_, controlSpliner_, timeGrid_, shotIdx, settings_,
+ nIntegrationSteps)));
+ }
+
+ switch (settings_.costEvaluationType_)
+ {
+ case DmsSettings::SIMPLE:
+ {
+ this->costEvaluator_ = std::shared_ptr<CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>(
+ costPtrs.front(), optVariablesDms_, timeGrid_, settings_));
+ break;
+ }
+ case DmsSettings::FULL:
+ {
+ this->costEvaluator_ = std::shared_ptr<CostEvaluatorFull<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new CostEvaluatorFull<STATE_DIM, CONTROL_DIM, SCALAR>(
+ costPtrs.front(), optVariablesDms_, controlSpliner_, shotContainers_, settings_));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown cost evaluation type"));
+ }
+
+ this->constraints_ = std::shared_ptr<ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, SCALAR>(
+ optVariablesDms_, timeGrid_, shotContainers_, discretizedConstraints_, x0, settings_));
+
+ this->optVariables_->resizeConstraintVars(this->getConstraintsCount());
+ }
+
+ void generateAndCompileCode(
+ std::vector<std::shared_ptr<core::ControlledSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>> systemPtrs,
+ std::vector<std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>> linearPtrs,
+ std::vector<std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>>
+ costPtrs,
+ std::vector<std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>>
+ boxConstraints,
+ std::vector<std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>>
+ generalConstraints,
+ const ct::core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x0)
+ {
+ typedef ct::core::ADCGScalar ScalarCG;
+ size_t wLength = (settings_.N_ + 1) * (STATE_DIM + CONTROL_DIM);
+
+ std::shared_ptr<tpl::TimeGrid<ScalarCG>> timeGrid(
+ new tpl::TimeGrid<ScalarCG>(settings_.N_, ScalarCG(settings_.T_)));
+
+ std::shared_ptr<SplinerBase<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>> controlSpliner;
+
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ controlSpliner =
+ std::shared_ptr<ZeroOrderHoldSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>>(
+ new ZeroOrderHoldSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>(timeGrid));
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ controlSpliner =
+ std::shared_ptr<LinearSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>>(
+ new LinearSpliner<ct::core::ControlVector<CONTROL_DIM, ScalarCG>, ScalarCG>(timeGrid));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown spline type"));
+ }
+
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, ScalarCG>> optVariablesDms(
+ new OptVectorDms<STATE_DIM, CONTROL_DIM, ScalarCG>(wLength, settings_));
+
+
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, ScalarCG>> discretizedConstraints;
+ if (boxConstraints.size() > 0 || generalConstraints.size() > 0)
+ discretizedConstraints = std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ optVariablesDms, controlSpliner, timeGrid, settings_.N_));
+
+ if (boxConstraints.size() > 0)
+ discretizedConstraints->setBoxConstraints(boxConstraints.front());
+
+ if (generalConstraints.size() > 0)
+ discretizedConstraints->setGeneralConstraints(generalConstraints.front());
+
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, ScalarCG>>> shotContainers;
+ for (size_t shotIdx = 0; shotIdx < settings_.N_; shotIdx++)
+ {
+ std::shared_ptr<ControllerDms<STATE_DIM, CONTROL_DIM, ScalarCG>> newController(
+ new ControllerDms<STATE_DIM, CONTROL_DIM, ScalarCG>(controlSpliner, shotIdx));
+ systemPtrs[shotIdx]->setController(newController);
+ linearPtrs[shotIdx]->setController(newController);
+
+ size_t nIntegrationSteps =
+ (timeGrid_->getShotEndTime(shotIdx) - timeGrid_->getShotStartTime(shotIdx)) / settings_.dt_sim_ + 0.5;
+
+ shotContainers.push_back(std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new ShotContainer<STATE_DIM, CONTROL_DIM, ScalarCG>(systemPtrs[shotIdx], linearPtrs[shotIdx],
+ costPtrs[shotIdx], optVariablesDms, controlSpliner, timeGrid, shotIdx, settings_,
+ nIntegrationSteps)));
+ }
+
+ std::shared_ptr<tpl::DiscreteCostEvaluatorBase<ScalarCG>> costEvaluator;
+
+ switch (settings_.costEvaluationType_)
+ {
+ case DmsSettings::SIMPLE:
+ {
+ costEvaluator = std::shared_ptr<CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ costPtrs.front(), optVariablesDms, timeGrid, settings_));
+ break;
+ }
+ case DmsSettings::FULL:
+ {
+ costEvaluator = std::shared_ptr<CostEvaluatorFull<STATE_DIM, CONTROL_DIM, ScalarCG>>(
+ new CostEvaluatorFull<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ costPtrs.front(), optVariablesDms, controlSpliner, shotContainers, settings_));
+ break;
+ }
+ default:
+ throw(std::runtime_error("Unknown cost evaluation type"));
+ }
+
+ optVariablesDms->resizeConstraintVars(this->getConstraintsCount());
+
+ std::shared_ptr<ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, ScalarCG>> constraints(
+ new ConstraintsContainerDms<STATE_DIM, CONTROL_DIM, ScalarCG>(
+ optVariablesDms, timeGrid, shotContainers, discretizedConstraints, x0, settings_));
+
+ if (settings_.solverSettings_.useGeneratedCostGradient_)
+ {
+ std::function<Eigen::Matrix<ScalarCG, 1, 1>(const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>&)> fCost = [&](
+ const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>& xOpt) {
+ optVariablesDms->setOptimizationVars(xOpt);
+
+ controlSpliner->computeSpline(optVariablesDms->getOptimizedInputs().toImplementation());
+ for (auto shotContainer : shotContainers)
+ shotContainer->reset();
+
+ Eigen::Matrix<ScalarCG, 1, 1> out;
+ out << costEvaluator->eval();
+ return out;
+ };
+
+ settings_.cppadSettings_.createJacobian_ = true;
+
+ this->costCodegen_ = std::shared_ptr<ct::core::DerivativesCppadJIT<-1, 1>>(
+ new ct::core::DerivativesCppadJIT<-1, 1>(fCost, this->getVarCount()));
+ this->costCodegen_->compileJIT(settings_.cppadSettings_, "dmsCostFunction");
+ }
+
+ if (settings_.solverSettings_.useGeneratedConstraintJacobian_)
+ {
+ std::function<Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>(const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>&)>
+ fConstraints = [&](const Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1>& xOpt) {
+ optVariablesDms->setOptimizationVars(xOpt);
+
+ controlSpliner->computeSpline(optVariablesDms->getOptimizedInputs().toImplementation());
+ for (auto shotContainer : shotContainers)
+ shotContainer->reset();
+
+
+ Eigen::Matrix<ScalarCG, Eigen::Dynamic, 1> out(
+ this->getConstraintsCount()); // out.resize(this->getConstraintsCount, 1);
+ constraints->evalConstraints(out);
+ return out;
+ };
+
+ settings_.cppadSettings_.createJacobian_ = false;
+
+ this->constraintsCodegen_ =
+ std::shared_ptr<ct::core::DerivativesCppadJIT<-1, -1>>(new ct::core::DerivativesCppadJIT<-1, -1>(
+ fConstraints, this->getVarCount(), this->getConstraintsCount()));
+ this->constraintsCodegen_->compileJIT(settings_.cppadSettings_, "dmsConstraints");
+ }
+ }
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~DmsProblem() {}
+ virtual void updateProblem() override
+ {
+ controlSpliner_->computeSpline(optVariablesDms_->getOptimizedInputs().toImplementation());
+ for (auto shotContainer : shotContainers_)
+ shotContainer->reset();
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // // std::cout << "optVariablesDms_->getOptimizedTimeSegments(): " << optVariablesDms_->getOptimizedTimeSegments().transpose() << std::endl;
+ // timeGrid_->updateTimeGrid(optVariablesDms_->getOptimizedTimeSegments());
+ // }
+ }
+
+ /**
+ * @brief Updates the settings
+ *
+ * @param[in] settings New dms settings
+ */
+ void configure(const DmsSettings& settings) { settings_ = settings; }
+ /**
+ * @brief Retrieves the solution state trajectory at every shot
+ *
+ * @return The state solution trajectory
+ */
+ const state_vector_array_t& getStateSolution() { return optVariablesDms_->getOptimizedStates(); }
+ /**
+ * @brief Retrieves the solution control trajectory at every shot
+ *
+ * @return The control solution trajectory
+ */
+ const control_vector_array_t& getInputSolution() { return optVariablesDms_->getOptimizedInputs(); }
+ /**
+ * @brief Retrieves the solution time trajectory at every shot
+ *
+ * @return The time solution trajectory
+ */
+ const time_array_t& getTimeSolution() { return timeGrid_->toImplementation(); }
+ /**
+ * @brief Retrieves a dense state solution trajectory
+ *
+ * @return The dense state solution trajectory
+ */
+ const state_vector_array_t& getStateTrajectory()
+ {
+ // stateSolutionDense_.clear();
+ // for(auto shotContainer : shotContainers_)
+ // shotContainer->integrateShot();
+
+ // stateSolutionDense_.push_back(shotContainers_.front()->getXHistory().front());
+ // for(auto shotContainer : shotContainers_)
+ // {
+ // state_vector_array_t x_traj = shotContainer->getXHistory();
+ // for(size_t j = 1; j < x_traj.size(); ++j)
+ // stateSolutionDense_.push_back(x_traj[j]);
+ // }
+ return optVariablesDms_->getOptimizedStates();
+ ;
+ }
+
+ /**
+ * @brief Retrieves a dense input solution trajectory
+ *
+ * @return The dense control solution trajectory
+ */
+ const control_vector_array_t& getInputTrajectory()
+ {
+ // inputSolutionDense_.clear();
+ // for(auto shotContainer : shotContainers_)
+ // shotContainer->integrateShot();
+
+ // inputSolutionDense_.push_back(shotContainers_.front()->getUHistory().front());
+ // for(auto shotContainer : shotContainers_)
+ // {
+ // control_vector_array_t u_traj = shotContainer->getUHistory();
+ // for(size_t j = 1; j < u_traj.size(); ++j)
+ // inputSolutionDense_.push_back(u_traj[j]);
+ // }
+ // return inputSolutionDense_;
+ return optVariablesDms_->getOptimizedInputs();
+ }
+
+ /**
+ * @brief Retrieves a dense time solution trajectory
+ *
+ * @return The dense time solution trajectory
+ */
+ const time_array_t& getTimeArray()
+ {
+ // timeSolutionDense_.clear();
+ // for(auto shotContainer : shotContainers_)
+ // shotContainer->integrateShot();
+
+ // timeSolutionDense_.push_back(shotContainers_.front()->getTHistory().front());
+ // for(auto shotContainer : shotContainers_)
+ // {
+ // time_array_t t_traj = shotContainer->getTHistory();
+ // for(size_t j = 1; j < t_traj.size(); ++j)
+ // timeSolutionDense_.push_back(t_traj[j]);
+ // }
+ // return timeSolutionDense_;
+ return timeGrid_->toImplementation();
+ }
+
+ /**
+ * @brief Sets the initial guess of the optimization
+ *
+ * @param[in] x_init_guess The state trajectory initial guess
+ * @param[in] u_init_guess The control trajectory initial guess
+ * @param[in] t_init_guess The time trajectory initial guess
+ */
+ void setInitialGuess(const state_vector_array_t& x_init_guess,
+ const control_vector_array_t& u_init_guess,
+ const time_array_t& t_init_guess = time_array_t(0.0))
+ {
+ optVariablesDms_->setInitGuess(x_init_guess, u_init_guess);
+ }
+
+ /**
+ * @brief Return the timehorizon of the problem
+ *
+ * @return The time horizon
+ */
+ const core::Time getTimeHorizon() const { return timeGrid_->getTimeHorizon(); }
+ /**
+ * @brief Updates the timehorizon
+ *
+ * @param[in] tf The new time horizon
+ */
+ void changeTimeHorizon(const SCALAR tf) { timeGrid_->changeTimeHorizon(tf); }
+ /**
+ * @brief Updates the initial state
+ *
+ * @param[in] x0 The new inital state
+ */
+ void changeInitialState(const state_vector_t& x0)
+ {
+ // constraints_->changeInitialConstraint(x0);
+ optVariablesDms_->changeInitialState(x0);
+ }
+
+ /**
+ * @brief Prints the solution trajectories
+ */
+ void printSolution() { optVariablesDms_->printoutSolution(); }
+private:
+ DmsSettings settings_;
+
+ std::shared_ptr<ConstraintDiscretizer<STATE_DIM, CONTROL_DIM, SCALAR>> discretizedConstraints_;
+
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> optVariablesDms_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+
+ state_vector_array_t stateSolutionDense_;
+ control_vector_array_t inputSolutionDense_;
+ time_array_t timeSolutionDense_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsSettings.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSettings.h
new file mode 100644
index 0000000..701bfe4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSettings.h
@@ -0,0 +1,166 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <map>
+
+#include <boost/property_tree/info_parser.hpp>
+
+#include <ct/optcon/nlp/solver/NlpSolverSettings.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Defines the DMS settings
+ */
+class DmsSettings
+{
+public:
+ typedef enum SplineType { ZERO_ORDER_HOLD = 0, PIECEWISE_LINEAR = 1, num_types_splining } SplineType_t;
+ typedef enum ObjectiveType { KEEP_TIME_AND_GRID = 0, OPTIMIZE_GRID = 1, num_types_objectives } ObjectiveType_t;
+ typedef enum IntegrationType { EULER = 0, RK4 = 1, RK5 = 2, num_types_integration } IntegrationType_t;
+ typedef enum CostEvaluationType { SIMPLE = 0, FULL = 1, num_types_costevaluation } CostEvaluationType_t;
+
+ /**
+ * @brief Default constructor. Sets some default DMS settings. Note
+ * that the optimal settings are strongly dependent on the
+ * problem and it is highly recommended to use custom settings
+ */
+ DmsSettings()
+ : N_(30),
+ T_(5),
+ nThreads_(1),
+ splineType_(ZERO_ORDER_HOLD),
+ costEvaluationType_(SIMPLE),
+ objectiveType_(KEEP_TIME_AND_GRID),
+ h_min_(0.1),
+ integrationType_(RK4),
+ dt_sim_(0.01),
+ absErrTol_(1e-10),
+ relErrTol_(1e-10)
+ {
+ }
+
+ size_t N_; // the number of shots
+ double T_; // the time horizon
+ size_t nThreads_; // number of threads
+ SplineType_t splineType_; // spline interpolation type between the nodes
+ CostEvaluationType_t costEvaluationType_; // the the of costevaluator
+ ObjectiveType_t objectiveType_; // Timegrid optimization on(expensive) or off?
+ double h_min_; // minimum admissible distance between two nodes in [sec]
+ IntegrationType_t integrationType_; // the integration type between the nodes
+ double dt_sim_; // and the corresponding integration timestep
+ double absErrTol_; // the absolute and relative integrator tolerances when using RK5
+ double relErrTol_;
+
+ NlpSolverSettings solverSettings_;
+ ct::core::DerivativesCppadSettings cppadSettings_;
+
+ void print()
+ {
+ std::cout << "=============================================================" << std::endl;
+ std::cout << "\tMultiple Shooting Settings: " << std::endl;
+ std::cout << "=============================================================" << std::endl;
+
+ std::cout << "Shooting intervals N : " << N_ << std::endl;
+ std::cout << "Total Time horizon: " << T_ << "s" << std::endl;
+ std::cout << "Number of threads: " << nThreads_ << std::endl;
+ std::cout << "Splinetype: " << splineToString[splineType_] << std::endl;
+ std::cout << "Cost eval: " << costEvalToString[costEvaluationType_] << std::endl;
+ std::cout << "Objective type: " << objTypeToString[objectiveType_] << std::endl;
+ std::cout << "Integration type: " << integratorToString[integrationType_] << std::endl;
+ std::cout << "Simulation timestep dt_sim: " << dt_sim_ << std::endl;
+
+ std::cout << "=============================================================" << std::endl;
+
+ solverSettings_.print();
+ }
+
+ bool parametersOk() const
+ {
+ if (N_ < 1 || N_ > 1000)
+ return false;
+
+ if (T_ <= 0.0)
+ return false;
+
+ if (nThreads_ < 1 || nThreads_ > 32)
+ return false;
+
+ if (splineType_ < 0 || !(splineType_ < SplineType_t::num_types_splining))
+ return false;
+
+ if (costEvaluationType_ < 0 || !(costEvaluationType_ < CostEvaluationType_t::num_types_costevaluation))
+ return false;
+
+ if (objectiveType_ < 0 || !(objectiveType_ < ObjectiveType_t::num_types_objectives))
+ return false;
+
+ if (h_min_ < 0.01 || h_min_ > T_)
+ return false;
+
+ if (integrationType_ < 0 || !(integrationType_ < IntegrationType_t::num_types_integration))
+ return false;
+
+ if (dt_sim_ <= 0.0 || dt_sim_ > 100.0)
+ return false;
+
+ if (absErrTol_ <= 1e-20 || absErrTol_ > 1.0)
+ return false;
+
+ if (relErrTol_ <= 1e-20 || relErrTol_ > 1.0)
+ return false;
+
+ return solverSettings_.parametersOk();
+ }
+
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "dms")
+ {
+ if (verbose)
+ std::cout << "Trying to load DMS config from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ N_ = pt.get<unsigned int>(ns + ".N");
+ T_ = pt.get<double>(ns + ".T");
+ nThreads_ = pt.get<unsigned int>(ns + ".nThreads");
+ splineType_ = (SplineType_t)pt.get<unsigned int>(ns + ".InterpolationType");
+ costEvaluationType_ = (CostEvaluationType_t)pt.get<unsigned int>(ns + ".CostEvaluationType");
+ objectiveType_ = (ObjectiveType_t)pt.get<unsigned int>(ns + ".ObjectiveType");
+ h_min_ = pt.get<double>(ns + ".h_min");
+
+ integrationType_ = (IntegrationType_t)pt.get<unsigned int>(ns + ".IntegrationType");
+ dt_sim_ = pt.get<double>(ns + ".dt_sim");
+ absErrTol_ = pt.get<double>(ns + ".AbsErrTol");
+ relErrTol_ = pt.get<double>(ns + ".RelErrTol");
+
+ solverSettings_.load(filename, verbose, ns + ".solver"); // todo bring in again
+ cppadSettings_.load(filename, verbose, ns + ".cppad");
+
+ if (verbose)
+ {
+ std::cout << "Loaded DMS config from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+
+private:
+ std::map<SplineType, std::string> splineToString = {
+ {ZERO_ORDER_HOLD, "Zero order hold"}, {PIECEWISE_LINEAR, "Piecewise Linear"}};
+ std::map<ObjectiveType, std::string> objTypeToString = {
+ {KEEP_TIME_AND_GRID, "Timegrid fix"}, {OPTIMIZE_GRID, "Timegrid Optimization On"}};
+ std::map<IntegrationType, std::string> integratorToString = {
+ {EULER, "Euler"}, {RK4, "Runge-Kutta 4th order"}, {RK5, "RK5 adaptive step size"}};
+ std::map<CostEvaluationType, std::string> costEvalToString = {{SIMPLE, "Simple"}, {FULL, "Full"}};
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/DmsSolver.h b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSolver.h
new file mode 100644
index 0000000..70160ae
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/DmsSolver.h
@@ -0,0 +1,314 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/problem/OptConProblem.h>
+#include <ct/optcon/solver/OptConSolver.h>
+
+#include <ct/optcon/dms/dms_core/DmsProblem.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+
+#include <ct/optcon/nlp/Nlp>
+
+#include <memory>
+
+namespace ct {
+namespace optcon {
+
+/** @defgroup DMS DMS
+ *
+ * @brief The direct multiple shooting module
+ */
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The DMS policy used as a solution container
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+struct DmsPolicy
+{
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ state_vector_array_t xSolution_;
+ control_vector_array_t uSolution_;
+ time_array_t tSolution_;
+};
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Class to solve a specfic DMS problem
+ *
+ * An example employing different DMS solvers is given in unit test \ref oscDMSTest.cpp
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DmsSolver : public OptConSolver<DmsSolver<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsPolicy<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsSettings,
+ STATE_DIM,
+ CONTROL_DIM,
+ SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef OptConSolver<DmsSolver<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsPolicy<STATE_DIM, CONTROL_DIM, SCALAR>,
+ DmsSettings,
+ STATE_DIM,
+ CONTROL_DIM>
+ Base;
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef DmsPolicy<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+ /**
+ * @brief Custom constructor, converts the optcon problem to a DMS problem
+ *
+ * @param[in] problem The optimal control problem
+ * @param[in] settingsDms The dms settings
+ */
+ DmsSolver(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> problem, DmsSettings settingsDms)
+ : nlpSolver_(nullptr), settings_(settingsDms)
+ {
+ // Create system, linearsystem and costfunction instances
+ this->setProblem(problem);
+
+ dmsProblem_ = std::shared_ptr<DmsProblem<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new DmsProblem<STATE_DIM, CONTROL_DIM, SCALAR>(settingsDms, this->systems_, this->linearSystems_,
+ this->costFunctions_, this->boxConstraints_, this->generalConstraints_, x0_));
+
+ // SNOPT only works for the double type
+ if (settingsDms.solverSettings_.solverType_ == NlpSolverSettings::SNOPT)
+ nlpSolver_ = std::shared_ptr<SnoptSolver>(new SnoptSolver(dmsProblem_, settingsDms.solverSettings_));
+ else if (settingsDms.solverSettings_.solverType_ == NlpSolverSettings::IPOPT)
+ nlpSolver_ = std::shared_ptr<IpoptSolver>(new IpoptSolver(dmsProblem_, settingsDms.solverSettings_));
+ else
+ std::cout << "Unknown solver type... Exiting" << std::endl;
+
+ configure(settingsDms);
+ }
+
+ virtual void generateAndCompileCode(const OptConProblem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>& problemCG,
+ const ct::core::DerivativesCppadSettings& settings) override
+ {
+ // Create system, linearsystem and costfunction instances
+ typedef std::shared_ptr<core::ControlledSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>> SysPtrCG;
+ typedef std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>> LinearSysPtrCG;
+ typedef std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>> CostPtrCG;
+ typedef std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>>
+ ConstraintCG;
+ ct::core::StateVector<STATE_DIM, ct::core::ADCGScalar> x0CG = problemCG.getInitialState();
+
+ std::vector<SysPtrCG> systemsCG;
+ std::vector<LinearSysPtrCG> linearSystemsCG;
+ std::vector<CostPtrCG> costFunctionsCG;
+ std::vector<ConstraintCG> boxConstraintsCG;
+ std::vector<ConstraintCG> generalConstraintsCG;
+
+ for (size_t i = 0; i < settings_.N_; i++)
+ {
+ systemsCG.push_back(SysPtrCG(problemCG.getNonlinearSystem()->clone()));
+ linearSystemsCG.push_back(LinearSysPtrCG(problemCG.getLinearSystem()->clone()));
+ costFunctionsCG.push_back(CostPtrCG(problemCG.getCostFunction()->clone()));
+ }
+
+ if (problemCG.getBoxConstraints())
+ boxConstraintsCG.push_back(ConstraintCG(problemCG.getBoxConstraints()->clone()));
+
+ if (problemCG.getGeneralConstraints())
+ generalConstraintsCG.push_back(ConstraintCG(problemCG.getGeneralConstraints()->clone()));
+
+ dmsProblem_->generateAndCompileCode(
+ systemsCG, linearSystemsCG, costFunctionsCG, boxConstraintsCG, generalConstraintsCG, x0CG);
+ }
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~DmsSolver() {}
+ virtual void configure(const DmsSettings& settings) override
+ {
+ dmsProblem_->configure(settings);
+ dmsProblem_->changeTimeHorizon(tf_);
+ dmsProblem_->changeInitialState(x0_);
+ }
+
+ virtual bool solve() override
+ {
+ if (!nlpSolver_->isInitialized())
+ nlpSolver_->configure(settings_.solverSettings_);
+
+ return nlpSolver_->solve();
+ }
+
+ virtual const Policy_t& getSolution() override
+ {
+ policy_.xSolution_ = dmsProblem_->getStateSolution();
+ policy_.uSolution_ = dmsProblem_->getInputSolution();
+ policy_.tSolution_ = dmsProblem_->getTimeSolution();
+ return policy_;
+ }
+
+ virtual const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const override
+ {
+ return core::StateTrajectory<STATE_DIM, SCALAR>(dmsProblem_->getTimeArray(), dmsProblem_->getStateTrajectory());
+ }
+
+ virtual const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const override
+ {
+ return core::ControlTrajectory<CONTROL_DIM, SCALAR>(
+ dmsProblem_->getTimeArray(), dmsProblem_->getInputTrajectory());
+ }
+
+ virtual const core::tpl::TimeArray<SCALAR>& getTimeArray() const override { return dmsProblem_->getTimeArray(); }
+ virtual void setInitialGuess(const Policy_t& initialGuess) override
+ {
+ dmsProblem_->setInitialGuess(initialGuess.xSolution_, initialGuess.uSolution_);
+ }
+
+ virtual SCALAR getTimeHorizon() const override { return dmsProblem_->getTimeHorizon(); }
+ virtual void changeTimeHorizon(const SCALAR& tf) override
+ {
+ tf_ = tf;
+ if (dmsProblem_)
+ dmsProblem_->changeTimeHorizon(tf);
+ }
+
+ virtual void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0) override
+ {
+ x0_ = x0;
+ if (dmsProblem_)
+ dmsProblem_->changeInitialState(x0);
+ }
+
+ virtual void changeCostFunction(const typename Base::OptConProblem_t::CostFunctionPtr_t& cf) override
+ {
+ this->getCostFunctionInstances().resize(settings_.N_);
+ if (cf)
+ for (size_t i = 0; i < settings_.N_; i++)
+ this->getCostFunctionInstances()[i] = typename Base::OptConProblem_t::CostFunctionPtr_t(cf->clone());
+ }
+
+ virtual void changeNonlinearSystem(const typename Base::OptConProblem_t::DynamicsPtr_t& dyn) override
+ {
+ this->getNonlinearSystemsInstances().resize(settings_.N_);
+
+ if (dyn)
+ for (size_t i = 0; i < settings_.N_; i++)
+ this->getNonlinearSystemsInstances()[i] = typename Base::OptConProblem_t::DynamicsPtr_t(dyn->clone());
+ }
+
+ virtual void changeLinearSystem(const typename Base::OptConProblem_t::LinearPtr_t& lin) override
+ {
+ this->getLinearSystemsInstances().resize(settings_.N_);
+
+ if (lin)
+ for (size_t i = 0; i < settings_.N_; i++)
+ this->getLinearSystemsInstances()[i] = typename Base::OptConProblem_t::LinearPtr_t(lin->clone());
+ }
+
+ virtual void changeBoxConstraints(const typename Base::OptConProblem_t::ConstraintPtr_t con) override
+ {
+ this->getBoxConstraintsInstances().push_back(typename Base::OptConProblem_t::ConstraintPtr_t(con->clone()));
+ }
+
+ virtual void changeGeneralConstraints(const typename Base::OptConProblem_t::ConstraintPtr_t con) override
+ {
+ this->getGeneralConstraintsInstances().push_back(typename Base::OptConProblem_t::ConstraintPtr_t(con->clone()));
+ }
+
+ /**
+ * @brief Prints out the solution trajectories of the DMS problem
+ */
+ void printSolution() { dmsProblem_->printSolution(); }
+ virtual std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() override
+ {
+ return systems_;
+ }
+ virtual const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const override
+ {
+ return systems_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() override
+ {
+ return linearSystems_;
+ }
+ virtual const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const override
+ {
+ return linearSystems_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() override
+ {
+ return costFunctions_;
+ }
+ virtual const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const override
+ {
+ return costFunctions_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() override
+ {
+ return boxConstraints_;
+ }
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const override
+ {
+ return boxConstraints_;
+ }
+
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() override
+ {
+ return generalConstraints_;
+ }
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances()
+ const override
+ {
+ return generalConstraints_;
+ }
+
+
+private:
+ std::shared_ptr<DmsProblem<STATE_DIM, CONTROL_DIM, SCALAR>> dmsProblem_; /*!<The dms problem*/
+ std::shared_ptr<tpl::NlpSolver<SCALAR>> nlpSolver_; /*!<The nlp solver for solving the dmsproblem*/
+ DmsSettings settings_; /*!<The dms settings*/
+
+ Policy_t policy_; /*!<The solution container*/
+
+ state_vector_t x0_; /*!<The initial state for the optimization*/
+ SCALAR tf_; /*!<The timehorizon of the problem*/
+
+ std::vector<typename OptConProblem_t::DynamicsPtr_t> systems_;
+ std::vector<typename OptConProblem_t::LinearPtr_t> linearSystems_;
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t> costFunctions_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> boxConstraints_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> generalConstraints_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/OptVectorDms.h b/ct_optcon/include/ct/optcon/dms/dms_core/OptVectorDms.h
new file mode 100644
index 0000000..3ec4aa7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/OptVectorDms.h
@@ -0,0 +1,214 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+
+#include <Eigen/Core>
+#include <map>
+
+#include "TimeGrid.h"
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/spline/SplinerBase.h>
+#include <ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h>
+#include <ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h>
+#include <ct/optcon/nlp/OptVector.h>
+#include <ct/optcon/dms/dms_core/DmsSettings.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class is a wrapper around the NLP Optvector. It wraps the
+ * Vectors from the NLP solvers into state, control and time
+ * trajectories
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class OptVectorDms : public tpl::OptVector<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+ typedef tpl::OptVector<SCALAR> Base;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] n The number of optimization variables
+ * @param[in] settings The dms settings
+ */
+ OptVectorDms(size_t n, const DmsSettings& settings);
+
+ OptVectorDms(const OptVectorDms& arg) = delete;
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~OptVectorDms() {}
+ /**
+ * @brief Returns the optimized state for a specific shot
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The optimized state
+ */
+ state_vector_t getOptimizedState(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the optimized control input for a specific shot
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The optimized control input
+ */
+ control_vector_t getOptimizedControl(const size_t pairNum) const;
+
+ /**
+ * @brief Return the optimized time segment for a specific shot
+ *
+ * @param[in] pairNum The pair number
+ *
+ * @return The optimized time segment.
+ */
+ SCALAR getOptimizedTimeSegment(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the optimized state for all shots
+ *
+ * @return The optimized states.
+ */
+ const state_vector_array_t& getOptimizedStates();
+
+ /**
+ * @brief Returns the optimized control inputs for all shots
+ *
+ * @return The optimized control inputs.
+ */
+ const control_vector_array_t& getOptimizedInputs();
+
+ /**
+ * @brief Returns the optimized time segments for all shots
+ *
+ * @return The optimized time segments.
+ */
+ const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& getOptimizedTimeSegments();
+
+ /**
+ * @brief Returns the starting index for the state at shot pairNum
+ * inside the optimization vector
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The state index.
+ */
+ size_t getStateIndex(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the starting index for the control input at shot pairNum
+ * inside the optimization vector
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The state index.
+ */
+ size_t getControlIndex(const size_t pairNum) const;
+
+ /**
+ * @brief Returns the starting index for the time segment input at shot pairNum
+ * inside the optimization vector
+ *
+ * @param[in] pairNum The shot number
+ *
+ * @return The state index.
+ */
+ size_t getTimeSegmentIndex(const size_t pairNum) const;
+
+
+ /**
+ * @brief Sets an initial guess for the optimal solution. The optimal
+ * solution is set as a linear interpolation between inital
+ * state x0 and final state xf. The initial guess for the
+ * control input is assumed to be constant and equal to u0
+ *
+ * @param[in] x0 The initial state
+ * @param[in] x_f The final state
+ * @param[in] u0 The control input
+ */
+ void setInitGuess(const state_vector_t& x0, const state_vector_t& x_f, const control_vector_t& u0);
+
+ /**
+ * @brief Sets an initial guess for the optimal solution.
+ *
+ * @param[in] x_init Initial guess for the state trajectory
+ * @param[in] u_init Initial guess for the control trajectory
+ */
+ void setInitGuess(const state_vector_array_t& x_init, const control_vector_array_t& u_init);
+
+ /**
+ * @brief Updates the initial state
+ *
+ * @param[in] x0 The new initial state
+ */
+ void changeInitialState(const state_vector_t& x0);
+
+ /**
+ * @brief Updates the final state
+ *
+ * @param[in] xF The new final state
+ */
+ void changeDesiredState(const state_vector_t& xF);
+
+ /**
+ * @brief Sets bounds on the time segment variables
+ */
+ void setLowerTimeSegmentBounds();
+
+ /**
+ * @brief Returns the number of pairs
+ *
+ * @return Number of pairs
+ */
+ size_t numPairs() { return numPairs_; }
+ /**
+ * @brief Prints out the solution trajectories
+ */
+ void printoutSolution();
+
+private:
+ DmsSettings settings_;
+
+ size_t numPairs_;
+ /* maps the number of a "pair" to the index in w where ... */
+ std::map<size_t, size_t> pairNumToStateIdx_; /* ... its state starts */
+ std::map<size_t, size_t> pairNumToControlIdx_; /* ... its control starts */
+ std::map<size_t, size_t>
+ shotNumToShotDurationIdx_; /* its shot time starts is in w (last element doesn't have one) */
+
+ state_vector_array_t stateSolution_;
+ control_vector_array_t inputSolution_;
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> optimizedTimeSegments_;
+};
+
+} // namespace optcon
+} // namespace ct
+
+#include "implementation/OptVectorDms-impl.h"
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/RKnDerivatives.h b/ct_optcon/include/ct/optcon/dms/dms_core/RKnDerivatives.h
new file mode 100644
index 0000000..f74afa2
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/RKnDerivatives.h
@@ -0,0 +1,426 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ControllerDms.h>
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class implements analytical sensitivity generation for the
+ * euler and rk4 integration scheme
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+class RKnDerivatives
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::control_matrix_t control_matrix_t;
+ typedef typename DIMENSIONS::state_control_matrix_t state_control_matrix_t;
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ RKnDerivatives() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] controlSpliner The control spliner
+ * @param[in] shotIdx The shot number
+ * @param[in] settings The dms settings
+ */
+ RKnDerivatives(std::shared_ptr<SplinerBase<control_vector_t>> controlSpliner,
+ size_t shotIdx,
+ const DmsSettings& settings)
+ : shotIdx_(shotIdx),
+ settings_(settings),
+ controlSpliner_(controlSpliner),
+ x_log_(nullptr),
+ u_log_(nullptr),
+ t_log_(nullptr),
+ A_log_(nullptr),
+ B_log_(nullptr),
+ dXdSi_history_(state_matrix_array_t(0)),
+ dXdQi_history_(state_control_matrix_array_t(0)),
+ dXdQip1_history_(state_control_matrix_array_t(0)),
+ dXdHi_history_(state_vector_array_t(0))
+ {
+ }
+
+ void setLogs(std::shared_ptr<state_vector_array_t> x_log,
+ std::shared_ptr<control_vector_array_t> u_log,
+ std::shared_ptr<time_array_t> t_log,
+ std::shared_ptr<state_matrix_array_t> A_log,
+ std::shared_ptr<state_control_matrix_array_t> B_log)
+ {
+ x_log_ = x_log;
+ u_log_ = u_log;
+ t_log_ = t_log;
+ A_log_ = A_log;
+ B_log_ = B_log;
+ }
+
+
+ // compute sensitivity of all X in the trajectory w.r.t. s_i
+ void compute_dXdSi()
+ {
+ dXdSi_history_.clear();
+ dXdSi_history_.push_back(Eigen::MatrixXd::Identity(STATE_DIM, STATE_DIM));
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ size_t nSteps = A_log_->size() / 1;
+ for (size_t i = 0; i < nSteps; i++)
+ {
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+
+ state_matrix_t temp = Eigen::MatrixXd::Identity(STATE_DIM, STATE_DIM) + dt_sim * (*A_log_)[i];
+
+ dXdSi_history_.push_back(temp * dXdSi_history_.back());
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1.0;
+
+ double b1 = 1.0 / 6.0;
+ double b2 = 1.0 / 3.0;
+ double b3 = 1.0 / 3.0;
+ double b4 = 1.0 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // step 1:
+ state_matrix_t dK1dSi = (*A_log_)[i * m] * dXdSi_history_.back();
+
+ // step 2:
+ state_matrix_t dK2dSi = (*A_log_)[i * m + 1] * (dXdSi_history_.back() + h * a_21 * dK1dSi);
+
+ // step 3:
+ state_matrix_t dK3dSi = (*A_log_)[i * m + 2] * (dXdSi_history_.back() + h * a_32 * dK2dSi);
+
+ // step 4:
+ state_matrix_t dK4dSi = (*A_log_)[i * m + 3] * (dXdSi_history_.back() + h * a_43 * dK3dSi);
+
+
+ // summation:
+ state_matrix_t temp =
+ dXdSi_history_.back() + h * (b1 * dK1dSi + b2 * dK2dSi + b3 * dK3dSi + b4 * dK4dSi);
+
+ dXdSi_history_.push_back(temp);
+ }
+ break;
+ }
+ default:
+ {
+ std::cerr << "... ERROR: sensitivity calculation not implemented for this integration type. Exiting"
+ << std::endl;
+ exit(0);
+ }
+ }
+ }
+
+
+ // compute sensitivity of all X in the trajectory w.r.t. q_i
+ void compute_dXdQi()
+ {
+ dXdQi_history_.clear();
+ dXdQi_history_.push_back(state_control_matrix_t::Zero());
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ for (size_t i = 0; i < A_log_->size(); ++i)
+ {
+ control_matrix_t dSpldQ0 = controlSpliner_->splineDerivative_q_i((*t_log_)[i], shotIdx_);
+
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+
+ dXdQi_history_.push_back(dXdQi_history_.back() +
+ dt_sim * ((*A_log_)[i] * dXdQi_history_.back() + (*B_log_)[i] * dSpldQ0));
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1;
+
+ double b1 = 1.0 / 6.0;
+ double b2 = 1.0 / 3.0;
+ double b3 = 1.0 / 3.0;
+ double b4 = 1.0 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ assert(i * m < A_log_->size());
+
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // TODO: write these steps as computationally more efficient loop if they are correct.
+ // step 1:
+ state_control_matrix_t dK1dQi =
+ (*A_log_)[i * m] * dXdQi_history_.back() +
+ (*B_log_)[i * m] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m], shotIdx_);
+
+ // step 2:
+ state_control_matrix_t dK2dQi =
+ (*A_log_)[i * m + 1] * (dXdQi_history_.back() + h * a_21 * dK1dQi) +
+ (*B_log_)[i * m + 1] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m + 1], shotIdx_);
+
+ // step 3:
+ state_control_matrix_t dK3dQi =
+ (*A_log_)[i * m + 2] * (dXdQi_history_.back() + h * a_32 * dK2dQi) +
+ (*B_log_)[i * m + 2] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m + 2], shotIdx_);
+
+ // step 4:
+ state_control_matrix_t dK4dQi =
+ (*A_log_)[i * m + 3] * (dXdQi_history_.back() + h * a_43 * dK3dQi) +
+ (*B_log_)[i * m + 3] * controlSpliner_->splineDerivative_q_i((*t_log_)[i * m + 3], shotIdx_);
+
+
+ // summation
+ state_control_matrix_t temp =
+ dXdQi_history_.back() + h * (b1 * dK1dQi + b2 * dK2dQi + b3 * dK3dQi + b4 * dK4dQi);
+
+ dXdQi_history_.push_back(temp);
+ }
+ break;
+ }
+ default:
+ {
+ std::cerr << "... ERROR: sensitivity calculation not implemented for this integration type. Exiting"
+ << std::endl;
+ exit(0);
+ }
+ }
+ }
+
+
+ // compute sensitivity of all X in the trajectory w.r.t. q_(i+1)
+ void compute_dXdQip1()
+ {
+ dXdQip1_history_.clear();
+ dXdQip1_history_.push_back(state_control_matrix_t::Zero());
+
+ assert(t_log_->size() == A_log_->size() + 1);
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ for (size_t i = 0; i < A_log_->size(); ++i)
+ {
+ control_matrix_t dSpldQ1 = controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i], shotIdx_);
+
+ assert(dSpldQ1 == dSpldQ1);
+
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+
+ dXdQip1_history_.push_back(
+ dXdQip1_history_.back() +
+ dt_sim * ((*A_log_)[i] * dXdQip1_history_.back() + (*B_log_)[i] * dSpldQ1));
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1;
+
+ double b1 = 1 / 6.0;
+ double b2 = 1 / 3.0;
+ double b3 = 1 / 3.0;
+ double b4 = 1 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // TODO: write these steps as computationally more efficient loop if they are correct.
+ // step 1:
+ state_control_matrix_t dK1dQip1 =
+ (*A_log_)[i * m] * dXdQip1_history_.back() +
+ (*B_log_)[i * m] * controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m], shotIdx_);
+
+ // step 2:
+ state_control_matrix_t dK2dQip1 =
+ (*A_log_)[i * m + 1] * (dXdQip1_history_.back() + h * a_21 * dK1dQip1) +
+ (*B_log_)[i * m + 1] *
+ controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m + 1], shotIdx_);
+
+ // step 3:
+ state_control_matrix_t dK3dQip1 =
+ (*A_log_)[i * m + 2] * (dXdQip1_history_.back() + h * a_32 * dK2dQip1) +
+ (*B_log_)[i * m + 2] *
+ controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m + 2], shotIdx_);
+
+ // step 4:
+ state_control_matrix_t dK4dQip1 =
+ (*A_log_)[i * m + 3] * (dXdQip1_history_.back() + h * a_43 * dK3dQip1) +
+ (*B_log_)[i * m + 3] *
+ controlSpliner_->splineDerivative_q_iplus1((*t_log_)[i * m + 3], shotIdx_);
+
+
+ // summation
+ state_control_matrix_t temp =
+ dXdQip1_history_.back() + h * (b1 * dK1dQip1 + b2 * dK2dQip1 + b3 * dK3dQip1 + b4 * dK4dQip1);
+
+ dXdQip1_history_.push_back(temp);
+ }
+ break;
+ }
+ default:
+ {
+ throw(std::runtime_error(
+ "... ERROR: sensitivity calculation not implemented for this integration type."));
+ }
+ }
+ }
+
+ void compute_dXdHi()
+ {
+ dXdHi_history_.clear();
+ // shotContainer_->getControlledSystemPtr()->computeDynamics(x_log_->back(), t_log_->back(), dynamics);
+ dXdHi_history_.push_back(Eigen::VectorXd(STATE_DIM).setZero());
+
+ assert(t_log_->size() == A_log_->size() + 1);
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ for (size_t i = 0; i < A_log_->size(); ++i)
+ {
+ double dt_sim = (*t_log_)[i + 1] - (*t_log_)[i];
+ dXdHi_history_.push_back(
+ dXdHi_history_.back() +
+ dt_sim * ((*A_log_)[i] * dXdHi_history_.back() +
+ (*B_log_)[i] * controlSpliner_->splineDerivative_h_i((*t_log_)[i], shotIdx_)));
+ }
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ size_t m = 4;
+ size_t nCycles = A_log_->size() / m;
+
+ double a_21 = 0.5;
+ double a_32 = 0.5;
+ double a_43 = 1;
+
+ double b1 = 1 / 6.0;
+ double b2 = 1 / 3.0;
+ double b3 = 1 / 3.0;
+ double b4 = 1 / 6.0;
+
+ for (size_t i = 0; i < nCycles; i++)
+ {
+ double h = (*t_log_)[(i + 1) * m] - (*t_log_)[i * m];
+
+ // TODO: write these steps as computationally more efficient loop if they are correct.
+ // step 1:
+ state_vector_t dK1dHi =
+ (*A_log_)[i * m] * dXdHi_history_.back() +
+ (*B_log_)[i * m] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m], shotIdx_);
+
+ // step 2:
+ state_vector_t dK2dHi =
+ (*A_log_)[i * m + 1] * (dXdHi_history_.back() + h * a_21 * dK1dHi) +
+ (*B_log_)[i * m + 1] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m + 1], shotIdx_);
+
+ // step 3:
+ state_vector_t dK3dHi =
+ (*A_log_)[i * m + 2] * (dXdHi_history_.back() + h * a_32 * dK2dHi) +
+ (*B_log_)[i * m + 2] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m + 2], shotIdx_);
+
+ // step 4:
+ state_vector_t dK4dHi =
+ (*A_log_)[i * m + 3] * (dXdHi_history_.back() + h * a_43 * dK3dHi) +
+ (*B_log_)[i * m + 3] * controlSpliner_->splineDerivative_h_i((*t_log_)[i * m + 3], shotIdx_);
+
+ // summation
+ state_vector_t temp =
+ dXdHi_history_.back() + h * (b1 * dK1dHi + b2 * dK2dHi + b3 * dK3dHi + b4 * dK4dHi);
+
+ dXdHi_history_.push_back(temp);
+ }
+
+ break;
+ }
+
+ default:
+ {
+ std::cerr << "... ERROR: sensitivity calculation not implemented for this integration type. Exiting"
+ << std::endl;
+ exit(0);
+ }
+ }
+ }
+
+ void getdXdSiTraj(state_matrix_array_t& dXdSiTraj) { dXdSiTraj = dXdSi_history_; }
+ void getdXdQiTraj(state_control_matrix_array_t& dXdQiTraj) { dXdQiTraj = dXdQi_history_; }
+ void getdXdQip1Traj(state_control_matrix_array_t& dXdQip1Traj) { dXdQip1Traj = dXdQip1_history_; }
+ void getdXdHiTraj(state_vector_array_t& dXdHiTraj) { dXdHiTraj = dXdHi_history_; }
+private:
+ std::shared_ptr<SplinerBase<control_vector_t>> controlSpliner_;
+ const size_t shotIdx_;
+ const DmsSettings settings_;
+
+ std::shared_ptr<state_vector_array_t> x_log_;
+ std::shared_ptr<control_vector_array_t> u_log_;
+ std::shared_ptr<time_array_t> t_log_;
+ std::shared_ptr<state_matrix_array_t> A_log_;
+ std::shared_ptr<state_control_matrix_array_t> B_log_;
+
+ state_matrix_array_t dXdSi_history_;
+ state_control_matrix_array_t dXdQi_history_;
+ state_control_matrix_array_t dXdQip1_history_;
+ state_vector_array_t dXdHi_history_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/SensitivityIntegratorCT.h b/ct_optcon/include/ct/optcon/dms/dms_core/SensitivityIntegratorCT.h
new file mode 100644
index 0000000..a006592
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/SensitivityIntegratorCT.h
@@ -0,0 +1,526 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @brief This class can integrate a controlled system and a costfunction.
+ * Furthermore, it provides first order derivatives with respect to
+ * initial state and control
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ * @tparam SCALAR The scalar type
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class SensitivityIntegratorCT
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector;
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> control_vector;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> state_control_matrix;
+
+
+ /**
+ * @brief Constructor
+ *
+ * @param[in] system The controlled system
+ * @param[in] stepperType The integration stepper type
+ */
+ SensitivityIntegratorCT(const std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& system,
+ const ct::core::IntegrationType stepperType = ct::core::IntegrationType::EULERCT)
+ : cacheData_(false), cacheSensitivities_(false)
+ {
+ setControlledSystem(system);
+ initializeDerived(stepperType);
+ }
+
+ /**
+ * @brief Destroys the object.
+ */
+ ~SensitivityIntegratorCT() {}
+ /**
+ * @brief Initializes the steppers
+ *
+ * @param[in] stepperType The desired integration stepper type
+ */
+ void initializeDerived(const ct::core::IntegrationType stepperType)
+ {
+ switch (stepperType)
+ {
+ case ct::core::IntegrationType::EULERCT:
+ {
+ stepperState_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_vector, SCALAR>());
+ stepperDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_matrix, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_matrix, SCALAR>());
+ stepperDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_control_matrix, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_control_matrix, SCALAR>());
+ stepperCost_ = std::shared_ptr<ct::core::internal::StepperCTBase<SCALAR, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<SCALAR, SCALAR>());
+ stepperCostDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<state_vector, SCALAR>());
+ stepperCostDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<control_vector, SCALAR>>(
+ new ct::core::internal::StepperEulerCT<control_vector, SCALAR>());
+ break;
+ }
+
+ case ct::core::IntegrationType::RK4CT:
+ {
+ stepperState_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_vector, SCALAR>());
+ stepperDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_matrix, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_matrix, SCALAR>());
+ stepperDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_control_matrix, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_control_matrix, SCALAR>());
+ stepperCost_ = std::shared_ptr<ct::core::internal::StepperCTBase<SCALAR, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<SCALAR, SCALAR>());
+ stepperCostDX0_ = std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<state_vector, SCALAR>());
+ stepperCostDU0_ = std::shared_ptr<ct::core::internal::StepperCTBase<control_vector, SCALAR>>(
+ new ct::core::internal::StepperRK4CT<control_vector, SCALAR>());
+ break;
+ }
+
+ default:
+ throw std::runtime_error("Invalid CT integration type");
+ }
+ }
+
+ /**
+ * @brief Prepares the integrator to provide first order sensitivity
+ * generation by setting a linearsystem, enabling state caching
+ * and settings up the function objects
+ *
+ * @param[in] linearSystem The linearized system
+ */
+ void setLinearSystem(const std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& linearSystem)
+ {
+ linearSystem_ = linearSystem;
+ cacheData_ = true;
+
+ dX0dot_ = [this](const state_matrix& dX0In, state_matrix& dX0dt, const SCALAR t) {
+ if (cacheSensitivities_)
+ arraydX0_.push_back(dX0In);
+
+ dX0dt = arrayA_[dX0Index_] * dX0In;
+ dX0Index_++;
+ };
+
+ dU0dot_ = [this](const state_control_matrix& dU0In, state_control_matrix& dU0dt, const SCALAR t) {
+ if (cacheSensitivities_)
+ arraydU0_.push_back(dU0In);
+
+ dU0dt = arrayA_[dU0Index_] * dU0In +
+ arrayB_[dU0Index_] *
+ controlledSystem_->getController()->getDerivativeU0(
+ statesCached_[dU0Index_], timesCached_[dU0Index_]);
+ dU0Index_++;
+ };
+
+ dUfdot_ = [this](const state_control_matrix& dUfIn, state_control_matrix& dUfdt, const SCALAR t) {
+ if (cacheSensitivities_)
+ arraydUf_.push_back(dUfIn);
+
+ dUfdt = arrayA_[dU0Index_] * dUfIn +
+ arrayB_[dU0Index_] *
+ controlledSystem_->getController()->getDerivativeUf(
+ statesCached_[dU0Index_], timesCached_[dU0Index_]);
+ dU0Index_++;
+ };
+ }
+
+ /**
+ * @brief Changes the controlledsystem to be integrated
+ *
+ * @param[in] controlledSystem The new controlled system
+ */
+ void setControlledSystem(
+ const std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& controlledSystem)
+ {
+ controlledSystem_ = controlledSystem;
+ xDot_ = [this](const state_vector& x, state_vector& dxdt, const SCALAR t) {
+ control_vector controlAction;
+ controlledSystem_->getController()->computeControl(x, t, controlAction);
+
+ if (cacheData_)
+ {
+ statesCached_.push_back(x);
+ controlsCached_.push_back(controlAction);
+ timesCached_.push_back(t);
+ }
+
+ controlledSystem_->computeControlledDynamics(x, t, controlAction, dxdt);
+ };
+ }
+
+ /**
+ * @brief Prepares the integrator to provide cost integration and first
+ * order cost derivatives. This is done by enabling sensitivity
+ * caching and setting up the function objects
+ *
+ * @param[in] costFun The new costfunction
+ */
+ void setCostFunction(const std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFun)
+ {
+ costFunction_ = costFun;
+ cacheSensitivities_ = true;
+ costDot_ = [this](const SCALAR& costIn, SCALAR& cost, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ cost = costFunction_->evaluateIntermediate();
+ costIndex_++;
+ };
+
+ costdX0dot_ = [this](const state_vector& costdX0In, state_vector& costdX0dt, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ costdX0dt = arraydX0_[costIndex_].transpose() * costFunction_->stateDerivativeIntermediate();
+ costIndex_++;
+ };
+
+ costdU0dot_ = [this](const control_vector& costdU0In, control_vector& costdU0dt, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ costdU0dt = arraydU0_[costIndex_].transpose() * costFunction_->stateDerivativeIntermediate() +
+ controlledSystem_->getController()->getDerivativeU0(
+ statesCached_[costIndex_], timesCached_[costIndex_]) *
+ costFunction_->controlDerivativeIntermediate();
+ costIndex_++;
+ };
+
+ costdUfdot_ = [this](const control_vector& costdUfIn, control_vector& costdUfdt, const SCALAR t) {
+ costFunction_->setCurrentStateAndControl(
+ statesCached_[costIndex_], controlsCached_[costIndex_], timesCached_[costIndex_]);
+ costdUfdt = arraydUf_[costIndex_].transpose() * costFunction_->stateDerivativeIntermediate() +
+ controlledSystem_->getController()->getDerivativeUf(
+ statesCached_[costIndex_], timesCached_[costIndex_]) *
+ costFunction_->controlDerivativeIntermediate();
+ costIndex_++;
+ };
+ }
+
+ /**
+ * @brief Integrates the system starting from state and startTime
+ * for numSteps integration steps. Returns the full state
+ * and time trajectories
+ *
+ * @param[in, out] state The initial state for integration
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number steps
+ * @param[in] dt The integration timestep
+ * @param[out] stateTrajectory The output state trajectory
+ * @param[out] timeTrajectory The output time trajectory
+ */
+ void integrate(state_vector& state,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& stateTrajectory,
+ ct::core::tpl::TimeArray<SCALAR>& timeTrajectory)
+ {
+ clearStates();
+ clearLinearization();
+ cacheData_ = true;
+ stateTrajectory.clear();
+ timeTrajectory.clear();
+ SCALAR time = startTime;
+ stateTrajectory.push_back(state);
+ timeTrajectory.push_back(time);
+
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperState_->do_step(xDot_, state, time, dt);
+ time += dt;
+ stateTrajectory.push_back(state);
+ timeTrajectory.push_back(time);
+ }
+ }
+
+ /**
+ * @brief Integrates the system starting from state and startTime
+ * for numSteps integration steps. Returns only the final
+ * state and time
+ *
+ * @param[int, out] state The initial state for integration
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number steps
+ * @param[in] dt The integration timestep
+ */
+ void integrate(state_vector& state, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ clearStates();
+ clearLinearization();
+ SCALAR time = startTime;
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperState_->do_step(xDot_, state, time, dt);
+ time += dt;
+ }
+ }
+
+
+ /**
+ * @brief Integrates the sensitivity ODE of the integrator with
+ * respec to the initial state x0
+ *
+ * @param[in, out] dX0 The sensitivity matrix wrt x0
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration timestep
+ */
+ void integrateSensitivityDX0(state_matrix& dX0, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ dX0Index_ = 0;
+ SCALAR time = startTime;
+ dX0.setIdentity();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperDX0_->do_step(dX0dot_, dX0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity ODE of the integrator with
+ * respec to the initial control input u0
+ *
+ * @param[in, out] dU0 The sensitivity matrix wrt u0
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration timestep
+ */
+ void integrateSensitivityDU0(state_control_matrix& dU0,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ dU0Index_ = 0;
+ SCALAR time = startTime;
+ dU0.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperDU0_->do_step(dU0dot_, dU0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity ODE of the integrator with
+ * respec to the final control input uf
+ *
+ * @param[in, out] dUF The sensitivity matrix wrt uF
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration timestep
+ */
+ void integrateSensitivityDUf(state_control_matrix& dUf,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ dU0Index_ = 0;
+ SCALAR time = startTime;
+ dUf.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperDU0_->do_step(dUfdot_, dUf, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the costfunction using the states and controls
+ * from the costintegration
+ *
+ * @param[in, out] cost The initial cost
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCost(SCALAR& cost, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ SCALAR time = startTime;
+ costIndex_ = 0;
+ if (statesCached_.size() == 0 || controlsCached_.size() == 0 || timesCached_.size() == 0)
+ throw std::runtime_error("States cached are empty");
+
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCost_->do_step(costDot_, cost, time, dt);
+ time += dt;
+ }
+ }
+
+
+ /**
+ * @brief Integrates the sensitivity of the cost with respect to
+ * the initial state x0
+ *
+ * @param[in, out] dX0 The initial cost sensitivity vector
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCostSensitivityDX0(state_vector& dX0, const SCALAR startTime, const size_t numSteps, const SCALAR dt)
+ {
+ costIndex_ = 0;
+ SCALAR time = startTime;
+ dX0.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCostDX0_->do_step(costdX0dot_, dX0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity of the cost with respect to
+ * the initial control input u0
+ *
+ * @param[in, out] dU0 The initial cost sensitivity vector
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCostSensitivityDU0(control_vector& dU0,
+ const SCALAR startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ costIndex_ = 0;
+ SCALAR time = startTime;
+ dU0.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCostDU0_->do_step(costdU0dot_, dU0, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Integrates the sensitivity of the cost with respect to
+ * the final control input uF
+ *
+ * @param[in, out] dUf The initial cost sensitivity vector
+ * @param[in] startTime The start time
+ * @param[in] numSteps The number of integration steps
+ * @param[in] dt The integration time step
+ */
+ void integrateCostSensitivityDUf(control_vector& dUf,
+ const SCALAR& startTime,
+ const size_t numSteps,
+ const SCALAR dt)
+ {
+ costIndex_ = 0;
+ SCALAR time = startTime;
+ dUf.setZero();
+ for (size_t i = 0; i < numSteps; ++i)
+ {
+ stepperCostDU0_->do_step(costdUfdot_, dUf, time, dt);
+ time += dt;
+ }
+ }
+
+ /**
+ * @brief Linearizes the system around the rollout from the state
+ * interation
+ */
+ void linearize()
+ {
+ for (size_t i = 0; i < statesCached_.size(); ++i)
+ {
+ arrayA_.push_back(linearSystem_->getDerivativeState(statesCached_[i], controlsCached_[i], timesCached_[i]));
+ arrayB_.push_back(
+ linearSystem_->getDerivativeControl(statesCached_[i], controlsCached_[i], timesCached_[i]));
+ }
+ }
+
+ /**
+ * @brief Clears the cached states, controls and times
+ */
+ void clearStates()
+ {
+ statesCached_.clear();
+ controlsCached_.clear();
+ timesCached_.clear();
+ }
+
+
+ /**
+ * @brief Clears the cached sensitivities
+ */
+ void clearSensitivities()
+ {
+ arraydX0_.clear();
+ arraydU0_.clear();
+ arraydUf_.clear();
+ }
+
+
+ /**
+ * @brief Clears the linearized matrices
+ */
+ void clearLinearization()
+ {
+ arrayA_.clear();
+ arrayB_.clear();
+ }
+
+private:
+ std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> controlledSystem_;
+ std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> linearSystem_;
+ std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFunction_;
+
+ // Integrate the function
+ std::function<void(const SCALAR, SCALAR&, const SCALAR)> costDot_;
+ std::function<void(const state_vector&, state_vector&, const SCALAR)> costdX0dot_;
+ std::function<void(const control_vector&, control_vector&, const SCALAR)> costdU0dot_;
+ std::function<void(const control_vector&, control_vector&, const SCALAR)> costdUfdot_;
+
+ // Sensitivities
+ std::function<void(const state_matrix&, state_matrix&, const SCALAR)> dX0dot_;
+ std::function<void(const state_control_matrix&, state_control_matrix&, const SCALAR)> dU0dot_;
+ std::function<void(const state_control_matrix&, state_control_matrix&, const SCALAR)> dUfdot_;
+
+ // Cache
+ bool cacheData_;
+ bool cacheSensitivities_;
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> statesCached_;
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> controlsCached_;
+ ct::core::tpl::TimeArray<SCALAR> timesCached_;
+
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> arrayA_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> arrayB_;
+
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> arraydX0_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> arraydU0_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> arraydUf_;
+
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>> stepperState_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_matrix, SCALAR>> stepperDX0_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_control_matrix, SCALAR>> stepperDU0_;
+
+ std::shared_ptr<ct::core::internal::StepperCTBase<SCALAR, SCALAR>> stepperCost_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<state_vector, SCALAR>> stepperCostDX0_;
+ std::shared_ptr<ct::core::internal::StepperCTBase<control_vector, SCALAR>> stepperCostDU0_;
+
+ size_t costIndex_;
+ size_t dX0Index_;
+ size_t dU0Index_;
+
+ std::function<void(const state_vector&, state_vector&, const SCALAR)> xDot_;
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/ShotContainer.h b/ct_optcon/include/ct/optcon/dms/dms_core/ShotContainer.h
new file mode 100644
index 0000000..76f02e9
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/ShotContainer.h
@@ -0,0 +1,360 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <cmath>
+#include <functional>
+
+#include "SensitivityIntegratorCT.h"
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+#include <ct/optcon/dms/dms_core/DmsDimensions.h>
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ControllerDms.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief This class performs the state and the sensitivity integration on
+ * a shot
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ShotContainer
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ typedef typename DIMENSIONS::state_matrix_t state_matrix_t;
+ typedef typename DIMENSIONS::state_control_matrix_t state_control_matrix_t;
+
+ typedef typename DIMENSIONS::state_matrix_array_t state_matrix_array_t;
+ typedef typename DIMENSIONS::state_control_matrix_array_t state_control_matrix_array_t;
+
+ ShotContainer() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] controlledSystem The nonlinear system
+ * @param[in] linearSystem The linearized system
+ * @param[in] costFct The costfunction
+ * @param[in] w The optimization vector
+ * @param[in] controlSpliner The control input spliner
+ * @param[in] timeGrid The timegrid
+ * @param[in] shotNr The shot number
+ * @param[in] settings The dms settings
+ */
+ ShotContainer(std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> controlledSystem,
+ std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> linearSystem,
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ size_t shotNr,
+ DmsSettings settings,
+ size_t nIntegrationSteps)
+ : controlledSystem_(controlledSystem),
+ linearSystem_(linearSystem),
+ costFct_(costFct),
+ w_(w),
+ controlSpliner_(controlSpliner),
+ timeGrid_(timeGrid),
+ shotNr_(shotNr),
+ settings_(settings),
+ integrationCount_(0),
+ costIntegrationCount_(0),
+ sensIntegrationCount_(0),
+ costSensIntegrationCount_(0),
+ cost_(SCALAR(0.0)),
+ discreteQ_(state_vector_t::Zero()),
+ discreteR_(control_vector_t::Zero()),
+ discreteRNext_(control_vector_t::Zero())
+ {
+ if (shotNr_ >= settings.N_)
+ throw std::runtime_error("Dms Shot Integrator: shot index >= settings.N_ - check your settings.");
+
+ switch (settings_.integrationType_)
+ {
+ case DmsSettings::EULER:
+ {
+ integratorCT_ = std::allocate_shared<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>,
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>>(
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>(),
+ controlledSystem_, core::EULERCT);
+ break;
+ }
+ case DmsSettings::RK4:
+ {
+ integratorCT_ = std::allocate_shared<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>,
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>>(
+ Eigen::aligned_allocator<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>>(),
+ controlledSystem_, core::RK4CT);
+ break;
+ }
+ case DmsSettings::RK5:
+ {
+ throw std::runtime_error("Currently we do not support adaptive integrators in dms");
+ }
+
+ default:
+ {
+ std::cerr << "... ERROR: unknown integration type. Exiting" << std::endl;
+ exit(0);
+ }
+ }
+
+ tStart_ = timeGrid_->getShotStartTime(shotNr_);
+ // SCALAR t_shot_end = timeGrid_->getShotEndTime(shotNr_);
+
+ // +0.5 needed to avoid rounding errors from double to size_t
+ nSteps_ = nIntegrationSteps;
+ // std::cout << "shotNr_: " << shotNr_ << "\t nSteps: " << nSteps_ << std::endl;
+
+ integratorCT_->setLinearSystem(linearSystem_);
+
+ if (settings_.costEvaluationType_ == DmsSettings::FULL)
+ integratorCT_->setCostFunction(costFct_);
+ }
+
+ /**
+ * @brief Performs the state integration between the shots
+ */
+ void integrateShot()
+ {
+ if ((w_->getUpdateCount() != integrationCount_))
+ {
+ integrationCount_ = w_->getUpdateCount();
+ state_vector_t initState = w_->getOptimizedState(shotNr_);
+ integratorCT_->integrate(
+ initState, tStart_, nSteps_, SCALAR(settings_.dt_sim_), stateSubsteps_, timeSubsteps_);
+ }
+ }
+
+ void integrateCost()
+ {
+ if ((w_->getUpdateCount() != costIntegrationCount_))
+ {
+ costIntegrationCount_ = w_->getUpdateCount();
+ integrateShot();
+ cost_ = SCALAR(0.0);
+ integratorCT_->integrateCost(cost_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ }
+ }
+
+ /**
+ * @brief Performs the state and the sensitivity integration between the shots
+ */
+ void integrateSensitivities()
+ {
+ if ((w_->getUpdateCount() != sensIntegrationCount_))
+ {
+ sensIntegrationCount_ = w_->getUpdateCount();
+ integrateShot();
+ discreteA_.setIdentity();
+ discreteB_.setZero();
+ integratorCT_->linearize();
+ integratorCT_->integrateSensitivityDX0(discreteA_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ integratorCT_->integrateSensitivityDU0(discreteB_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+
+ if (settings_.splineType_ == DmsSettings::PIECEWISE_LINEAR)
+ {
+ discreteBNext_.setZero();
+ integratorCT_->integrateSensitivityDUf(discreteBNext_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ }
+ }
+ }
+
+ void integrateCostSensitivities()
+ {
+ if ((w_->getUpdateCount() != costSensIntegrationCount_))
+ {
+ costSensIntegrationCount_ = w_->getUpdateCount();
+ integrateSensitivities();
+ discreteQ_.setZero();
+ discreteR_.setZero();
+ integratorCT_->integrateCostSensitivityDX0(discreteQ_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ integratorCT_->integrateCostSensitivityDU0(discreteR_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+
+ if (settings_.splineType_ == DmsSettings::PIECEWISE_LINEAR)
+ {
+ discreteRNext_.setZero();
+ integratorCT_->integrateCostSensitivityDUf(discreteRNext_, tStart_, nSteps_, SCALAR(settings_.dt_sim_));
+ }
+ }
+ }
+
+ void reset()
+ {
+ integratorCT_->clearStates();
+ integratorCT_->clearSensitivities();
+ integratorCT_->clearLinearization();
+ }
+
+ /**
+ * @brief Returns the integrated state
+ *
+ * @return The integrated state
+ */
+ const state_vector_t getStateIntegrated() { return stateSubsteps_.back(); }
+ /**
+ * @brief Returns the end time of the integration
+ *
+ * @return The end time of the integration.
+ */
+ const SCALAR getIntegrationTimeFinal() { return timeSubsteps_.back(); }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * discretized state s_i
+ *
+ * @return The integrated sensitivity
+ */
+ const state_matrix_t getdXdSiIntegrated() { return discreteA_; }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * discretized inputs q_i
+ *
+ * @return The integrated sensitivity
+ */
+ const state_control_matrix_t getdXdQiIntegrated() { return discreteB_; }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * discretized inputs q_{i+1}
+ *
+ * @return The integrated sensitivity
+ */
+ const state_control_matrix_t getdXdQip1Integrated() { return discreteBNext_; }
+ /**
+ * @brief Returns the integrated ODE sensitivity with respect to the
+ * time segments h_i
+ *
+ * @return The integrated sensitivity
+ */
+ // const state_vector_t getdXdHiIntegrated()
+ // {
+ // return dXdHi_history_.back();
+ // }
+
+ /**
+ * @brief Gets the full integrated state trajectory.
+ *
+ * @return The integrated state trajectory
+ */
+ const state_vector_array_t& getXHistory() const { return stateSubsteps_; }
+ /**
+ * @brief Returns the control input trajectory used during the state integration
+ *
+ * @return The control trajectory
+ */
+ const control_vector_array_t& getUHistory()
+ {
+ inputSubsteps_.clear();
+ for (size_t t = 0; t < timeSubsteps_.size(); ++t)
+ {
+ inputSubsteps_.push_back(controlSpliner_->evalSpline(timeSubsteps_[t], shotNr_));
+ }
+ return inputSubsteps_;
+ }
+
+ /**
+ * @brief Returns the time trajectory used during the integration
+ *
+ * @return The time trajectory
+ */
+ const time_array_t& getTHistory() const { return timeSubsteps_; }
+ /**
+ * @brief Gets the cost integrated.
+ *
+ * @return The integrated cost.
+ */
+ const SCALAR getCostIntegrated() const { return cost_; }
+ /**
+ * @brief Returns the cost gradient with respect to s_i integrated over
+ * the shot
+ *
+ * @return The cost gradient
+ */
+ const state_vector_t getdLdSiIntegrated() const { return discreteQ_; }
+ /**
+ * @brief Returns the cost gradient with respect to q_i integrated over
+ * the shot
+ *
+ * @return The cost gradient
+ */
+ const control_vector_t getdLdQiIntegrated() const { return discreteR_; }
+ /**
+ * @brief Returns to cost gradient with respect to q_{i+1} integrated
+ * over the shot
+ *
+ * @return The cost gradient
+ */
+ const control_vector_t getdLdQip1Integrated() const { return discreteRNext_; }
+ /**
+ * @brief Returns to cost gradient with respect to h_i integrated over
+ * the shot
+ *
+ * @return The cost gradient
+ */
+ // const double getdLdHiIntegrated() const
+ // {
+ // return costGradientHi_;
+ // }
+
+
+private:
+ std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> controlledSystem_;
+ std::shared_ptr<ct::core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> linearSystem_;
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+
+ const size_t shotNr_;
+ const DmsSettings settings_;
+
+ size_t integrationCount_;
+ size_t costIntegrationCount_;
+ size_t sensIntegrationCount_;
+ size_t costSensIntegrationCount_;
+
+ // Integrated trajectories
+ state_vector_array_t stateSubsteps_;
+ control_vector_array_t inputSubsteps_;
+ time_array_t timeSubsteps_;
+
+ //Sensitivity Trajectories
+ state_matrix_t discreteA_;
+ state_control_matrix_t discreteB_;
+ state_control_matrix_t discreteBNext_;
+
+ //Cost and cost gradient
+ SCALAR cost_;
+ state_vector_t discreteQ_;
+ control_vector_t discreteR_;
+ control_vector_t discreteRNext_;
+
+ std::shared_ptr<SensitivityIntegratorCT<STATE_DIM, CONTROL_DIM, SCALAR>> integratorCT_;
+ size_t nSteps_;
+ SCALAR tStart_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/TimeGrid.h b/ct_optcon/include/ct/optcon/dms/dms_core/TimeGrid.h
new file mode 100644
index 0000000..b7b5c22
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/TimeGrid.h
@@ -0,0 +1,161 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+//#define DEBUG_TIMEGRID
+
+#include <math.h>
+#include <cmath>
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup DMS
+ *
+ * This class describes the time-grid underlying the shots in the dms problem In
+ * total, we have N+1 pairs of (s_i, q_i) and N shots between them.
+ *
+ * We assume that starting time t_0 = 0.0 [sec]
+ */
+template <typename SCALAR>
+class TimeGrid
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ TimeGrid() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] numberOfShots The number of shots
+ * @param[in] timeHorizon The dms time horizon
+ */
+ TimeGrid(const size_t numberOfShots, const SCALAR timeHorizon)
+ : numberOfShots_(numberOfShots), timeHorizon_(timeHorizon), t_(numberOfShots + 1, SCALAR(0.0))
+ {
+ makeUniformGrid();
+ }
+
+ /**
+ * @brief Updates the timegrid when the number of shots changes
+ *
+ * @param[in] numberOfShots The new number of shots
+ */
+ void changeShotCount(const size_t numberOfShots)
+ {
+ numberOfShots_ = numberOfShots;
+ t_.clear();
+ t_.resize(numberOfShots_ + 1, SCALAR(0.0));
+ makeUniformGrid();
+ }
+
+ /**
+ * @brief Updates the timegrid when the timehorizon changes
+ *
+ * @param[in] timeHorizon The new time horizon
+ */
+ void changeTimeHorizon(const SCALAR timeHorizon)
+ {
+ timeHorizon_ = timeHorizon;
+ makeUniformGrid();
+ }
+
+
+ /**
+ * @brief This method updates the timegrid when new optimized time
+ * segments arrive from the nlp solver. Only gets called when
+ * using timegrid optimization, otherwise the timegrid stays
+ * fixed
+ *
+ * @param[in] h_segment The vector of the new optimized time segments
+ */
+ void updateTimeGrid(const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& h_segment)
+ {
+ t_[0] = SCALAR(0.0); //by convention (just for documentation)
+
+ for (size_t i = 0; i < (size_t)h_segment.size(); ++i)
+ t_[i + 1] = t_[i] + h_segment(i);
+
+#ifdef DEBUG_TIMEGRID
+ std::cout << " ... in updateTimeGrid(). t_ = ";
+ for (size_t i = 0; i < t_.size(); i++)
+ std::cout << std::setprecision(10) << t_[i] << " ";
+
+ std::cout << std::endl;
+#endif
+ }
+
+
+ /**
+ * @brief Creates a uniform timegrid
+ */
+ void makeUniformGrid()
+ {
+ for (size_t i = 0; i < numberOfShots_ + 1; i++)
+ t_[i] = i * (SCALAR)(timeHorizon_ / (SCALAR)numberOfShots_);
+ }
+
+
+ /**
+ * @brief Returns to start time of a shot
+ *
+ * @param[in] shot_index The shot number
+ *
+ * @return The start time
+ */
+ const SCALAR getShotStartTime(const size_t shot_index) const { return t_[shot_index]; }
+ /**
+ * @brief Returns the end time of a shot
+ *
+ * @param[in] shot_index The shot number
+ *
+ * @return The end time
+ */
+ const SCALAR getShotEndTime(const size_t shot_index) const { return t_[shot_index + 1]; }
+ /**
+ * @brief Returns to duration of a shot
+ *
+ * @param[in] shot_index The shot index
+ *
+ * @return The duration
+ */
+ const SCALAR getShotDuration(const size_t shot_index) const { return (t_[shot_index + 1] - t_[shot_index]); }
+ /**
+ * @brief Returns the underlying TimeArray
+ *
+ * @return The underlying TimeArray
+ */
+ const ct::core::tpl::TimeArray<SCALAR>& toImplementation() { return t_; }
+ /**
+ * @brief Returns the initial timehorizon of the problem
+ *
+ * @return The initial time horizon
+ */
+ const SCALAR getTimeHorizon() const { return timeHorizon_; }
+ /**
+ * @brief Returns the optimized timehorizon
+ *
+ * @return The optimized timehorizon
+ */
+ const SCALAR getOptimizedTimeHorizon() const { return t_.back(); }
+private:
+ size_t numberOfShots_;
+ SCALAR timeHorizon_;
+
+ // the individual times of each pair from i=0,..., N
+ ct::core::tpl::TimeArray<SCALAR> t_;
+};
+}
+
+typedef tpl::TimeGrid<double> TimeGrid;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h
new file mode 100644
index 0000000..43d2a76
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h
@@ -0,0 +1,152 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <omp.h>
+#include <math.h>
+#include <cmath>
+#include <functional>
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/ShotContainer.h>
+#include <ct/optcon/nlp/DiscreteCostEvaluatorBase.h>
+
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Performs the full cost integration over the shots
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostEvaluatorFull : public tpl::DiscreteCostEvaluatorBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+ typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
+ typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;
+ typedef typename DIMENSIONS::time_array_t time_array_t;
+
+ CostEvaluatorFull() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] costFct The cost function
+ * @param[in] w The optimization vector
+ * @param[in] controlSpliner The control spliner
+ * @param[in] shotInt The shot number
+ * @param[in] settings The dms settings
+ */
+ CostEvaluatorFull(std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner,
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotInt,
+ DmsSettings settings)
+ : costFct_(costFct), w_(w), controlSpliner_(controlSpliner), shotContainers_(shotInt), settings_(settings)
+ {
+ }
+
+ /**
+ * @brief The destructor.
+ */
+ virtual ~CostEvaluatorFull() {}
+ virtual SCALAR eval() override
+ {
+ SCALAR cost = SCALAR(0.0);
+
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateCost();
+ }
+
+ for (auto shotContainer : shotContainers_)
+ cost += shotContainer->getCostIntegrated();
+
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ cost += costFct_->evaluateTerminal();
+ return cost;
+ }
+
+ virtual void evalGradient(size_t grad_length, Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad) override
+ {
+ grad.setZero();
+
+ assert(shotContainers_.size() == settings_.N_);
+
+// go through all shots, integrate the state trajectories and evaluate cost accordingly
+// intermediate costs
+#pragma omp parallel for num_threads(settings_.nThreads_)
+ for (auto shotContainer = shotContainers_.begin(); shotContainer < shotContainers_.end(); ++shotContainer)
+ {
+ (*shotContainer)->integrateCostSensitivities();
+ }
+
+ for (size_t shotNr = 0; shotNr < shotContainers_.size(); ++shotNr)
+ {
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ grad.segment(w_->getStateIndex(shotNr), STATE_DIM) += shotContainers_[shotNr]->getdLdSiIntegrated();
+ grad.segment(w_->getControlIndex(shotNr), CONTROL_DIM) +=
+ shotContainers_[shotNr]->getdLdQiIntegrated();
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ grad.segment(w_->getStateIndex(shotNr), STATE_DIM) += shotContainers_[shotNr]->getdLdSiIntegrated();
+ grad.segment(w_->getControlIndex(shotNr), CONTROL_DIM) +=
+ shotContainers_[shotNr]->getdLdQiIntegrated();
+ grad.segment(w_->getControlIndex(shotNr + 1), CONTROL_DIM) +=
+ shotContainers_[shotNr]->getdLdQip1Integrated();
+ break;
+ }
+ default:
+ throw(std::runtime_error(
+ " cost gradient not yet implemented for this type of interpolation. Exiting"));
+ }
+
+ // H-part.
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // costFct_->setCurrentStateAndControl(shotContainers_[shotNr]->getStateIntegrated(),
+ // controlSpliner_->evalSpline(shotContainers_[shotNr]->getIntegrationTimeFinal(), shotNr));
+ // grad(w_->getTimeSegmentIndex(shotNr)) = costFct_->evaluateIntermediate() + shotContainers_[shotNr]->getdLdHiIntegrated();
+ // }
+ }
+
+ /* gradient of terminal cost */
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ grad.segment(w_->getStateIndex(settings_.N_), STATE_DIM) +=
+ costFct_->stateDerivativeTerminal(); // * dXdSi.back();
+ }
+
+private:
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<SplinerBase<control_vector_t, SCALAR>> controlSpliner_;
+ std::vector<std::shared_ptr<ShotContainer<STATE_DIM, CONTROL_DIM, SCALAR>>> shotContainers_;
+
+ const DmsSettings settings_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h
new file mode 100644
index 0000000..d2b32f7
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h
@@ -0,0 +1,172 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <omp.h>
+#include <math.h>
+#include <cmath>
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+#include <ct/optcon/dms/dms_core/OptVectorDms.h>
+#include <ct/optcon/dms/dms_core/spline/SplinerBase.h>
+#include <ct/optcon/nlp/DiscreteCostEvaluatorBase.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Evaluates the cost at the shots and performs some interpolation
+ * in between
+ *
+ * @tparam STATE_DIM The state dimension
+ * @tparam CONTROL_DIM The control dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class CostEvaluatorSimple : public tpl::DiscreteCostEvaluatorBase<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR> DIMENSIONS;
+
+ typedef typename DIMENSIONS::state_vector_t state_vector_t;
+ typedef typename DIMENSIONS::control_vector_t control_vector_t;
+
+ CostEvaluatorSimple() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] costFct The cost function
+ * @param[in] w The optimization variables
+ * @param[in] timeGrid The time grid
+ * @param[in] settings The dms settings
+ */
+ CostEvaluatorSimple(std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct,
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w,
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid,
+ DmsSettings settings)
+ : costFct_(costFct), w_(w), timeGrid_(timeGrid), settings_(settings)
+ {
+ phi_.resize(settings_.N_ + 1);
+ // phi_diff_h_ = Eigen::VectorXd::Ones(settings_.N_+1, 1);
+ updatePhi();
+ }
+
+ virtual ~CostEvaluatorSimple() {}
+ virtual SCALAR eval() override;
+
+ virtual void evalGradient(size_t grad_length, Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad) override;
+
+private:
+ /**
+ * @brief Updates the weights for the cost interpolation
+ */
+ void updatePhi();
+
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFct_;
+ std::shared_ptr<OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>> w_;
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+ const DmsSettings settings_;
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> phi_; /* the summation weights */
+ // Eigen::VectorXd phi_diff_h_; /* the summation weights for derivative w.r.t h */
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>::eval()
+{
+ // updatePhi();
+ SCALAR cost = SCALAR(0.0);
+
+ for (size_t i = 0; i < settings_.N_ + 1; ++i)
+ {
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ cost += phi_(i) * costFct_->evaluateIntermediate();
+ }
+
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ cost += costFct_->evaluateTerminal();
+ assert(cost == cost);
+ return cost;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>::evalGradient(size_t grad_length,
+ Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad)
+{
+ // updatePhi();
+ grad.setZero();
+ for (size_t i = 0; i < settings_.N_ + 1; ++i)
+ {
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ grad.segment(w_->getStateIndex(i), STATE_DIM) += phi_(i) * costFct_->stateDerivativeIntermediate();
+ grad.segment(w_->getControlIndex(i), CONTROL_DIM) += phi_(i) * costFct_->controlDerivativeIntermediate();
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID && i < settings_.N_)
+ // {
+ // if(settings_.splineType_ == DmsSettings::ZERO_ORDER_HOLD)
+ // {
+ // costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ // double dJdH = phi_diff_h_(i) * costFct_->evaluateIntermediate();
+ // size_t idx = w_->getTimeSegmentIndex(i);
+ // grad(idx) = dJdH;
+ // }
+
+ // else if(settings_.splineType_ == DmsSettings::PIECEWISE_LINEAR)
+ // {
+ // costFct_->setCurrentStateAndControl(w_->getOptimizedState(i), w_->getOptimizedControl(i));
+ // double dJdH = 0.5 * costFct_->evaluateIntermediate();
+ // costFct_->setCurrentStateAndControl(w_->getOptimizedState(i+1), w_->getOptimizedControl(i+1));
+ // dJdH += 0.5 * costFct_->evaluateIntermediate();
+ // grad(w_->getTimeSegmentIndex(i)) = dJdH;
+ // }
+ // }
+ }
+
+ /* gradient of terminal cost */
+ costFct_->setCurrentStateAndControl(w_->getOptimizedState(settings_.N_), control_vector_t::Zero());
+ grad.segment(w_->getStateIndex(settings_.N_), STATE_DIM) += costFct_->stateDerivativeTerminal();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void CostEvaluatorSimple<STATE_DIM, CONTROL_DIM, SCALAR>::updatePhi()
+{
+ switch (settings_.splineType_)
+ {
+ case DmsSettings::ZERO_ORDER_HOLD:
+ {
+ for (size_t i = 0; i < settings_.N_; i++)
+ phi_(i) = (timeGrid_->getShotDuration(i));
+
+ phi_(settings_.N_) = SCALAR(0.0);
+ break;
+ }
+ case DmsSettings::PIECEWISE_LINEAR:
+ {
+ phi_(0) = SCALAR(0.5) * (timeGrid_->getShotDuration(0));
+
+ for (size_t i = 1; i < settings_.N_; i++)
+ phi_(i) = SCALAR(0.5) * (timeGrid_->getShotEndTime(i) - timeGrid_->getShotStartTime(i - 1));
+
+ phi_(settings_.N_) = SCALAR(0.5) * (timeGrid_->getShotDuration(settings_.N_));
+ break;
+ }
+ default:
+ throw(std::runtime_error(" ERROR: Unknown spline-type in CostEvaluatorSimple - exiting."));
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/implementation/OptVectorDms-impl.h b/ct_optcon/include/ct/optcon/dms/dms_core/implementation/OptVectorDms-impl.h
new file mode 100644
index 0000000..bb8e356
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/implementation/OptVectorDms-impl.h
@@ -0,0 +1,243 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::OptVectorDms(size_t n, const DmsSettings& settings)
+ : tpl::OptVector<SCALAR>(n), settings_(settings), numPairs_(settings.N_ + 1)
+{
+ // if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // optimizedTimeSegments_.resize(numPairs_ - 1);
+
+ size_t currIndex = 0;
+
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ pairNumToStateIdx_.insert(std::make_pair(i, currIndex));
+ currIndex += STATE_DIM;
+
+ pairNumToControlIdx_.insert(std::make_pair(i, currIndex));
+ currIndex += CONTROL_DIM;
+
+ // if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // if(i < numPairs_-1)
+ // {
+ // shotNumToShotDurationIdx_.insert(std::make_pair(i, currIndex));
+ // currIndex += 1;
+ // }
+ }
+
+ // if (settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // setLowerTimeSegmentBounds();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedState(const size_t pairNum) const
+{
+ size_t index = getStateIndex(pairNum);
+ return (this->x_.segment(index, STATE_DIM));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_t
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedControl(const size_t pairNum) const
+{
+ size_t index = getControlIndex(pairNum);
+ return (this->x_.segment(index, CONTROL_DIM));
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedTimeSegment(const size_t pairNum) const
+{
+ size_t index = getTimeSegmentIndex(pairNum);
+ // std::cout << "x_(index) : " << x_(index) << std::endl;
+ return this->x_(index);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_array_t&
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedStates()
+{
+ stateSolution_.clear();
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ stateSolution_.push_back(getOptimizedState(i));
+ }
+ return stateSolution_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename DmsDimensions<STATE_DIM, CONTROL_DIM, SCALAR>::control_vector_array_t&
+OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedInputs()
+{
+ inputSolution_.clear();
+ for (size_t i = 0; i < numPairs_; i++)
+ inputSolution_.push_back(getOptimizedControl(i));
+
+ return inputSolution_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getOptimizedTimeSegments()
+{
+ for (size_t i = 0; i < numPairs_ - 1; ++i)
+ optimizedTimeSegments_(i) = getOptimizedTimeSegment(i);
+
+ return optimizedTimeSegments_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getStateIndex(const size_t pairNum) const
+{
+ return pairNumToStateIdx_.find(pairNum)->second;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getControlIndex(const size_t pairNum) const
+{
+ return pairNumToControlIdx_.find(pairNum)->second;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+size_t OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::getTimeSegmentIndex(const size_t shotNr) const
+{
+ return (shotNumToShotDurationIdx_.find(shotNr)->second);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::changeInitialState(const state_vector_t& x0)
+{
+ size_t s_index = getStateIndex(0);
+ this->x_.segment(s_index, STATE_DIM) = x0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::changeDesiredState(const state_vector_t& xF)
+{
+ size_t s_index = pairNumToStateIdx_.find(settings_.N_)->second;
+ this->x_.segment(s_index, STATE_DIM) = xF;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::setInitGuess(const state_vector_t& x0,
+ const state_vector_t& x_f,
+ const control_vector_t& u0)
+{
+ size_t type = 1; // 0 = constant init guess, 1 = linearly interpolated between x0 and x_f
+
+ // init the states s_i and controls q_i
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ size_t s_index = getStateIndex(i);
+ size_t q_index = getControlIndex(i);
+
+ switch (type)
+ {
+ case 0:
+ {
+ this->xInit_.segment(s_index, STATE_DIM) = x0;
+ break;
+ }
+ case 1:
+ this->xInit_.segment(s_index, STATE_DIM) = x0 + (x_f - x0) * (i / (numPairs_ - 1));
+ break;
+ }
+ this->xInit_.segment(q_index, CONTROL_DIM) = u0;
+ }
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // for (size_t i = 0; i< numPairs_ - 1; i++)
+ // {
+ // size_t h_index = getTimeSegmentIndex(i);
+ // x_(h_index) = (double)settings_.T_ / (double)settings_.N_;
+ // }
+ // }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::setInitGuess(const state_vector_array_t& x_init,
+ const control_vector_array_t& u_init)
+{
+ if (x_init.size() != numPairs_)
+ throw std::runtime_error("initial guess state trajectory not matching number of shots");
+ if (u_init.size() != numPairs_)
+ throw std::runtime_error("initial guess input trajectory not matching number of shots");
+
+ for (size_t i = 0; i < numPairs_; i++)
+ {
+ size_t s_index = getStateIndex(i);
+ size_t q_index = getControlIndex(i);
+
+ this->xInit_.segment(s_index, STATE_DIM) = x_init[i];
+ this->xInit_.segment(q_index, CONTROL_DIM) = u_init[i];
+ }
+
+ // if(settings_.objectiveType_ == DmsSettings::OPTIMIZE_GRID)
+ // {
+ // for (size_t i = 0; i< numPairs_ - 1; i++)
+ // {
+ // size_t h_index = getTimeSegmentIndex(i);
+ // x_(h_index) = (double)settings_.T_ / (double)settings_.N_;
+ // }
+ // }
+
+ // std::cout << "x_ init: " << x_.transpose() << std::endl;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::setLowerTimeSegmentBounds()
+{
+ for (size_t i = 0; i < numPairs_ - 1; i++)
+ {
+ size_t h_index = shotNumToShotDurationIdx_.find(i)->second;
+ Eigen::Matrix<SCALAR, 1, 1> newElement;
+ newElement(0, 0) = settings_.h_min_;
+ this->xLb_.segment(h_index, 1) = newElement; //0.0;
+ newElement(0, 0) = 0.5;
+ this->xUb_.segment(h_index, 1) = newElement;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptVectorDms<STATE_DIM, CONTROL_DIM, SCALAR>::printoutSolution()
+{
+ std::cout << "... printing solutions: " << std::endl;
+ std::cout << "x_solution" << std::endl;
+ state_vector_array_t x_sol = getOptimizedStates();
+ for (size_t i = 0; i < x_sol.size(); ++i)
+ {
+ std::cout << x_sol[i].transpose() << std::endl;
+ }
+
+ std::cout << "u_solution" << std::endl;
+ control_vector_array_t u_sol = getOptimizedInputs();
+ for (size_t i = 0; i < u_sol.size(); ++i)
+ {
+ std::cout << u_sol[i].transpose() << std::endl;
+ }
+
+ std::cout << "t_solution" << std::endl;
+ const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& t_sol = getOptimizedTimeSegments();
+ for (size_t i = 0; i < t_sol.size(); ++i)
+ {
+ std::cout << t_sol[i] << " ";
+ }
+ std::cout << std::endl;
+ std::cout << " ... done." << std::endl;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h b/ct_optcon/include/ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h
new file mode 100644
index 0000000..d2b365b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h
@@ -0,0 +1,121 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ct/optcon/dms/dms_core/spline/SplinerBase.h"
+#include <ct/optcon/dms/dms_core/TimeGrid.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The linear spline implementation
+ *
+ * @tparam T The vector type to be splined
+ */
+template <class T, typename SCALAR = double>
+class LinearSpliner : public SplinerBase<T, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef T vector_t;
+ typedef std::vector<vector_t, Eigen::aligned_allocator<vector_t>> vector_array_t;
+ typedef Eigen::Matrix<SCALAR, T::DIM, T::DIM> matrix_t;
+
+ LinearSpliner() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] grid The dms timegrid
+ */
+ LinearSpliner(std::shared_ptr<tpl::TimeGrid<SCALAR>> grid) : timeGrid_(grid) {}
+ virtual ~LinearSpliner() {}
+ void computeSpline(const vector_array_t& points) override { nodes_ = points; }
+ // evaluate spline and return vector at interpolation time
+ virtual vector_t evalSpline(const SCALAR time, const size_t shotIdx) override
+ {
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> result;
+ result.resize(T::DIM);
+
+ // int shotIdx = timeGrid_->getShotIndex(time);
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a whole shot*/
+ SCALAR t_s_start = timeGrid_->getShotStartTime(shotIdx); /* time when this particular shot started */
+ SCALAR t_s_end = timeGrid_->getShotEndTime(shotIdx); /* time when this particular shot ends */
+
+ assert(shotIdx < nodes_.size());
+
+ result = nodes_[shotIdx] * (t_s_end - time) / t_shot + nodes_[shotIdx + 1] * (time - t_s_start) / t_shot;
+
+ return result;
+ }
+
+
+ virtual vector_t splineDerivative_t(const SCALAR time, const size_t shotIdx) const override
+ {
+ vector_t result;
+
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a whole shot*/
+
+ result = (nodes_[shotIdx + 1] - nodes_[shotIdx]) / t_shot;
+
+ return result;
+ }
+
+
+ virtual vector_t splineDerivative_h_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ vector_t result;
+
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a whole shot*/
+ SCALAR t_s_start = timeGrid_->getShotStartTime(shotIdx); /* time when this particular shot started */
+
+ result = (time - t_s_start) * (nodes_[shotIdx] - nodes_[shotIdx + 1]) / (t_shot * t_shot);
+
+ return result;
+ }
+
+ virtual matrix_t splineDerivative_q_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ matrix_t drv;
+
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of a the shot*/
+ SCALAR t_s_end = timeGrid_->getShotEndTime(shotIdx); /* time when this particular shot ends */
+
+ drv.setIdentity();
+ drv *= (t_s_end - time) / t_shot;
+
+ return drv;
+ }
+
+
+ virtual matrix_t splineDerivative_q_iplus1(const SCALAR time, const size_t shotIdx) const override
+ {
+ matrix_t drv;
+
+ // int shotIdx = timeGrid_->getShotIndex(time);
+ SCALAR t_shot = timeGrid_->getShotDuration(shotIdx); /* current duration of the shot*/
+ SCALAR t_s_start = timeGrid_->getShotStartTime(shotIdx); /* time when this particular shot started */
+
+ drv.setIdentity();
+ drv *= (time - t_s_start) / t_shot;
+
+ return drv;
+ }
+
+
+private:
+ vector_array_t nodes_; // an array of references to grid points between which is interpolated
+
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/spline/SplinerBase.h b/ct_optcon/include/ct/optcon/dms/dms_core/spline/SplinerBase.h
new file mode 100644
index 0000000..ead9cc0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/spline/SplinerBase.h
@@ -0,0 +1,104 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Abstract base class for the control input splining between the
+ * DMS shots
+ *
+ * @tparam T The vector type which will be splined
+ */
+template <class T, typename SCALAR = double>
+class SplinerBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /**
+ * @brief Default constructor
+ */
+ SplinerBase(){};
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~SplinerBase(){};
+
+ typedef T vector_t;
+ typedef Eigen::Matrix<SCALAR, T::DIM, T::DIM> matrix_t;
+ typedef std::vector<vector_t, Eigen::aligned_allocator<vector_t>> vector_array_t;
+
+
+ /**
+ * @brief Updates the vector on the shots
+ *
+ * @param[in] points Updated vector array
+ */
+ virtual void computeSpline(const vector_array_t& points) = 0;
+
+ /**
+ * @brief Depending on the spline type, this method evaluates the
+ * control input between the shots
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The splined vector
+ */
+ virtual vector_t evalSpline(const SCALAR time, const size_t shotIdx) = 0;
+
+ /**
+ * @brief Returns the spline derivatives with respect to time
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The time derivative
+ */
+ virtual vector_t splineDerivative_t(const SCALAR time, const size_t shotIdx) const = 0;
+
+ /**
+ * @brief Returns the spline derivatives with respect to the time
+ * segment between the shots
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The resulting derivative
+ */
+ virtual vector_t splineDerivative_h_i(const SCALAR time, const size_t shotIdx) const = 0;
+
+ /**
+ * @brief Return the spline derivative with respect to the control
+ * input at shot i
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The resulting derivative
+ */
+ virtual matrix_t splineDerivative_q_i(const SCALAR time, const size_t shotIdx) const = 0;
+
+ /**
+ * @brief Returns the spline derivative with respect to the control
+ * input at shot i+1
+ *
+ * @param[in] time The evaluation time
+ * @param[in] shotIdx The shot number
+ *
+ * @return The resulting derivative
+ */
+ virtual matrix_t splineDerivative_q_iplus1(const SCALAR time, const size_t shotIdx) const = 0;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h b/ct_optcon/include/ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h
new file mode 100644
index 0000000..dac0ccd
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h
@@ -0,0 +1,79 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "ct/optcon/dms/dms_core/spline/SplinerBase.h"
+#include <ct/optcon/dms/dms_core/TimeGrid.h>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief The spline implementation for the zero order hold spliner
+ *
+ * @tparam T The vector type to be splined
+ */
+template <class T, typename SCALAR = double>
+class ZeroOrderHoldSpliner : public SplinerBase<T, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef T vector_t;
+ typedef Eigen::Matrix<SCALAR, T::DIM, T::DIM> matrix_t;
+ typedef std::vector<vector_t, Eigen::aligned_allocator<vector_t>> vector_array_t;
+
+ ZeroOrderHoldSpliner() = delete;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] grid The DMS timegrid
+ */
+ ZeroOrderHoldSpliner(std::shared_ptr<tpl::TimeGrid<SCALAR>> grid) : timeGrid_(grid) {}
+ virtual ~ZeroOrderHoldSpliner() {}
+ void computeSpline(const vector_array_t& points) override { zOholds_ = points; }
+ // evaluate spline and return vector at interpolation time
+ virtual vector_t evalSpline(const SCALAR time, const size_t shotIdx) override
+ {
+ assert(shotIdx < zOholds_.size());
+ assert(zOholds_[shotIdx] == zOholds_[shotIdx]);
+ return zOholds_[shotIdx];
+ }
+
+ virtual vector_t splineDerivative_t(const SCALAR time, const size_t shotIdx) const override
+ {
+ return vector_t::Zero();
+ }
+
+ virtual vector_t splineDerivative_h_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ return vector_t::Zero();
+ }
+
+ virtual matrix_t splineDerivative_q_i(const SCALAR time, const size_t shotIdx) const override
+ {
+ return matrix_t::Identity();
+ }
+
+ virtual matrix_t splineDerivative_q_iplus1(const SCALAR time, const size_t shotIdx) const override
+ {
+ return matrix_t::Zero();
+ }
+
+
+private:
+ // zero order hold variables
+ vector_array_t zOholds_;
+
+ std::shared_ptr<tpl::TimeGrid<SCALAR>> timeGrid_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp b/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp
new file mode 100644
index 0000000..b098be2
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp
@@ -0,0 +1,162 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::FHDTLQR(
+ std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFunction)
+ : costFunction_(costFunction)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::~FHDTLQR()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks)
+{
+ size_t N = x_trajectory.size() - 1;
+ solve(x_trajectory, u_trajectory, A, B, N, dt, K, performNumericalChecks);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> derivatives,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks)
+{
+ size_t N = x_trajectory.size() - 1;
+
+ state_matrix_array_t A;
+ control_gain_matrix_array_t B;
+
+ linearizeModel(x_trajectory, u_trajectory, N, dt, derivatives, A, B);
+
+ solve(x_trajectory, u_trajectory, A, B, N, dt, K, performNumericalChecks);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::linearizeModel(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ size_t N,
+ SCALAR dt,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& derivatives,
+ state_matrix_array_t& A,
+ control_gain_matrix_array_t& B)
+{
+ A.resize(N);
+ B.resize(N);
+
+ for (int i = 0; i < N; i++)
+ {
+ A[i] = state_matrix_t::Identity() + dt * derivatives->getDerivativeState(x_trajectory[i], u_trajectory[i]);
+ B[i] = dt * derivatives->getDerivativeControl(x_trajectory[i], u_trajectory[i]);
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void FHDTLQR<STATE_DIM, CONTROL_DIM, SCALAR>::solve(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ size_t N,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks)
+{
+ if (x_trajectory.size() != N + 1)
+ {
+ throw std::runtime_error("State trajectory does not have correct length. State trajectory length: " +
+ std::to_string(x_trajectory.size()) + ", should be: " + std::to_string(N + 1));
+ }
+ if (u_trajectory.size() != N)
+ {
+ throw std::runtime_error("Input trajectory does not have correct length. Input trajectory length: " +
+ std::to_string(u_trajectory.size()) + ", should be: " + std::to_string(N));
+ }
+ if (A.size() != N)
+ {
+ throw std::runtime_error("Linearization A does not have correct length. Linearization length: " +
+ std::to_string(A.size()) + ", should be: " + std::to_string(N));
+ }
+ if (B.size() != N)
+ {
+ throw std::runtime_error("Linearization B does not have correct length. Linearization length: " +
+ std::to_string(B.size()) + ", should be: " + std::to_string(N));
+ }
+
+ K.resize(N);
+
+ // initialize cost-to-go
+ costFunction_->setCurrentStateAndControl(x_trajectory[N], control_vector_t::Zero(), dt * N);
+ state_matrix_t P = costFunction_->stateSecondDerivativeTerminal();
+
+ if (performNumericalChecks)
+ {
+ Eigen::Matrix<SCALAR, -1, 1> realEigVals = P.eigenvalues().real();
+ if (realEigVals.minCoeff() < 0.0)
+ {
+ std::cout << "P: " << std::endl << P;
+ throw std::runtime_error("[LQR] Q is not positive semi-definite.");
+ }
+ }
+
+ for (int i = N - 1; i >= 0; i--)
+ {
+ costFunction_->setCurrentStateAndControl(x_trajectory[i], u_trajectory[i], i * dt);
+
+ state_matrix_t Q = costFunction_->stateSecondDerivativeIntermediate() * dt;
+
+ control_matrix_t R = costFunction_->controlSecondDerivativeIntermediate() * dt;
+
+ if (performNumericalChecks)
+ {
+ if (Q.minCoeff() < 0.0)
+ {
+ std::cout << "Q: " << std::endl << Q;
+ throw std::runtime_error("[LQR] Q contains negative entries.");
+ }
+ if (R.minCoeff() < 0.0)
+ {
+ std::cout << "R: " << std::endl << R;
+ throw std::runtime_error("[LQR] R contains negative entries.");
+ }
+ if (R.diagonal().minCoeff() <= 0.0)
+ {
+ std::cout << "R: " << std::endl << R;
+ throw std::runtime_error("[LQR] R contains zero entries on the diagonal.");
+ }
+ if (!Q.isDiagonal())
+ {
+ std::cout << "[LQR] Warning: Q is not diagonal." << std::endl;
+ }
+ if (!R.isDiagonal())
+ {
+ std::cout << "[LQR] Warning: R is not diagonal." << std::endl;
+ }
+ }
+
+ //ricattiEq_.iterateNaive(Q, R, A, B, P, K[i]);
+ ricattiEq_.iterateRobust(Q, R, A[i], B[i], P, K[i]);
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/FHDTLQR.hpp b/ct_optcon/include/ct/optcon/lqr/FHDTLQR.hpp
new file mode 100644
index 0000000..57f99bb
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/FHDTLQR.hpp
@@ -0,0 +1,182 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp>
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+namespace ct {
+namespace optcon {
+
+/*! \defgroup LQR LQR
+ *
+ * \brief Linear Quadratic Regulator Module
+ * This module holds two verified LQR implementations in C++.
+ */
+
+/*!
+ * \ingroup LQR
+ *
+ * \brief Finite-Horizon Discrete Time LQR
+ *
+ * compute the finite-horizon discrete time LQR solution
+ * (Example: stabilize a linear time-varying system about a trajectory).
+ * The user can either provide the linearized system matrices, or alternatively a pointer to a derivatives instance
+ *
+ * The feedback law has form
+ * \f[
+ * u_{fb} = -K \cdot (x - x_{ref})
+ * \f]
+ *
+ * @tparam STATE_DIM system state dimension
+ * @tparam CONTROL_DIM system input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class FHDTLQR
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ typedef core::StateVectorArray<STATE_DIM, SCALAR> state_vector_array_t;
+ typedef core::ControlVectorArray<CONTROL_DIM, SCALAR> control_vector_array_t;
+ typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> state_matrix_array_t;
+
+ typedef ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> control_feedback_array_t;
+ typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> control_gain_matrix_array_t;
+
+
+ //! Constructor
+ /*!
+ * @param costFunction the cost function to be used for designing the TVLQR
+ */
+ FHDTLQR(std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> costFunction);
+
+ ~FHDTLQR();
+
+
+ //! design a time-varying feedback trajectory using user-provided matrices A and B
+ /*!
+ *
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param A
+ * trajectory of system matrices A = df/dx
+ * @param B
+ * trajectory of system matrices B = df/du
+ * @param dt
+ * sampling time
+ * @param K
+ * trajectory of resulting control feedback arrays
+ * @param performNumericalChecks
+ * (optional) perform some numerical checks while solving for K
+ */
+ void designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks = true);
+
+
+ //! design a time-varying feedback trajectory using a user-provided derivative-pointer
+ /*!
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param derivatives
+ * shared_ptr to a LinearSystem, allowing to compute A and B
+ * @param dt
+ * sampling time
+ * @param K
+ * trajectory of resulting control feedback arrays
+ * @param performNumericalChecks
+ * (optional) perform some numerical checks while solving for K
+ */
+ void designController(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> derivatives,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks = true);
+
+
+private:
+ //! compute trajectories of A and B matrices along the given reference trajectory using the user-provided derivative instance
+ /*!
+ *
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param N
+ * number of discrete sampling points
+ * @param dt
+ * sampling time dt
+ * @param derivatives
+ * user-provided derivatives
+ * @param A
+ * resulting linear state matrices A
+ * @param B
+ * resulting linear state-input matrices B
+ */
+ void linearizeModel(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ size_t N,
+ SCALAR dt,
+ std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>>& derivatives,
+ state_matrix_array_t& A,
+ control_gain_matrix_array_t& B);
+
+
+ //! solve for the LQR feedback gains
+ /*!
+ *
+ * @param x_trajectory
+ * state reference trajectory
+ * @param u_trajectory
+ * control reference trajectory
+ * @param A
+ * given linear state matrices A
+ * @param B
+ * given linear state-input matrices B
+ * @param N
+ * number of discrete sampling points
+ * @param dt
+ * sampling time
+ * @param K
+ * the resulting array of state-feedback matrices
+ * @param performNumericalChecks
+ * (optional) perform some numerical checks while solving
+ */
+ void solve(const state_vector_array_t& x_trajectory,
+ const control_vector_array_t& u_trajectory,
+ const state_matrix_array_t& A,
+ const control_gain_matrix_array_t& B,
+ size_t N,
+ SCALAR dt,
+ control_feedback_array_t& K,
+ bool performNumericalChecks = true);
+
+
+ std::shared_ptr<CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>>
+ costFunction_; //! a quadratic costfunction for solving the optimal control problem
+ DynamicRiccatiEquation<STATE_DIM, CONTROL_DIM, SCALAR> ricattiEq_; //! the Riccati Equations
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/LQR-impl.hpp b/ct_optcon/include/ct/optcon/lqr/LQR-impl.hpp
new file mode 100644
index 0000000..c4cb757
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/LQR-impl.hpp
@@ -0,0 +1,78 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+#include <matlabCppInterface/Engine.hpp>
+#endif
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool LQR<STATE_DIM, CONTROL_DIM>::compute(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool RisDiagonal,
+ bool solveRiccatiIteratively)
+{
+ control_matrix_t R_inverse;
+ state_matrix_t P;
+
+ bool success = care_.solve(Q, R, A, B, P, RisDiagonal, R_inverse, solveRiccatiIteratively);
+
+ K = (R_inverse * (B.transpose() * P));
+
+ return success;
+}
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool LQR<STATE_DIM, CONTROL_DIM>::computeMatlab(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool checkControllability)
+{
+ if (!matlabEngine_.isInitialized())
+ matlabEngine_.initialize();
+
+ matlabEngine_.put("Q", Q);
+ matlabEngine_.put("R", R);
+ matlabEngine_.put("A", A);
+ matlabEngine_.put("B", B);
+
+ if (checkControllability)
+ {
+ matlabEngine_.executeCommand("Co=ctrb(A,B);");
+ matlabEngine_.executeCommand("unco=length(A)-rank(Co);");
+ int uncontrollableStates = 0;
+ matlabEngine_.get("unco", uncontrollableStates);
+
+ if (uncontrollableStates > 0)
+ {
+ std::cout << "System is not controllable, # uncontrollable states: " << uncontrollableStates << std::endl;
+ }
+ }
+
+ matlabEngine_.executeCommand("[K,~,~] = lqr(A,B,Q,R);");
+
+ Eigen::MatrixXd Kmatlab;
+ matlabEngine_.get("K", Kmatlab);
+
+ K = Kmatlab;
+
+ return true;
+}
+#endif //USE_MATLAB_CPP_INTERFACE
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/LQR.hpp b/ct_optcon/include/ct/optcon/lqr/LQR.hpp
new file mode 100644
index 0000000..d222aff
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/LQR.hpp
@@ -0,0 +1,88 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "riccati/CARE.hpp"
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+#include <matlabCppInterface/Engine.hpp>
+#endif
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *
+ * \brief continuous-time infinite-horizon LQR
+ *
+ * Implements continous-time infinite-horizon LQR.
+ * Resulting feedback law will take the form
+ * \f[
+ * u_{fb} = -K \cdot (x - x_{ref})
+ * \f]
+ *
+ * @tparam STATE_DIM
+ * @tparam CONTROL_DIM
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+class LQR
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<double, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ //! design the infinite-horizon LQR controller.
+ /*!
+ * @param Q state-weighting matrix
+ * @param R control input weighting matrix
+ * @param A linear system dynamics matrix A
+ * @param B linear system dynamics matrix B
+ * @param K control feedback matrix K (to be designed)
+ * @param RisDiagonal set to true if R is a diagonal matrix (efficiency boost)
+ * @param solveRiccatiIteratively
+ * use closed-form solution of the infinite-horizon Riccati Equation
+ * @return success
+ */
+ bool compute(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool RisDiagonal = false,
+ bool solveRiccatiIteratively = false);
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+ //! design the LQR controller in MATLAB
+ /*!
+ * Note that this controller should be exactly the same
+ */
+ bool computeMatlab(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool checkControllability = false);
+#endif //USE_MATLAB_CPP_INTERFACE
+
+
+private:
+ CARE<STATE_DIM, CONTROL_DIM> care_; // continuous-time algebraic riccati equation
+
+#ifdef USE_MATLAB_CPP_INTERFACE
+ matlab::Engine matlabEngine_;
+#endif
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/CARE-impl.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/CARE-impl.hpp
new file mode 100644
index 0000000..9848704
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/CARE-impl.hpp
@@ -0,0 +1,213 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+CARE<STATE_DIM, CONTROL_DIM>::CARE()
+{
+ // we have to find the optimal work size of schur reordering
+ schur_matrix_t T;
+ schur_matrix_t U;
+
+ int SELECT[2 * STATE_DIM];
+ int N = 2 * STATE_DIM;
+ double WR[T.ColsAtCompileTime];
+ double WI[T.ColsAtCompileTime];
+ int MS;
+ double S;
+ double SEP;
+ double WORKDUMMY[1];
+ int LWORK = -1;
+ int IWORKQUERY[1];
+ int LIWORK = -1;
+ int INFO = 0;
+ int TCols = schur_matrix_t::ColsAtCompileTime;
+
+#ifdef CT_USE_LAPACK
+ dtrsen_("N", "V", &SELECT[0], &TCols, T.data(), &N, U.data(), &N, &WR[0], &WI[0], &MS, &S, &SEP, WORKDUMMY, &LWORK,
+ &IWORKQUERY[0], &LIWORK, &INFO);
+
+ LWORK_ = WORKDUMMY[0] + 32;
+ LIWORK_ = IWORKQUERY[0] + 32;
+
+ WORK_.resize(LWORK_);
+ IWORK_.resize(LIWORK_);
+
+ if (INFO != 0)
+ {
+ std::cout << "Lapack invocation of dtrsen failed!" << std::endl;
+ exit(-1);
+ }
+#endif
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool CARE<STATE_DIM, CONTROL_DIM>::solve(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t& P,
+ bool RisDiagonal,
+ control_matrix_t& R_inverse,
+ bool useIterativeSolver)
+{
+ if (RisDiagonal)
+ {
+ R_inverse.setZero();
+ R_inverse.diagonal().noalias() = R.diagonal().cwiseInverse();
+ }
+ else
+ {
+ R_inverse.noalias() = R.inverse();
+ }
+
+ schur_matrix_t M;
+ M << A, -B * R_inverse * B.transpose(), -Q, -A.transpose();
+
+ if (useIterativeSolver)
+ return solveSchurIterative(M, P);
+ else
+ return solveSchurDirect(M, P);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+typename CARE<STATE_DIM, CONTROL_DIM>::state_matrix_t CARE<STATE_DIM, CONTROL_DIM>::computeSteadyStateRiccatiMatrix(
+ const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ const bool RisDiagonal,
+ const bool useIterativeSolver)
+{
+ state_matrix_t P;
+ control_matrix_t Rinv;
+
+ solve(Q, R, A, B, P, RisDiagonal, Rinv, useIterativeSolver);
+
+ return P;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool CARE<STATE_DIM, CONTROL_DIM>::solveSchurIterative(const schur_matrix_t& M,
+ state_matrix_t& P,
+ double epsilon,
+ int maxIterations)
+{
+ bool converged = false;
+
+ schur_matrix_t Mlocal = M;
+
+ int iterations = 0;
+ while (!converged)
+ {
+ if (iterations > maxIterations)
+ return false;
+
+ schur_matrix_t Mdiff = Mlocal - Mlocal.inverse();
+
+ schur_matrix_t Mnew = Mlocal - 0.5 * Mdiff;
+
+ converged = Mnew.isApprox(Mlocal, epsilon);
+
+ Mlocal = Mnew;
+
+ iterations++;
+ }
+
+ /* break down W and extract W11 W12 W21 W22 (what is the size of these?) */
+ state_matrix_t M11(Mlocal.template block<STATE_DIM, STATE_DIM>(0, 0));
+ state_matrix_t M12(Mlocal.template block<STATE_DIM, STATE_DIM>(0, STATE_DIM));
+ state_matrix_t M21(Mlocal.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0));
+ state_matrix_t M22(Mlocal.template block<STATE_DIM, STATE_DIM>(STATE_DIM, STATE_DIM));
+
+ /* find M and N using the elements of W */
+ factor_matrix_t U;
+ factor_matrix_t V;
+
+ U.template block<STATE_DIM, STATE_DIM>(0, 0) = M12;
+ U.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0) = M22 + state_matrix_t::Identity();
+
+ V.template block<STATE_DIM, STATE_DIM>(0, 0) = M11 + state_matrix_t::Identity();
+ V.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0) = M21;
+
+
+ /* Solve for S from the equation MS=N */
+ FullPivLU_.compute(U);
+
+ P = FullPivLU_.solve(-V);
+
+ return true;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+bool CARE<STATE_DIM, CONTROL_DIM>::solveSchurDirect(const schur_matrix_t& M, state_matrix_t& P)
+{
+#ifdef CT_USE_LAPACK
+ const bool computeU = true;
+ schur_.compute(M, computeU);
+
+ if (schur_.info() != Eigen::Success)
+ {
+ throw std::runtime_error(
+ "LQR Schur computation failed. Most likely problem is set up wrongly or not solvable.");
+ }
+
+ schur_matrix_t U(schur_.matrixU());
+ schur_matrix_t T(schur_.matrixT());
+
+ int SELECT[2 * STATE_DIM];
+ double WR[2 * STATE_DIM];
+ double WI[2 * STATE_DIM];
+ int MS;
+ double S;
+ double SEP;
+ int INFO = 0;
+ int N = 2 * STATE_DIM;
+
+ for (size_t i = 0; i < 2 * STATE_DIM; i++)
+ {
+ // check if last row or eigenvalue is complex (2x2 block)
+ if (i == (2 * STATE_DIM - 1) || std::abs(T(i + 1, i)) < 1e-12)
+ {
+ SELECT[i] = static_cast<int>(T(i, i) < 0);
+ }
+ else
+ {
+ // we have a complex block
+ SELECT[i] = static_cast<int>((T(i, i) + T(i + 1, i + 1)) / 2.0 < 0);
+ SELECT[i + 1] = SELECT[i];
+ i++;
+ }
+ }
+
+ dtrsen_("N", "V", &SELECT[0], &N, T.data(), &N, U.data(), &N, &WR[0], &WI[0], &MS, &S, &SEP, WORK_.data(), &LWORK_,
+ IWORK_.data(), &LIWORK_, &INFO);
+
+ const state_matrix_t& U11 = U.template block<STATE_DIM, STATE_DIM>(0, 0);
+ const state_matrix_t& U21 = U.template block<STATE_DIM, STATE_DIM>(STATE_DIM, 0);
+
+ // solve here for better numerical properties
+ P.noalias() = U21 * U11.inverse();
+
+ if (INFO != 0)
+ {
+ return false;
+ }
+
+ return true;
+#else
+ throw std::runtime_error(
+ "solveSchurDirect() in CARE can only be used if the lapack library is installed on your system.");
+#endif
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/CARE.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/CARE.hpp
new file mode 100644
index 0000000..b47f882
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/CARE.hpp
@@ -0,0 +1,100 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <iostream>
+
+// Schur reordering from Lapack
+#ifdef CT_USE_LAPACK
+extern "C" void dtrsen_(const char* JOB,
+ const char* COMPQ,
+ const int* SELECT,
+ const int* N,
+ const double* T,
+ const int* LDT,
+ const double* Q,
+ const int* LDQ,
+ double* WR,
+ double* WI,
+ int* M,
+ double* S,
+ double* SEP,
+ double* WORK,
+ const int* LWORK,
+ int* IWORK,
+ const int* LIWORK,
+ int* INFO);
+#endif
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *+
+ * \brief Continuous-Time Algebraic Riccati Equation
+ *
+ * solves the continuous-time Infinite-Horizon Algebraic Riccati Equation
+ *
+ * @tparam STATE_DIM system state dimension
+ * @tparam CONTROL_DIM system control input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM>
+class CARE
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<double, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<double, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<double, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ typedef Eigen::Matrix<double, 2 * STATE_DIM, 2 * STATE_DIM> schur_matrix_t;
+ typedef Eigen::Matrix<double, 2 * STATE_DIM, STATE_DIM> factor_matrix_t;
+
+ CARE();
+
+ // Implementation using the Schur-Method
+ // This is numerically more stable and should be preferred over the naive implementation
+ bool solve(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t& P,
+ bool RisDiagonal,
+ control_matrix_t& R_inverse,
+ bool useIterativeSolver = false);
+
+ state_matrix_t computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ const bool RisDiagonal = false,
+ const bool useIterativeSolver = false);
+
+private:
+ bool solveSchurIterative(const schur_matrix_t& M,
+ state_matrix_t& P,
+ double epsilon = 1e-6,
+ int maxIterations = 100000);
+
+ bool solveSchurDirect(const schur_matrix_t& M, state_matrix_t& P);
+
+ Eigen::RealSchur<schur_matrix_t> schur_;
+ Eigen::FullPivLU<factor_matrix_t> FullPivLU_;
+
+ // Lapack
+ int LWORK_;
+ int LIWORK_;
+
+ Eigen::VectorXd WORK_;
+ Eigen::VectorXi IWORK_;
+};
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/DARE-impl.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/DARE-impl.hpp
new file mode 100644
index 0000000..30484a4
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/DARE-impl.hpp
@@ -0,0 +1,71 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+DARE<STATE_DIM, CONTROL_DIM, SCALAR>::DARE()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DARE<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+DARE<STATE_DIM, CONTROL_DIM, SCALAR>::computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool verbose,
+ const SCALAR eps,
+ size_t maxIter)
+{
+ // If initialized to zero, in the next iteration P becomes Q. Directly initializing to Q avoids the extra iteration.
+ return computeSteadyStateRiccatiMatrix(Q, R, A, B, Q, K, verbose, eps, maxIter);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+typename DARE<STATE_DIM, CONTROL_DIM, SCALAR>::state_matrix_t
+DARE<STATE_DIM, CONTROL_DIM, SCALAR>::computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t P,
+ control_feedback_t& K,
+ bool verbose,
+ const SCALAR eps,
+ size_t maxIter)
+{
+ state_matrix_t P_prev;
+ size_t numIter = 0;
+
+ SCALAR diff = 1;
+
+ while (diff >= eps && numIter < maxIter)
+ {
+ P_prev = P;
+ dynamicRDE_.iterateRobust(Q, R, A, B, P, K);
+ diff = (P - P_prev).cwiseAbs().maxCoeff();
+ if (!K.allFinite())
+ throw std::runtime_error("DARE : Failed to converge");
+ numIter++;
+ }
+
+ if (diff >= eps) throw std::runtime_error("DARE : Failed to converge");
+
+ if (verbose)
+ {
+ std::cout << "DARE : converged after " << numIter << " iterations out of a maximum of " << maxIter << std::endl;
+ std::cout << "Resulting K: " << K << std::endl;
+ }
+
+ return P;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/DARE.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/DARE.hpp
new file mode 100644
index 0000000..2255eb6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/DARE.hpp
@@ -0,0 +1,84 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include "DynamicRiccatiEquation.hpp"
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *+
+ * \brief Discrete-Time Algebraic Riccati Equation
+ *
+ * solves the discrete-time Infinite-Horizon Algebraic Riccati Equation iteratively
+ *
+ * @tparam STATE_DIM system state dimension
+ * @tparam CONTROL_DIM system control input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DARE
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+ DARE();
+
+ /*! compute the discrete-time steady state Riccati-Matrix
+ * this method iterates over the time-varying discrete-time Riccati Equation to compute the steady-state solution.
+ * @param Q state weight
+ * @param R control weight
+ * @param A discrete-time linear system matrix A
+ * @param B discrete-time linear system matrix B
+ * @param verbose print additional information
+ * @param eps treshold to stop iterating
+ * @return steady state riccati matrix P
+ */
+ state_matrix_t computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ control_feedback_t& K,
+ bool verbose = false,
+ const SCALAR eps = 1e-6,
+ size_t maxIter = 1000);
+
+ /*! compute the discrete-time steady state Riccati-Matrix with warm initialization of P matrix
+ * this method iterates over the time-varying discrete-time Riccati Equation to compute the steady-state solution.
+ * @param Q state weight
+ * @param R control weight
+ * @param A discrete-time linear system matrix A
+ * @param B discrete-time linear system matrix B
+ * @param P warm initialized P matrix
+ * @param verbose print additional information
+ * @param eps treshold to stop iterating
+ * @return steady state riccati matrix P
+ */
+ state_matrix_t computeSteadyStateRiccatiMatrix(const state_matrix_t& Q,
+ const control_matrix_t& R,
+ const state_matrix_t& A,
+ const control_gain_matrix_t& B,
+ state_matrix_t P,
+ control_feedback_t& K,
+ bool verbose = false,
+ const SCALAR eps = 1e-6,
+ size_t maxIter = 1000);
+
+
+private:
+ DynamicRiccatiEquation<STATE_DIM, CONTROL_DIM> dynamicRDE_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp b/ct_optcon/include/ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp
new file mode 100644
index 0000000..a22c117
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp
@@ -0,0 +1,120 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \ingroup LQR
+ *+
+ * \brief Dynamic Riccati Equation
+ *
+ * solves the Dynamic Algebraic Riccati Equation
+ *
+ * @tparam STATE_DIM the system state dimension
+ * @tparam CONTROL_DIM the system control input dimension
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class DynamicRiccatiEquation
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, 1> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM> control_gain_matrix_t;
+ typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> control_feedback_t;
+
+
+ DynamicRiccatiEquation() : epsilon_(1e-5), eigenvalueSolver_(CONTROL_DIM){};
+
+ ~DynamicRiccatiEquation(){};
+
+ // Naive, direct literal implementation
+ // computes P = Q + A^T ( P - P B ( R + B^T P B )^-1 B^T P) A.
+ // = Q + A^T ( P - P B H_inverse B^T P) A
+ // = Q + A^T S A
+ // where H_inverse = (R + B^T P B)^-1 and S = P - P B H_inverse B^T * P
+ // i.e. P is altered and H_inverse is returned for convenience and use in other functions
+ void iterateNaive(const state_matrix_t& Q_n,
+ const control_matrix_t& R_n,
+ const state_matrix_t& A_n,
+ const control_gain_matrix_t& B_n,
+ state_matrix_t& P_np1,
+ control_feedback_t& K_n)
+ {
+ control_matrix_t H_n = R_n;
+ H_n.noalias() += B_n.transpose() * P_np1 * B_n;
+ control_matrix_t H_n_inverse = H_n.inverse();
+
+ //std::cout << "H_n (naive)" << std::endl << H_n << std::endl;
+ //std::cout << "H_n_inverse (naive)" << std::endl << H_n.inverse() << std::endl;
+
+ K_n.noalias() = -1.0 * H_n_inverse * B_n.transpose() * P_np1 * A_n;
+
+ // here we compute P_n
+ P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
+ P_np1.noalias() -= K_n.transpose() * H_n * K_n;
+
+ P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
+ }
+
+
+ void iterateRobust(const state_matrix_t& Q_n,
+ const control_matrix_t& R_n,
+ const state_matrix_t& A_n,
+ const control_gain_matrix_t& B_n,
+ state_matrix_t& P_np1,
+ control_feedback_t& K_n)
+ {
+ control_matrix_t H_n = R_n;
+ H_n.noalias() += B_n.transpose() * P_np1 * B_n;
+ H_n = (H_n + H_n.transpose()).eval() / 2.0;
+
+ // compute eigenvalues with eigenvectors enabled
+ eigenvalueSolver_.compute(H_n, true);
+ const control_matrix_t& V = eigenvalueSolver_.eigenvectors().real();
+ const control_vector_t& lambda = eigenvalueSolver_.eigenvalues().real();
+ assert(eigenvalueSolver_.eigenvectors().imag().norm() < 1e-7 && "Eigenvectors not real");
+ assert(eigenvalueSolver_.eigenvalues().imag().norm() < 1e-7 && "Eigenvalues is not real");
+ // Corrected Eigenvalue Matrix
+ control_matrix_t D = control_matrix_t::Zero();
+ // make D positive semi-definite (as described in IV. B.)
+ D.diagonal() = lambda.cwiseMax(control_vector_t::Zero()) + epsilon_ * control_vector_t::Ones();
+
+ assert((V.transpose() - V.inverse()).norm() < 1e-5 && "WARNING: V transpose and V inverse are not similar!");
+
+ // reconstruct H
+ H_n.noalias() = V * D * V.transpose();
+
+ // invert D
+ control_matrix_t D_inverse = control_matrix_t::Zero();
+ // eigenvalue-wise inversion
+ D_inverse.diagonal() = D.diagonal().cwiseInverse();
+ control_matrix_t H_n_inverse = V * D_inverse * V.transpose();
+
+ K_n.noalias() = -1.0 * H_n_inverse * (B_n.transpose() * P_np1 * A_n);
+
+ // here we compute P_n
+ P_np1 = Q_n + (A_n.transpose() * P_np1 * A_n);
+ P_np1.noalias() -= K_n.transpose() * H_n * K_n;
+
+ P_np1 = (P_np1 + P_np1.transpose()).eval() / 2.0;
+ }
+
+
+private:
+ SCALAR epsilon_;
+ Eigen::EigenSolver<control_matrix_t> eigenvalueSolver_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/matlab.hpp b/ct_optcon/include/ct/optcon/matlab.hpp
new file mode 100644
index 0000000..7f3fe1c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/matlab.hpp
@@ -0,0 +1,24 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef MATLAB_FULL_LOG
+#define MATLAB
+#endif // MATLAB_FULL_LOG
+
+#ifdef MATLAB
+#include <matlabCppInterface/MatFile.hpp>
+#else //MATLAB
+namespace matlab {
+//! a dummy class which is created for compatibility reasons if the MATLAB flag is not set.
+class MatFile
+{
+public:
+ MatFile() {}
+};
+} // namespace matlab
+#endif //MATLAB
diff --git a/ct_optcon/include/ct/optcon/mpc/MPC-impl.h b/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
new file mode 100644
index 0000000..81adecd
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
@@ -0,0 +1,350 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <typename OPTCON_SOLVER>
+MPC<OPTCON_SOLVER>::MPC(const OptConProblem_t& problem,
+ const typename OPTCON_SOLVER::Settings_t& solverSettings,
+ const mpc_settings& mpcsettings,
+ std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> customPolicyHandler,
+ std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> customTimeHorizon)
+ :
+
+ solver_(problem, solverSettings),
+ mpc_settings_(mpcsettings),
+ dynamics_(problem.getNonlinearSystem()->clone()),
+ forwardIntegrator_(dynamics_, mpcsettings.stateForwardIntegratorType_),
+ firstRun_(true),
+ runCallCounter_(0),
+ policyHandler_(new PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>())
+{
+ checkSettings(mpcsettings);
+
+ // =========== INIT WARM START STRATEGY =============
+
+ if (mpc_settings_.coldStart_ == false)
+ {
+ if (customPolicyHandler)
+ {
+ std::cout << "Initializing MPC with a custom policy handler (warmstarter) provided by the user."
+ << std::endl;
+ policyHandler_ = customPolicyHandler;
+ }
+ else
+ {
+ if (std::is_base_of<NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, Scalar_t>, OPTCON_SOLVER>::value)
+ {
+ // default policy handler for standard discrete-time iLQG implementation
+ policyHandler_ = std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>>(
+ new StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, Scalar_t>(solverSettings.dt));
+ }
+ else
+ {
+ throw std::runtime_error(
+ "ERROR in MPC Constructor -- no default warm start strategy available for the selected "
+ "controller.");
+ }
+ }
+ }
+
+
+ // ============== INIT MPC TIME HORIZON STRATEGY ==============
+ if (customTimeHorizon)
+ {
+ std::cout << "Initializing MPC with a custom time-horizon strategy provided by the user." << std::endl;
+ timeHorizonStrategy_ = customTimeHorizon;
+ }
+ else
+ {
+ const Scalar_t initTimeHorizon = solver_.getTimeHorizon();
+
+ timeHorizonStrategy_ = std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>>(
+ new tpl::MpcTimeHorizon<Scalar_t>(mpc_settings_, initTimeHorizon));
+ }
+
+
+ // ============== INIT MPC TIME KEEPER ==============
+ timeKeeper_ = tpl::MpcTimeKeeper<Scalar_t>(timeHorizonStrategy_, mpc_settings_);
+}
+
+
+template <typename OPTCON_SOLVER>
+OPTCON_SOLVER& MPC<OPTCON_SOLVER>::getSolver()
+{
+ return solver_;
+};
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::setTimeHorizonStrategy(std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy)
+{
+ timeHorizonStrategy_ = timeHorizonStrategy;
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::setInitialGuess(const Policy_t& initGuess)
+{
+ solver_.setInitialGuess(initGuess);
+ policyHandler_->setPolicy(initGuess);
+ currentPolicy_ = initGuess;
+}
+
+
+template <typename OPTCON_SOLVER>
+bool MPC<OPTCON_SOLVER>::timeHorizonReached()
+{
+ return timeKeeper_.finalPointReached();
+}
+
+
+template <typename OPTCON_SOLVER>
+const typename MPC<OPTCON_SOLVER>::Scalar_t MPC<OPTCON_SOLVER>::timeSinceFirstSuccessfulSolve(const Scalar_t& extTime)
+{
+ return timeKeeper_.timeSinceFirstSuccessfulSolve(extTime);
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::doForwardIntegration(const Scalar_t& t_forward_start,
+ const Scalar_t& t_forward_stop,
+ core::StateVector<STATE_DIM, Scalar_t>& x_start,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController)
+{
+ if (mpc_settings_.stateForwardIntegration_ == true)
+ {
+ if (forwardIntegrationController)
+ {
+ // ... either with a given third-party controller
+ integrateForward(t_forward_start, t_forward_stop, x_start, forwardIntegrationController);
+ }
+ else
+ {
+ // ... or with the controller obtained from the solver (solution of last mpc-run).
+ std::shared_ptr<Policy_t> prevController(new Policy_t(currentPolicy_));
+ integrateForward(t_forward_start, t_forward_stop, x_start, prevController);
+ }
+ }
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::prepareIteration(const Scalar_t& extTime)
+{
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: started to prepare MPC iteration() " << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ runCallCounter_++;
+
+ const Scalar_t currTimeHorizon = solver_.getTimeHorizon();
+ Scalar_t newTimeHorizon;
+
+
+ if (firstRun_)
+ timeKeeper_.initialize();
+
+ timeKeeper_.computeNewTimings(extTime, currTimeHorizon, newTimeHorizon, t_forward_start_, t_forward_stop_);
+
+ // update the Optimal Control Solver with new time horizon and state information
+ solver_.changeTimeHorizon(newTimeHorizon);
+
+
+ // Calculate new initial guess / warm-starting policy
+ policyHandler_->designWarmStartingPolicy(t_forward_stop_, newTimeHorizon, currentPolicy_);
+
+ // todo: remove this after through testing
+ if (t_forward_stop_ < t_forward_start_)
+ throw std::runtime_error("ERROR: t_forward_stop < t_forward_start is impossible.");
+
+
+ /**
+ * re-initialize the OptConSolver and solve the optimal control problem.
+ */
+ solver_.setInitialGuess(currentPolicy_);
+
+ solver_.prepareMPCIteration();
+}
+
+
+template <typename OPTCON_SOLVER>
+bool MPC<OPTCON_SOLVER>::finishIteration(const core::StateVector<STATE_DIM, Scalar_t>& x,
+ const Scalar_t x_ts,
+ Policy_t& newPolicy,
+ Scalar_t& newPolicy_ts,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController)
+{
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: started mpc finish Iteration() with state-timestamp " << x_ts << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ timeKeeper_.startDelayMeasurement(x_ts);
+
+ // initialize the time-stamp for policy which is to be designed
+ newPolicy_ts = x_ts;
+
+ core::StateVector<STATE_DIM, Scalar_t> x_start = x;
+
+ if (!firstRun_)
+ doForwardIntegration(t_forward_start_, t_forward_stop_, x_start, forwardIntegrationController);
+
+
+ solver_.changeInitialState(x_start);
+
+ bool solveSuccessful = solver_.finishMPCIteration();
+
+ if (solveSuccessful)
+ {
+ newPolicy_ts = newPolicy_ts + (t_forward_stop_ - t_forward_start_);
+
+ // get optimized policy and state trajectory from OptConSolver
+ currentPolicy_ = solver_.getSolution();
+
+ // obtain the time which passed since the previous successful solve
+ Scalar_t dtp = timeKeeper_.timeSincePreviousSuccessfulSolve(x_ts);
+
+ // post-truncation may be an option of the solve-call took longer than the estimated delay
+ if (mpc_settings_.postTruncation_)
+ {
+ if (dtp > t_forward_stop_ && !firstRun_)
+ {
+ // the time-difference to be account for by post-truncation
+ Scalar_t dt_post_truncation = dtp - t_forward_stop_;
+
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: additional post-truncation about " << dt_post_truncation << " [sec]."
+ << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ // the time which was effectively truncated away (e.g. discrete-time case)
+ Scalar_t dt_truncated_eff;
+
+ policyHandler_->truncateSolutionFront(dt_post_truncation, currentPolicy_, dt_truncated_eff);
+
+ // update policy timestamp with the truncated time
+ newPolicy_ts += dt_truncated_eff;
+ }
+ else if (t_forward_stop_ >= dtp && !firstRun_)
+ {
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: controller opt faster than pre-integration horizon. Consider tuning "
+ "pre-integration. "
+ << std::endl;
+#endif //DEBUG_PRINT_MPC
+ }
+
+ } // post truncation
+
+ } // solve successful
+
+
+#ifdef DEBUG_PRINT_MPC
+ std::cout << "DEBUG_PRINT_MPC: start timestamp outgoing policy: " << newPolicy_ts << std::endl;
+ std::cout << "DEBUG_PRINT_MPC: ended finishIteration() " << std::endl << std::endl;
+#endif //DEBUG_PRINT_MPC
+
+ // update policy result
+ newPolicy = currentPolicy_;
+
+ // stop the delay measurement. This needs to be the last method called in finishIteration().
+ timeKeeper_.stopDelayMeasurement(x_ts);
+
+ // in the first run, the policy time-stamp needs to be shifted about the solving time
+ if (firstRun_)
+ {
+ newPolicy_ts = newPolicy_ts + timeKeeper_.getMeasuredDelay();
+ firstRun_ = false;
+ }
+
+ return solveSuccessful;
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::resetMpc(const Scalar_t& newTimeHorizon)
+{
+ firstRun_ = true;
+
+ runCallCounter_ = 0;
+
+ // reset the time horizon of the strategy
+ timeHorizonStrategy_->updateInitialTimeHorizon(newTimeHorizon);
+
+ solver_.changeTimeHorizon(newTimeHorizon);
+
+ timeKeeper_.initialize();
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::updateSettings(const mpc_settings& settings)
+{
+ checkSettings(settings);
+
+ if (settings.stateForwardIntegratorType_ != mpc_settings_.stateForwardIntegratorType_)
+ forwardIntegrator_ = ct::core::Integrator<STATE_DIM, Scalar_t>(dynamics_, settings.stateForwardIntegratorType_);
+
+ mpc_settings_ = settings;
+ timeKeeper_.updateSettings(settings);
+ timeHorizonStrategy_->updateSettings(settings);
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::printMpcSummary()
+{
+ std::cout << std::endl;
+ std::cout << "================ MPC Summary ================" << std::endl;
+ std::cout << "Number of MPC calls:\t\t\t" << runCallCounter_ << std::endl;
+
+ if (mpc_settings_.measureDelay_)
+ {
+ std::cout << "Max measured solving time [sec]:\t" << timeKeeper_.getMaxMeasuredDelay() << std::endl;
+ std::cout << "Min measured solving time [sec]:\t" << timeKeeper_.getMinMeasuredDelay() << std::endl;
+ std::cout << "Total sum of meas. delay [sec]: \t" << timeKeeper_.getSummedDelay() << std::endl;
+ std::cout << "Average measured delay [sec]: \t" << timeKeeper_.getSummedDelay() / runCallCounter_
+ << std::endl;
+ }
+ else
+ {
+ std::cout << "Used fixed delay[sec]: \t" << 0.000001 * mpc_settings_.fixedDelayUs_ << std::endl;
+ }
+
+ std::cout << "================ End Summary ================" << std::endl;
+ std::cout << std::endl;
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::integrateForward(const Scalar_t startTime,
+ const Scalar_t stopTime,
+ core::StateVector<STATE_DIM, Scalar_t>& state,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>>& controller)
+{
+ dynamics_->setController(controller);
+
+ int nSteps = std::max(0, (int)std::lround((stopTime - startTime) / mpc_settings_.stateForwardIntegration_dt_));
+
+ // adaptive pre-integration
+ forwardIntegrator_.integrate_n_steps(state, startTime, nSteps, mpc_settings_.stateForwardIntegration_dt_);
+}
+
+
+template <typename OPTCON_SOLVER>
+void MPC<OPTCON_SOLVER>::checkSettings(const mpc_settings& settings)
+{
+ // if external timing is active, internal delay measurement must be turned off
+ if (settings.useExternalTiming_ && settings.measureDelay_)
+ throw std::runtime_error(
+ "MPC: measuring delay internally contradicts using external Timing. Switch off one of those options.");
+}
+
+} //namespace optcon
+} //namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/MPC.h b/ct_optcon/include/ct/optcon/mpc/MPC.h
new file mode 100644
index 0000000..8a84461
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MPC.h
@@ -0,0 +1,256 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <type_traits>
+
+#include <ct/optcon/problem/OptConProblem.h>
+#include <ct/optcon/solver/OptConSolver.h>
+
+#include "MpcSettings.h"
+#include "MpcTimeKeeper.h"
+
+#include "policyhandler/PolicyHandler.h"
+#include "timehorizon/MpcTimeHorizon.h"
+
+#include <ct/optcon/solver/NLOptConSolver.hpp>
+#include "policyhandler/default/StateFeedbackPolicyHandler.h"
+
+//#define DEBUG_PRINT_MPC //! use this flag to enable debug printouts in the MPC implementation
+
+
+namespace ct {
+namespace optcon {
+
+
+/** \defgroup MPC MPC
+ *
+ * \brief Model Predictive Control Module
+ */
+
+/**
+ * \ingroup MPC
+ *+
+ * \brief Main MPC class.
+ *
+ * This MPC class allows to use any solver that derives from the OptConSolver base class in Model-Predictive-Control fashion.
+ * MPC will automatically construct the solver
+ *
+ * Main assumptions:
+ * This MPC class is deliberately designed such that the time-keeping is managed by itself. The main assumption is that the controller
+ * which is designed here, gets applied to the system instantaneously after the run() call is executed. Furthermore, we assume that all Optimal Control Problems start at
+ * time zero. This also applies to the cost- and the constraint functionals which are to be provided by the user.
+ *
+ * Sidenotes:
+ * between the calls to run(), the user can arbitrarily modify his cost-functions, etc. in order to change the problem.
+ *
+ * @param OPTCON_SOLVER
+ * the optimal control solver to be employed, for example SLQ or DMS
+ *
+ */
+template <typename OPTCON_SOLVER>
+class MPC
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_DIM = OPTCON_SOLVER::STATE_D;
+ static const size_t CONTROL_DIM = OPTCON_SOLVER::CONTROL_D;
+
+ static const size_t P_DIM = OPTCON_SOLVER::POS_DIM;
+ static const size_t V_DIM = OPTCON_SOLVER::VEL_DIM;
+
+ typedef typename OPTCON_SOLVER::Scalar_t Scalar_t;
+ typedef typename OPTCON_SOLVER::Policy_t Policy_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, Scalar_t> OptConProblem_t;
+
+
+ //! MPC solver constructor
+ /*!
+ *
+ * @param problem
+ * the optimal control problem set up by the user
+ * @param solverSettings
+ * settings class/struct for the optimal control solver of choice. Be sure to tune the solver settings such that they are suitable for MPC!
+ * @param mpcsettings
+ * mpc-specific settings, see class MpcSettings.h
+ * @param customPolicyHandler
+ * user-provided custom policy handler, which derives from base class 'PolicyHandler'.
+ * If not specified, MPC will use one of its default implementations or throw an error if there is no default match.
+ * @param customTimeHorizon
+ * user-provided custom time horizon strategy, which derives from base class 'MpcTimeHorizon'.
+ * If not specified, MPC will use one of its default implementations or throw an error if there is no default match.
+ */
+ MPC(const OptConProblem_t& problem,
+ const typename OPTCON_SOLVER::Settings_t& solverSettings,
+ const mpc_settings& mpcsettings = mpc_settings(),
+ std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> customPolicyHandler = nullptr,
+ std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> customTimeHorizon = nullptr);
+
+
+ //! Allows access to the solver member, required mainly for unit testing.
+ /*!
+ * @return reference to the optimal control problem solver
+ */
+ OPTCON_SOLVER& getSolver();
+
+
+ //! Additional method to insert a custom time horizon strategy, independent from the constructor
+ /*!
+ * @param timeHorizonStrategy
+ * the time horizon strategy provided by the user
+ */
+ void setTimeHorizonStrategy(std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy);
+
+ //! set a new initial guess for the policy
+ /**
+ * @param initGuess
+ */
+ void setInitialGuess(const Policy_t& initGuess);
+
+ //! Check if final time horizon for this task was reached
+ bool timeHorizonReached();
+
+
+ //! retrieve the time that elapsed since the first successful solve() call to an Optimal Control Problem
+ /*!
+ * @param the external time stamp
+ * @return time elapsed, the returned time can be used externally, for example to update cost functions
+ */
+ const Scalar_t timeSinceFirstSuccessfulSolve(const Scalar_t& extTime);
+
+
+ //! perform forward integration of the measured system state, to compensate for expected or already occurred time lags
+ /*!
+ * State forward integration
+ * @param t_forward_start
+ * time where forward integration starts
+ * @param t_forward_stop
+ * time where forward integration stops
+ * @param x_start
+ * initial state for forward integration, gets overwritten with forward-integrated state
+ * @param forwardIntegrationController
+ * (optional) external controller for forward integration
+ *
+ * \warning The effect of the integration will vanish one the MPC frequency is higher than the sampling frequency
+ */
+ void doForwardIntegration(const Scalar_t& t_forward_start,
+ const Scalar_t& t_forward_stop,
+ core::StateVector<STATE_DIM, Scalar_t>& x_start,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController =
+ nullptr);
+
+
+ /*!
+ * Prepare MPC iteration
+ * @param ext_ts the current external time
+ */
+ void prepareIteration(const Scalar_t& ext_ts);
+
+
+ //! finish MPC iteration
+ /*!
+ * @param x
+ * current system state
+ * @param x_ts
+ * time stamp of the current state (external time in seconds)
+ * @param newPolicy
+ * the new policy calculated based on above state, the timing info and the underlying OptConProblem
+ * @param newPolicy_ts
+ * time stamp of the resulting policy. This indicates when the policy is supposed to start being applied, relative to
+ * the user-provided state-timestamp x_ts.
+ * @param forwardIntegrationController
+ * optional input: in some scenarios, we wish to use a different kind controller for forward integrating the system than the one we are optimizing
+ * Such a controller can be handed over here as additional argument. If set to empty, MPC uses its own optimized controller from
+ * the last iteration, thus assuming perfect control trajectory tracking.
+ * @return true if solve was successful, false otherwise.
+ */
+ bool finishIteration(const core::StateVector<STATE_DIM, Scalar_t>& x,
+ const Scalar_t x_ts,
+ Policy_t& newPolicy,
+ Scalar_t& newPolicy_ts,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController =
+ nullptr);
+
+
+ //! reset the mpc problem and provide new problem time horizon (mandatory)
+ void resetMpc(const Scalar_t& newTimeHorizon);
+
+
+ //! update the mpc settings in all instances (main class, time keeper class, etc)
+ /*!
+ * update the mpc settings in all instances
+ * @param settings
+ * the new mpc settings provided by the user.
+ */
+ void updateSettings(const mpc_settings& settings);
+
+
+ //! printout simple statistical data
+ void printMpcSummary();
+
+
+private:
+ //! state forward propagation (for delay compensation)
+ /*!
+ * Perform forward integration about the given prediction horizon.
+ * - uses an arbitrary controller given, which is important for hierarchical setups where the actual controller may be refined further
+ * @param startTime
+ * time where forward integration starts w.r.t. the current policy
+ * @param stopTime
+ * time where forward integration stops w.r.t. the current policy
+ * @param state
+ * state to be forward propagated
+ * @param controller
+ * the controller to be used for forward propagation
+ */
+ void integrateForward(const Scalar_t startTime,
+ const Scalar_t stopTime,
+ core::StateVector<STATE_DIM, Scalar_t>& state,
+ const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>>& controller);
+
+ void checkSettings(const mpc_settings& settings);
+
+ //! timings for pre-integration
+ Scalar_t t_forward_start_;
+ Scalar_t t_forward_stop_;
+
+ //! optimal control solver employed for mpc
+ OPTCON_SOLVER solver_;
+
+ //! currently optimal policy, initial guess respectively
+ Policy_t currentPolicy_;
+
+ //! policy handler, which takes care of warm-starting
+ std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> policyHandler_;
+
+ //! time horizon strategy, e.g. receding horizon optimal control
+ std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy_;
+
+ //! time keeper
+ tpl::MpcTimeKeeper<Scalar_t> timeKeeper_;
+
+ //! mpc settings
+ mpc_settings mpc_settings_;
+
+ //! true for first run
+ bool firstRun_;
+
+ //! dynamics instance for forward integration
+ typename OPTCON_SOLVER::OptConProblem_t::DynamicsPtr_t dynamics_;
+
+ //! integrator for forward integration
+ ct::core::Integrator<STATE_DIM, Scalar_t> forwardIntegrator_;
+
+ //! counter which gets incremented at every call of the run() method
+ size_t runCallCounter_;
+};
+
+
+} //namespace optcon
+} //namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/MpcSettings.h b/ct_optcon/include/ct/optcon/mpc/MpcSettings.h
new file mode 100644
index 0000000..2c9b97b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MpcSettings.h
@@ -0,0 +1,172 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+//! select a mode in which MPC is supposed to run
+/*!
+ * There are four default implementations for time horizon strategies in MPC, which can be selected using the following "MPC_MODE" enum.
+ *
+ * - FIXED_FINAL_TIME
+ * plan until a fixed final time T is reached. Time Horizon will continuously shrink and eventually be zero.
+ *
+ * - FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON
+ * this option continuously shrinks the time horizon from the initil time horizon until some minimum time is reached.
+ * This minimum time can be specified in the mpc_settings struct as "minimumTimeHorizonMpc_"
+ *
+ * - CONSTANT_RECEDING_HORIZON
+ * Time Horizon remains constant for all times and is equal to the initial time horizon specified in the optimal control problem.
+ *
+ * - RECEDING_HORIZON_WITH_FIXED_FINAL_TIME
+ * Time Horizon remains constant until a fixed final time is near. It shrinks and eventually becomes zero.
+ * The overall time horizon gets set trough the initial problem time horizon. The smaller, receding time
+ * horizon can be specified in the mpc_settings struct as "minimumTimeHorizonMpc_"
+ */
+enum MPC_MODE
+{
+
+ FIXED_FINAL_TIME = 0, //!< FIXED_FINAL_TIME
+
+ CONSTANT_RECEDING_HORIZON, //!< CONSTANT_RECEDING_HORIZON
+
+ FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON, //!< FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON
+
+ RECEDING_HORIZON_WITH_FIXED_FINAL_TIME //!< RECEDING_HORIZON_WITH_FIXED_FINAL_TIME
+};
+
+
+//! MPC Settings struct
+struct mpc_settings
+{
+ /*!
+ * State prediction.
+ * Use state prediction (based on a given delay, the given initial state and a given policy)?
+ * If set to true, MPC will either use a measurement of the delay or use the fixed delay given below
+ * as "fixedDelayUs_".
+ */
+ bool stateForwardIntegration_ = false;
+
+ /*!
+ * Integrator type to use for the state prediction
+ */
+ ct::core::IntegrationType stateForwardIntegratorType_ = ct::core::IntegrationType::RK4;
+
+ /*!
+ * time step size employed for the forward integration
+ */
+ double stateForwardIntegration_dt_ = 0.001;
+
+ /*!
+ * Delay compensation.
+ * If measureDelay_ is set to true, the MPC timer automatically keeps track of the planning times and
+ * uses them for the state forward prediction.
+ * Only applies if stateForwardIntegration_ is set to true.
+ * */
+ bool measureDelay_ = false;
+
+ /*!
+ * The delayMeasurementMultiplier_ quantifies the level of trust you have in our planning time measurement.
+ * In case you wish to be conservative, for example because the planning times vary a lot, reduce this multiplier.
+ * Maximum confidence means, set it to 1.0.
+ * In essence, this number says "how much of our measured delay do we use for pre-integration".
+ * */
+ double delayMeasurementMultiplier_ = 0.7;
+
+ /*!
+ * If not using measureDelay_ = true, we can instead set a fixed, known delay in microseconds (!) here.
+ */
+ int fixedDelayUs_ = 0;
+
+ /*!
+ * Additional delay in microseconds (!), which gets added to either the measured delay of above fixedDelay.
+ */
+ int additionalDelayUs_ = 0;
+
+ /*!
+ * If solving the problem takes longer than predicted, we can post-truncate the solution trajectories.
+ */
+ bool postTruncation_ = false;
+
+ /*!
+ * MPC Time horizon modus
+ */
+ MPC_MODE mpc_mode = CONSTANT_RECEDING_HORIZON;
+
+ /*!
+ * see description of the different modes above
+ */
+ core::Time minimumTimeHorizonMpc_ = 1.0; // seconds
+
+ /*!
+ * cold starting or warm starting.
+ * Warm starting will either use a default strategy for common controller types, or a user specified
+ * custom warm starting strategy.
+ */
+ bool coldStart_ = false;
+
+
+ /*!
+ * override the internal clock with external timing.
+ * \warning when employing this option, the setExternalTime() method needs to be called in MPC
+ */
+ bool useExternalTiming_ = false;
+
+
+ //! Print MPC settings to console
+ void print()
+ {
+ std::cout << " ========================= MPC SETTINGS =============================" << std::endl;
+ std::cout << " stateForwardIntegration: \t " << stateForwardIntegration_ << std::endl;
+ std::cout << " stateForwardIntegrator: \t " << stateForwardIntegratorType_ << std::endl;
+ std::cout << " stateForwardIntegration_dt: \t " << stateForwardIntegration_dt_ << std::endl;
+ std::cout << " measureDelay: \t " << measureDelay_ << std::endl;
+ std::cout << " delayMeasurementMultiplier: \t " << delayMeasurementMultiplier_ << std::endl;
+ std::cout << " fixedDelayUs: \t " << fixedDelayUs_ << std::endl;
+ std::cout << " additionalDelayUs: \t " << additionalDelayUs_ << std::endl;
+ std::cout << " postTruncation_: \t " << postTruncation_ << std::endl;
+ std::cout << " mpc_mode: \t " << (int)mpc_mode << std::endl;
+ std::cout << " minimumTimeHorizonMpc: \t " << minimumTimeHorizonMpc_ << std::endl;
+ std::cout << " coldStart: \t " << coldStart_ << std::endl;
+ std::cout << " useExternalTiming: \t " << useExternalTiming_ << std::endl;
+ std::cout << " ============================== END =================================" << std::endl;
+ }
+};
+
+
+//! load the mpc settings from file
+/*!
+ * @param filename
+ * path of the file you wish to load from
+ * @param settings
+ * the loaded settings
+ *
+ * @todo make part of mpc_settings struct
+ */
+inline void loadMpcSettings(const std::string& filename, mpc_settings& settings)
+{
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ settings.measureDelay_ = pt.get<bool>("mpc.measureDelay");
+ settings.stateForwardIntegration_ = pt.get<bool>("mpc.stateForwardIntegration");
+ settings.stateForwardIntegration_dt_ = pt.get<double>("mpc.stateForwardIntegration_dt");
+ settings.stateForwardIntegratorType_ = (ct::core::IntegrationType)pt.get<int>("mpc.stateForwardIntegratorType");
+ settings.fixedDelayUs_ = pt.get<int>("mpc.fixedDelayUs");
+ settings.additionalDelayUs_ = pt.get<int>("mpc.additionalDelayUs");
+ settings.minimumTimeHorizonMpc_ = pt.get<double>("mpc.timeHorizon");
+ settings.coldStart_ = pt.get<bool>("mpc.coldStart");
+ settings.postTruncation_ = pt.get<bool>("mpc.postTruncation");
+ settings.delayMeasurementMultiplier_ = pt.get<double>("mpc.delayMeasurementMultiplier");
+ settings.useExternalTiming_ = pt.get<bool>("mpc.useExternalTiming");
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/MpcTimeKeeper.h b/ct_optcon/include/ct/optcon/mpc/MpcTimeKeeper.h
new file mode 100644
index 0000000..0a039ad
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/MpcTimeKeeper.h
@@ -0,0 +1,382 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "MpcSettings.h"
+#include "timehorizon/MpcTimeHorizon.h"
+
+#include <ct/optcon/problem/OptConProblem.h>
+
+/**
+ * define the following flag for debugging the MPC time keeper
+ */
+//#define DEBUG_PRINT_TIMEKEEPER
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+
+//! Time Keeper Class for Model Predictive Control
+/*!
+ * - uses an internal clock for bookkeeping of times between MPC clycles
+ * - this class is based on the assumption that all Optimal Control Problems start at t = 0. All times computed here are
+ * expressed as relative times w.r.t. the start of the Optimal Control Problem.
+ * - calculates the required shifting times between iterations and due to forward integration (initial state prediction)
+ * - update Time Horizons accordingly
+ */
+template <typename SCALAR = double>
+class MpcTimeKeeper
+{
+public:
+ //! Standard Constructor MpcTimeKeeper
+ /*!
+ * this constructor should not be used by the normal user. Todo: get rid of MpcTimeKeeper default constructor
+ */
+ MpcTimeKeeper() {}
+ //! Constructor for Mpc Time Keeper class
+ /*!
+ *
+ * @param timeHorizonStrategy the user-specified time horizon strategy, for example fixed time horizon
+ * @param mpc_settings the mpc_settings as specified by the user
+ */
+ MpcTimeKeeper(std::shared_ptr<MpcTimeHorizon<SCALAR>> timeHorizonStrategy, const mpc_settings& mpc_settings)
+ : mpc_settings_(mpc_settings),
+ initialized_(false),
+ finalPointReached_(false),
+ lastMeasuredDelay_(0.0),
+ maxDelayMeasured_(0.0),
+ minDelayMeasured_(std::numeric_limits<SCALAR>::max()),
+ summedDelay_(0.0),
+ timeHorizonStrategy_(timeHorizonStrategy),
+ firstRun_(true)
+ {
+ }
+
+
+ //! initialize the Mpc Time Keeper (mandatory)
+ /*!
+ * resets the TimeKeeper, needs to be called whenever MPC starts over from the beginning.
+ */
+ void initialize()
+ {
+ firstRun_ = true;
+
+ timer_.reset();
+ lastSolveTimer_.reset();
+ firstSolveTimer_.reset();
+
+ ext_timer_.reset();
+ ext_lastSolveTimer_.reset();
+ ext_firstSolveTimer_.reset();
+
+ lastMeasuredDelay_ = 0.0;
+ maxDelayMeasured_ = 0.0;
+ minDelayMeasured_ = std::numeric_limits<SCALAR>::max();
+ summedDelay_ = 0.0;
+ finalPointReached_ = false;
+
+ initialized_ = true;
+ }
+
+
+ //! compute new mpc timings, based on current time horizon and the given time horizon strategy
+ /*!
+ * @param externalTime the external timing, optional.
+ * @param current_T
+ * the currently active problem time horizon
+ * @param new_T
+ * the new, updated problem time horizon, which gets computed by the time horizon strategy
+ * @param t_forw_start
+ * time where to start the forward propagation of the state measurement based on delays
+ * @param t_forw_stop
+ * time where to stop the forward propagation of the state measurement based on delays
+ */
+ void computeNewTimings(const SCALAR externalTime,
+ const SCALAR current_T,
+ SCALAR& new_T,
+ SCALAR& t_forw_start,
+ SCALAR& t_forw_stop)
+ {
+ if (initialized_ == false)
+ throw std::runtime_error(
+ "Error in MPC time keeper: cannot update timings if MpcTimeKeeper not properly initialized.");
+
+
+ SCALAR timeSinceEndedLastSolve;
+ SCALAR timeSinceEndedFirstSolve;
+
+ if (!firstRun_)
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_firstSolveTimer_.stop(externalTime);
+ ext_lastSolveTimer_.stop(externalTime);
+ timeSinceEndedFirstSolve = ext_firstSolveTimer_.getElapsedTime();
+ timeSinceEndedLastSolve = ext_lastSolveTimer_.getElapsedTime();
+ }
+ else
+ {
+ lastSolveTimer_.stop();
+ firstSolveTimer_.stop();
+ timeSinceEndedLastSolve = lastSolveTimer_.getElapsedTime();
+ timeSinceEndedFirstSolve = firstSolveTimer_.getElapsedTime();
+ }
+ }
+ else
+ {
+ // set zero for the first mpc run
+ timeSinceEndedLastSolve = 0.0;
+ timeSinceEndedFirstSolve = 0.0;
+ }
+
+
+ // starting time of forward prediction, relative to previous controller
+ t_forw_start = timeSinceEndedLastSolve;
+
+
+ /**
+ * estimate the delay from planning, etc.
+ */
+ SCALAR delayToApply = computeDelayToApply();
+
+
+ // stopping time relative to previous controller/run
+ if (!firstRun_)
+ t_forw_stop = t_forw_start + delayToApply;
+ else
+ t_forw_stop = t_forw_start;
+
+
+ /**
+ * check for compliance of t_forward_stop and t_forward_start with time horizon of the current controller
+ */
+ if (t_forw_start > current_T)
+ {
+ std::cerr << "WARNING: forward integration start time cannot be bigger than last time horizon. Truncating "
+ "forward integration time."
+ << std::endl;
+ t_forw_start = current_T;
+ }
+ if (t_forw_stop > current_T)
+ {
+ std::cerr << "WARNING: forward integration stop time cannot be bigger than last time horizon. Truncating "
+ "forward integration time."
+ << std::endl;
+ t_forw_stop = current_T;
+ }
+
+
+ finalPointReached_ = timeHorizonStrategy_->computeNewTimeHorizon(timeSinceEndedFirstSolve, t_forw_stop, new_T);
+
+
+#ifdef DEBUG_PRINT_TIMEKEEPER
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: Time since first solve(): " << timeSinceEndedFirstSolve << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: Time since last solve(): " << timeSinceEndedLastSolve << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: t_forward_start: " << t_forw_start << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: t_forward_stop: " << t_forw_stop << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: New Time Horizon: " << new_T << std::endl;
+ std::cout << "DEBUG_PRINT_TIMEKEEPER: final point reached_: " << finalPointReached_ << std::endl;
+#endif
+ }
+
+
+ //! query this in order to find out if the final time horizon has been reached.
+ /*!
+ * May be used to trigger corresponding events, such as stopping the control loop.
+ * @return bool, true if final time horizon has been reached
+ */
+ const bool finalPointReached() const { return finalPointReached_; }
+ //! update mpc settings
+ /*!
+ * @param settings
+ * the new settings to be handed over
+ */
+ void updateSettings(const mpc_settings& settings) { mpc_settings_ = settings; }
+ //! start measuring time elapsed during planning / solving the optimal control problem
+ void startDelayMeasurement(const SCALAR& externalTime)
+ {
+ if (mpc_settings_.measureDelay_)
+ {
+ if (mpc_settings_.useExternalTiming_)
+ ext_timer_.start(externalTime);
+ else
+ timer_.start();
+ }
+ }
+
+
+ //! stop measuring time elapsed during solving the optimal control problem
+ void stopDelayMeasurement(const SCALAR& externalTime)
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_timer_.stop(externalTime);
+ lastMeasuredDelay_ = ext_timer_.getElapsedTime();
+ // measure how much time passed since last successful solve()
+ ext_lastSolveTimer_.start(externalTime);
+ }
+ else
+ {
+ timer_.stop();
+ lastMeasuredDelay_ = timer_.getElapsedTime();
+ // measure how much time passed since last successful solve()
+ lastSolveTimer_.start();
+ }
+
+ maxDelayMeasured_ = std::max(maxDelayMeasured_, lastMeasuredDelay_);
+ minDelayMeasured_ = std::min(minDelayMeasured_, lastMeasuredDelay_);
+ summedDelay_ += lastMeasuredDelay_;
+
+ if (lastMeasuredDelay_ < 0)
+ throw std::runtime_error("Fatal: measured delay cannot be < 0");
+
+#ifdef DEBUG_PRINT_TIMEKEEPER
+ std::cout << "Measured delay during Solution: " << lastMeasuredDelay_ << " seconds" << std::endl;
+ std::cout << "Max. measured delay during Solution: " << maxDelayMeasured_ << " seconds" << std::endl;
+ std::cout << "Min. measured delay during Solution: " << minDelayMeasured_ << " seconds" << std::endl;
+#endif
+
+ if (firstRun_)
+ {
+ // start timer for measuring how much time elapsed since the first successful plan
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_firstSolveTimer_.start(externalTime);
+ }
+ else
+ {
+ firstSolveTimer_.start();
+ }
+
+ firstRun_ = false;
+ }
+ }
+
+
+ //! retrieve the time that elapsed since the last successful solve() call to an Optimal Control Problem
+ /*!
+ * the returned time can be used to synchronize the calls to optimal control problems
+ * @return time elapsed
+ */
+ SCALAR timeSincePreviousSuccessfulSolve(const SCALAR& externalTime)
+ {
+ if (firstRun_)
+ return 0.0;
+ else
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_lastSolveTimer_.stop(externalTime);
+ return ext_lastSolveTimer_.getElapsedTime();
+ }
+ else
+ {
+ lastSolveTimer_.stop();
+ return lastSolveTimer_.getElapsedTime();
+ }
+ }
+ }
+
+
+ //! retrieve the time that elapsed since the first successful solve() call to an Optimal Control Problem
+ /*!
+ * the returned time can be used externally, for example to update cost functions
+ * @return time elapsed
+ */
+ const SCALAR timeSinceFirstSuccessfulSolve(const SCALAR& externalTime)
+ {
+ if (firstRun_)
+ return 0.0;
+ else
+ {
+ if (mpc_settings_.useExternalTiming_)
+ {
+ ext_firstSolveTimer_.stop(externalTime);
+ return ext_firstSolveTimer_.getElapsedTime();
+ }
+ else
+ {
+ firstSolveTimer_.stop();
+ return firstSolveTimer_.getElapsedTime();
+ }
+ }
+ }
+
+ //! obtain the delay which was measured during solving the optimal control problem
+ const SCALAR& getMeasuredDelay() const { return lastMeasuredDelay_; }
+ //! get the maximum measured delay (maximum over all cycles)
+ const SCALAR& getMaxMeasuredDelay() const { return maxDelayMeasured_; }
+ //! get the smallest measured delay (minimum over all cycles)
+ const SCALAR& getMinMeasuredDelay() const { return minDelayMeasured_; }
+ //! get the sum of all measured delays
+ const SCALAR& getSummedDelay() const { return summedDelay_; }
+private:
+ //! computes the time to forward integrate a measured state according to the current policy based on fixed delays and measured delays
+ /*!
+ * The delay to be applied consists of a fixed-time delay specified by the user (for example communication delays)
+ * and a variable delay, which is based on a delay-measurement, which captures the optimal control problem solving times.
+ * The delay to be applied is the sum of fixed and variable components.
+ * @return delay to be applied
+ */
+ SCALAR computeDelayToApply()
+ {
+ SCALAR fixedDelay = 1e-6 * mpc_settings_.additionalDelayUs_;
+ SCALAR variableDelay = 0.0;
+
+ if (mpc_settings_.measureDelay_) // use variable, measured delay
+ variableDelay = mpc_settings_.delayMeasurementMultiplier_ * lastMeasuredDelay_;
+ else // using fixed delay
+ fixedDelay += 1e-6 * mpc_settings_.fixedDelayUs_;
+
+#ifdef DEBUG_PRINT_TIMEKEEPER
+ std::cout << "Accumulated delay to apply: " << fixedDelay + variableDelay << " seconds" << std::endl;
+#endif
+
+ return fixedDelay + variableDelay;
+ }
+
+
+ mpc_settings mpc_settings_;
+
+ bool initialized_;
+
+ //! flag indicating that the time horizon has been hit
+ bool finalPointReached_;
+
+ //! timer for internally measuring the time elapsed during planning, internal, relative time
+ core::tpl::Timer<SCALAR> timer_;
+ //! timer to internally measure how much time elapsed since the last finished solve
+ core::tpl::Timer<SCALAR> lastSolveTimer_;
+ //! timer for internally measuring how much time elapsed since the first successful plan
+ core::tpl::Timer<SCALAR> firstSolveTimer_;
+
+ //! timer for internally measuring the time elapsed during planning, internal, relative time
+ core::tpl::ExternallyDrivenTimer<SCALAR> ext_timer_;
+ //! timer to internally measure how much time elapsed since the last finished solve
+ core::tpl::ExternallyDrivenTimer<SCALAR> ext_lastSolveTimer_;
+ //! timer for internally measuring how much time elapsed since the first successful plan
+ core::tpl::ExternallyDrivenTimer<SCALAR> ext_firstSolveTimer_;
+
+ SCALAR lastMeasuredDelay_; //! last delay in planning, internal, relative time
+ SCALAR maxDelayMeasured_; //! the max. delay occurred due to planning
+ SCALAR minDelayMeasured_; //! the min. delay occurred due to planning
+ SCALAR summedDelay_; //! sum of all delays measured
+
+ //! time horizon strategy specified by the user, e.g. constant receding horizon
+ std::shared_ptr<MpcTimeHorizon<SCALAR>> timeHorizonStrategy_;
+
+ bool firstRun_; //! set to true if first run active
+};
+
+
+} // namespace tpl
+
+typedef tpl::MpcTimeKeeper<double> MpcTimeKeeper;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler-impl.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler-impl.h
new file mode 100644
index 0000000..2c8ee36
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler-impl.h
@@ -0,0 +1,44 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::PolicyHandler()
+{
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::~PolicyHandler()
+{
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::designWarmStartingPolicy(const SCALAR& delay,
+ const SCALAR& TimeHorizon,
+ POLICY& policy)
+{
+ policy = initialPolicy_;
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::truncateSolutionFront(const SCALAR& delay,
+ POLICY& policy,
+ SCALAR& effectivelyTruncated)
+{
+}
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void PolicyHandler<POLICY, STATE_DIM, CONTROL_DIM, SCALAR>::setPolicy(const POLICY& newPolicy)
+{
+ initialPolicy_ = newPolicy;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler.h
new file mode 100644
index 0000000..68d2cff
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler.h
@@ -0,0 +1,61 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <typename POLICY, size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class PolicyHandler
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ PolicyHandler();
+
+ virtual ~PolicyHandler();
+
+ //! design a warm-starting policy for the optimal control problem solver
+ /*!
+ * Designs an initial guess for MPC (warm-start). An optimal strategy for warm-starting might be highly application/system dependent,
+ * thus the user can overload this method if desired. Straight-forward default implementations for common 'ct' solvers and policy
+ * types are provided in the folder "default".
+ * Note that the default policy handler simply performs "cold-starting" which means that the initially provided control policy is returned without modification.
+ *
+ * @param delay
+ * time difference between nominal starting time of the current policy and when the warm-start policy should start
+ * @param TimeHorizon
+ * desired overall policy time horizon (note: not covering the whole time-horizon may result in an error)
+ * @param policy
+ * the current policy, to be overwritten with the warm start
+ */
+ virtual void designWarmStartingPolicy(const SCALAR& delay, const SCALAR& TimeHorizon, POLICY& policy);
+
+
+ //! a method required for additional post-truncation.
+ /*!
+ * post truncation may become relevant if the delay is underestimated or pre-integration is turned off.
+ * @param delay
+ * the time to truncate away from the solution
+ * @param policy
+ * the policy to be truncated
+ * @param effectivelyTruncated
+ * the time which was truncated away
+ */
+ virtual void truncateSolutionFront(const SCALAR& delay, POLICY& policy, SCALAR& effectivelyTruncated);
+
+
+ //! set new policy to policy handler
+ void setPolicy(const POLICY& newPolicy);
+
+
+protected:
+ POLICY initialPolicy_; //! the initial policy
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h
new file mode 100644
index 0000000..4eaa32b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h
@@ -0,0 +1,121 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::StateFeedbackPolicyHandler(const SCALAR& dt) : dt_(dt)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::~StateFeedbackPolicyHandler()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::designWarmStartingPolicy(const SCALAR& delay,
+ const SCALAR& newTimeHorizon,
+ StateFeedbackController_t& policy)
+{
+ // get the current reference trajectories from the StateFeedbackController
+ core::FeedbackTrajectory<STATE_DIM, CONTROL_DIM, SCALAR>& FeedbackTraj = policy.getFeedbackTrajectory();
+ core::ControlTrajectory<CONTROL_DIM, SCALAR>& FeedForwardTraj = policy.getFeedforwardTrajectory();
+ core::StateTrajectory<STATE_DIM, SCALAR>& StateRefTraj = policy.getReferenceStateTrajectory();
+
+ // current number of discrete elements
+ int currentSize = FeedForwardTraj.size();
+
+ // compute new controller length as a function of the time horizon
+ int Kn_new = std::max(1, (int)std::lround(newTimeHorizon / dt_));
+
+ // compute number indices to be shifted. Note: it does not make sense to shift more entries than are available
+ int num_di = FeedForwardTraj.getIndexFromTime(delay);
+ num_di = std::min(num_di, currentSize - 1);
+
+
+#ifdef DEBUG_POLICYHANDLER
+ std::cout << "DEBUG_POLICYHANDLER: Controller shifting: " << std::endl
+ << "delay: " << delay << " newT: " << newTimeHorizon << std::endl
+ << " new Discrete Controller has: " << std::endl
+ << Kn_new << " control elements, shifted about " << num_di << " elements." << std::endl
+ << Kn_new + 1 << " state elements, shifted about " << num_di << " elements." << std::endl;
+#endif
+
+
+ // Step 1 - Truncate Front: remove first 'num_di' elements from controller and shift time accordingly
+ if (num_di > 0)
+ {
+ FeedForwardTraj.eraseFront(num_di, num_di * dt_);
+ FeedbackTraj.eraseFront(num_di, num_di * dt_);
+ StateRefTraj.eraseFront(num_di, num_di * dt_);
+ currentSize -= num_di;
+ }
+
+
+ // Step 2 - Resize overall controller
+ if (Kn_new > currentSize)
+ {
+ //extend at back with constant value taken from last element
+ bool timeIsRelative = true;
+ for (size_t i = 0; i < Kn_new - currentSize; i++)
+ {
+ FeedbackTraj.push_back(FeedbackTraj.back(), dt_, timeIsRelative);
+ FeedForwardTraj.push_back(FeedForwardTraj.back(), dt_, timeIsRelative);
+ StateRefTraj.push_back(StateRefTraj.back(), dt_, timeIsRelative);
+ }
+ }
+ else if (Kn_new < currentSize)
+ {
+ // remove elements from back
+ for (size_t i = 0; i < currentSize - Kn_new; i++)
+ {
+ FeedbackTraj.pop_back();
+ FeedForwardTraj.pop_back();
+ StateRefTraj.pop_back();
+ }
+ }
+
+ // safety check, which should never be entered
+ if (FeedForwardTraj.size() == 0)
+ {
+ throw std::runtime_error("ERROR in StateFeedbackPolicyHandler.h: new policy should not have size 0.");
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void StateFeedbackPolicyHandler<STATE_DIM, CONTROL_DIM, SCALAR>::truncateSolutionFront(const SCALAR& delay,
+ StateFeedbackController_t& policy,
+ SCALAR& effectivelyTruncated)
+{
+ // current controller length
+ size_t currentSize = policy.getFeedforwardTrajectory().size();
+
+ size_t num_di = policy.getFeedforwardTrajectory().getIndexFromTime(delay);
+ num_di = std::min(num_di, currentSize - 1);
+
+ effectivelyTruncated = num_di * dt_;
+
+#ifdef DEBUG_POLICYHANDLER
+ std::cout << "DEBUG_WARMSTART: Current Controller Size: " << currentSize << " elements." << std::endl;
+ std::cout << "DEBUG_WARMSTART: Controller truncation: truncation about " << num_di << " elements." << std::endl;
+ std::cout << "DEBUG_WARMSTART: Controller new size: " << currentSize - num_di << " elements." << std::endl;
+#endif
+
+ // remove first num_di elements from controller
+ if (num_di > 0 && num_di < currentSize)
+ {
+ policy.getFeedbackTrajectory().eraseFront(num_di, effectivelyTruncated);
+ policy.getFeedforwardTrajectory().eraseFront(num_di, effectivelyTruncated);
+ policy.getReferenceStateTrajectory().eraseFront(num_di, effectivelyTruncated);
+ }
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler.h b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler.h
new file mode 100644
index 0000000..3727254
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler.h
@@ -0,0 +1,48 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+//! the default policy handler for iLQR
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+class StateFeedbackPolicyHandler : public PolicyHandler<core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>,
+ STATE_DIM,
+ CONTROL_DIM,
+ SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> StateFeedbackController_t;
+
+ StateFeedbackPolicyHandler(const SCALAR& dt);
+
+ virtual ~StateFeedbackPolicyHandler();
+
+ virtual void designWarmStartingPolicy(const SCALAR& delay,
+ const SCALAR& newTimeHorizon,
+ StateFeedbackController_t& policy) override;
+
+ /*!
+ * required for additional post-truncation.
+ * @param delay the delay which is to be truncated away
+ * @param policy the resulting, truncated policy
+ * @param effectivelyTruncated the time which was effectively truncated away
+ * (can be different from the input in discrete-time case, for example)
+ */
+ virtual void truncateSolutionFront(const SCALAR& delay,
+ StateFeedbackController_t& policy,
+ SCALAR& effectivelyTruncated) override;
+
+private:
+ SCALAR dt_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h
new file mode 100644
index 0000000..3adb3ea
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h
@@ -0,0 +1,91 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+template <typename SCALAR>
+MpcTimeHorizon<SCALAR>::MpcTimeHorizon(const mpc_settings& settings, const SCALAR& initialTimeHorizon)
+ : mpc_settings_(settings), initialTimeHorizon_(initialTimeHorizon)
+{
+}
+
+template <typename SCALAR>
+MpcTimeHorizon<SCALAR>::~MpcTimeHorizon()
+{
+}
+
+template <typename SCALAR>
+bool MpcTimeHorizon<SCALAR>::computeNewTimeHorizon(const SCALAR& t_since_ended_first_solve,
+ const SCALAR& t_forward_prediction_stop,
+ SCALAR& new_T)
+{
+ /**
+ * compute desired end time and the time we have left from now. Will be ignored below, if not required in the scenario.
+ */
+ SCALAR timeLeft = initialTimeHorizon_ - (t_since_ended_first_solve + t_forward_prediction_stop);
+ timeLeft = std::max((SCALAR)0.0, timeLeft);
+
+
+ switch (mpc_settings_.mpc_mode)
+ {
+ case MPC_MODE::CONSTANT_RECEDING_HORIZON:
+ {
+ new_T = initialTimeHorizon_; // leave time horizon unchanged
+
+ return false; // in this mode, we never reach to the final time
+ }
+ case MPC_MODE::FIXED_FINAL_TIME:
+ {
+ new_T = timeLeft;
+
+ if (new_T == 0.0)
+ return true; // reached time horizon, return true
+
+ return false;
+ }
+ case MPC_MODE::FIXED_FINAL_TIME_WITH_MIN_TIME_HORIZON:
+ {
+ // std::max() ensures that the time is greater than the mininmum specified time horizon
+ new_T = std::max((SCALAR)mpc_settings_.minimumTimeHorizonMpc_, timeLeft);
+
+ if (new_T == mpc_settings_.minimumTimeHorizonMpc_)
+ return true;
+
+ return false;
+ }
+ case MPC_MODE::RECEDING_HORIZON_WITH_FIXED_FINAL_TIME:
+ {
+ new_T = std::min((SCALAR)mpc_settings_.minimumTimeHorizonMpc_, timeLeft);
+
+ if (new_T == 0.0)
+ return true;
+
+ return false;
+ }
+ default:
+ throw std::runtime_error("ERROR in MPC Constructor -- unknown Time Horizon Strategy.");
+ }
+}
+
+template <typename SCALAR>
+void MpcTimeHorizon<SCALAR>::updateSettings(const mpc_settings& mpcsettings)
+{
+ mpc_settings_ = mpcsettings;
+}
+
+template <typename SCALAR>
+void MpcTimeHorizon<SCALAR>::updateInitialTimeHorizon(const SCALAR& initTimeHorizon)
+{
+ initialTimeHorizon_ = initTimeHorizon;
+}
+
+} // namespace tpl
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon.h b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon.h
new file mode 100644
index 0000000..c1750cf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon.h
@@ -0,0 +1,59 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "../MpcSettings.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/*!
+ * This class implements the four default strategies for the time horizon in ct's MPC.
+ * In case a different time horizon strategy is required, the user can derive from this
+ * class and override the virtual functions
+ */
+template <typename SCALAR = double>
+class MpcTimeHorizon
+{
+public:
+ MpcTimeHorizon(const mpc_settings& settings, const SCALAR& initialTimeHorizon);
+
+ virtual ~MpcTimeHorizon();
+
+ //! compute new MPC time horizon
+ /*!
+ * @param t_since_ended_first_solve
+ * Time since the first successful solve
+ * @param t_forward_prediction_stop
+ * Relative time where to stop forward integration w.r.t. previous controller
+ * @param new_T
+ * Resulting, new time horizon provided to the solver
+ * @return true if TimeHorizon reached and MPC should stop
+ */
+ virtual bool computeNewTimeHorizon(const SCALAR& t_since_ended_first_solve,
+ const SCALAR& t_forward_prediction_stop,
+ SCALAR& new_T);
+
+ void updateSettings(const mpc_settings& mpcsettings);
+
+ //! update the time horizon which is used during the first call to the solver
+ void updateInitialTimeHorizon(const SCALAR& initTimeHorizon);
+
+
+protected:
+ mpc_settings mpc_settings_;
+
+ SCALAR initialTimeHorizon_;
+};
+
+} // namespace tpl
+
+typedef tpl::MpcTimeHorizon<double> MpcTimeHorizon;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCAlgorithm.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCAlgorithm.hpp
new file mode 100644
index 0000000..8ba1446
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCAlgorithm.hpp
@@ -0,0 +1,46 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCAlgorithm
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Backend_t;
+
+ typedef ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+
+ NLOCAlgorithm(const std::shared_ptr<Backend_t>& backend) : backend_(backend) {}
+ virtual ~NLOCAlgorithm() {}
+ virtual void configure(const Settings_t& settings) = 0;
+
+ virtual void prepareIteration() = 0;
+
+ virtual bool finishIteration() = 0;
+
+ virtual bool runIteration() = 0;
+
+ virtual void setInitialGuess(const Policy_t& initialGuess) = 0;
+
+ virtual void prepareMPCIteration() = 0;
+
+ virtual bool finishMPCIteration() = 0;
+
+protected:
+ std::shared_ptr<Backend_t> backend_;
+};
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
new file mode 100644
index 0000000..d68bcc9
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
@@ -0,0 +1,1834 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#define SYMPLECTIC_ENABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V > 0 && P > 0 && (V+P==ST)), void>::type
+#define SYMPLECTIC_DISABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V <= 0 || P <= 0 || (V+P!=ST)), void>::type
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendBase(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const Settings_t& settings)
+ :
+
+ substepRecorders_(),
+ integrators_(settings.nThreads + 1),
+ sensitivity_(settings.nThreads + 1),
+ integratorsEulerSymplectic_(settings.nThreads + 1),
+ integratorsRkSymplectic_(settings.nThreads + 1),
+
+ controller_(settings.nThreads + 1),
+ initialized_(false),
+ configured_(false),
+ iteration_(0),
+ settings_(settings),
+ K_(0),
+ d_norm_(0.0),
+ e_box_norm_(0.0),
+ e_gen_norm_(0.0),
+ lx_norm_(0.0),
+ lu_norm_(0.0),
+ lqocProblem_(new LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>()),
+
+ substepsX_(new StateSubsteps),
+ substepsU_(new ControlSubsteps),
+
+ intermediateCostBest_(std::numeric_limits<SCALAR>::infinity()),
+ finalCostBest_(std::numeric_limits<SCALAR>::infinity()),
+ lowestCost_(std::numeric_limits<SCALAR>::infinity()),
+ intermediateCostPrevious_(std::numeric_limits<SCALAR>::infinity()),
+ finalCostPrevious_(std::numeric_limits<SCALAR>::infinity()),
+ linearSystems_(settings.nThreads + 1),
+ boxConstraints_(settings.nThreads + 1, nullptr), // initialize constraints with null
+ generalConstraints_(settings.nThreads + 1, nullptr), // initialize constraints with null
+ firstRollout_(true),
+ alphaBest_(-1),
+ lqpCounter_(0)
+{
+ Eigen::initParallel();
+
+ for (int i = 0; i < settings.nThreads + 1; i++)
+ {
+ controller_[i] = ConstantControllerPtr(new core::ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>());
+ }
+
+ configure(settings);
+
+ changeTimeHorizon(optConProblem.getTimeHorizon());
+ changeInitialState(optConProblem.getInitialState());
+ changeCostFunction(optConProblem.getCostFunction());
+ changeNonlinearSystem(optConProblem.getNonlinearSystem());
+ changeLinearSystem(optConProblem.getLinearSystem());
+
+ // if the optimal problem has constraints, change
+ if (optConProblem.getBoxConstraints())
+ changeBoxConstraints(optConProblem.getBoxConstraints());
+ if (optConProblem.getGeneralConstraints())
+ changeGeneralConstraints(optConProblem.getGeneralConstraints());
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendBase(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : NLOCBackendBase(optConProblem, Settings_t::fromConfigFile(settingsFile, verbose, ns))
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOCBackendBase()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ if (initialGuess.x_ref().size() < (size_t)K_ + 1)
+ {
+ std::cout << "Initial state guess length too short. Received length " << initialGuess.x_ref().size()
+ << ", expected " << K_ + 1 << std::endl;
+ throw(std::runtime_error("initial state guess to short"));
+ }
+
+ if (initialGuess.uff().size() < (size_t)K_)
+ {
+ std::cout << "Initial control guess length too short. Received length " << initialGuess.uff().size()
+ << ", expected " << K_ << std::endl;
+ throw std::runtime_error("initial control guess to short");
+ }
+
+ if (initialGuess.K().size() < (size_t)K_)
+ {
+ std::cout << "Initial feedback length too short. Received length " << initialGuess.K().size() << ", expected "
+ << K_ << std::endl;
+ throw std::runtime_error("initial control guess to short");
+ }
+
+ u_ff_ = initialGuess.uff();
+ L_ = initialGuess.K();
+ x_ = initialGuess.x_ref();
+ x_prev_ = x_;
+
+ if (x_.size() > (size_t)K_ + 1)
+ {
+ std::cout << "Warning, initial state guess too long. Received length " << x_.size() << ", expected " << K_ + 1
+ << ", will truncate" << std::endl;
+ x_.resize(K_ + 1);
+ }
+
+ if (u_ff_.size() > (size_t)K_)
+ {
+ std::cout << "Warning, initial control guess too long. Received length " << u_ff_.size() << ", expected " << K_
+ << ", will truncate" << std::endl;
+ u_ff_.resize(K_);
+ }
+
+ if (L_.size() > (size_t)K_)
+ {
+ std::cout << "Warning, initial feedback guess too long. Received length " << L_.size() << ", expected " << K_
+ << ", will truncate" << std::endl;
+ L_.resize(K_);
+ }
+
+ initialized_ = true;
+
+ t_ = TimeArray(settings_.dt, x_.size(), 0.0);
+
+ reset();
+
+ // compute costs of the initial guess trajectory
+ computeCostsOfTrajectory(settings_.nThreads, x_, u_ff_, intermediateCostBest_, finalCostBest_);
+ intermediateCostPrevious_ = intermediateCostBest_;
+ finalCostPrevious_ = finalCostBest_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeTimeHorizon(const SCALAR& tf)
+{
+ if (tf < 0)
+ throw std::runtime_error("negative time horizon specified");
+
+ int newK = settings_.computeK(tf);
+
+ if (newK == K_)
+ return;
+
+ K_ = newK;
+
+ t_ = TimeArray(settings_.dt, K_ + 1, 0.0);
+
+ lx_.resize(K_ + 1);
+ x_.resize(K_ + 1);
+ x_prev_.resize(K_ + 1);
+ xShot_.resize(K_ + 1);
+
+ lu_.resize(K_);
+ u_ff_.resize(K_);
+ u_ff_prev_.resize(K_);
+ L_.resize(K_);
+
+ substepsX_->resize(K_ + 1);
+ substepsU_->resize(K_ + 1);
+
+ lqocProblem_->changeNumStages(K_);
+ lqocProblem_->setZero();
+
+ lqocSolver_->setProblem(lqocProblem_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeInitialState(
+ const core::StateVector<STATE_DIM, SCALAR>& x0)
+{
+ if (x_.size() == 0)
+ x_.resize(1);
+
+ x_[0] = x0;
+ reset(); // since initial state changed, we have to start fresh, i.e. with a rollout
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeCostFunction(
+ const typename OptConProblem_t::CostFunctionPtr_t& cf)
+{
+ if (cf == nullptr)
+ throw std::runtime_error("cost function is nullptr");
+
+ costFunctions_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ costFunctions_[i] = typename OptConProblem_t::CostFunctionPtr_t(cf->clone());
+ }
+
+ // recompute cost if line search is active
+ // TODO: this should be multi-threaded to save time
+ if (iteration_ > 0 && settings_.lineSearchSettings.active)
+ computeCostsOfTrajectory(settings_.nThreads, x_, u_ff_, intermediateCostBest_, finalCostBest_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeNonlinearSystem(
+ const typename OptConProblem_t::DynamicsPtr_t& dyn)
+{
+ if (dyn == nullptr)
+ throw std::runtime_error("system dynamics are nullptr");
+
+ systems_.resize(settings_.nThreads + 1);
+ substepRecorders_.resize(settings_.nThreads + 1);
+ integrators_.resize(settings_.nThreads + 1);
+ sensitivity_.resize(settings_.nThreads + 1);
+ integratorsEulerSymplectic_.resize(settings_.nThreads + 1);
+ integratorsRkSymplectic_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ systems_[i] = typename OptConProblem_t::DynamicsPtr_t(dyn->clone());
+ systems_[i]->setController(controller_[i]);
+
+ substepRecorders_[i] = std::shared_ptr<ct::core::SubstepRecorder<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new ct::core::SubstepRecorder<STATE_DIM, CONTROL_DIM, SCALAR>(systems_[i]));
+
+ if (controller_[i] == nullptr)
+ throw std::runtime_error("Controller not defined");
+
+ // if symplectic integrator then don't create normal ones
+ if (settings_.integrator != ct::core::IntegrationType::EULER_SYM &&
+ settings_.integrator != ct::core::IntegrationType::RK_SYM)
+ {
+ integrators_[i] = std::shared_ptr<ct::core::Integrator<STATE_DIM, SCALAR>>(
+ new ct::core::Integrator<STATE_DIM, SCALAR>(systems_[i], settings_.integrator, substepRecorders_[i]));
+ }
+ initializeSymplecticIntegrators<V_DIM, P_DIM, STATE_DIM>(i);
+
+
+ if (settings_.useSensitivityIntegrator)
+ {
+ if (settings_.integrator != ct::core::IntegrationType::EULER &&
+ settings_.integrator != ct::core::IntegrationType::EULERCT &&
+ settings_.integrator != ct::core::IntegrationType::RK4 &&
+ settings_.integrator != ct::core::IntegrationType::RK4CT &&
+ settings_.integrator != ct::core::IntegrationType::EULER_SYM)
+ throw std::runtime_error("sensitivity integrator only available for Euler and RK4 integrators");
+
+ sensitivity_[i] =
+ SensitivityPtr(new ct::core::SensitivityIntegrator<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(
+ settings_.getSimulationTimestep(), linearSystems_[i], controller_[i], settings_.integrator,
+ settings_.timeVaryingDiscretization));
+ }
+ else
+ {
+ sensitivity_[i] =
+ SensitivityPtr(new ct::core::SensitivityApproximation<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(
+ settings_.dt, linearSystems_[i], settings_.discretization));
+ }
+ }
+ reset(); // since system changed, we have to start fresh, i.e. with a rollout
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeBoxConstraints(
+ const typename OptConProblem_t::ConstraintPtr_t& con)
+{
+ if (con == nullptr)
+ throw std::runtime_error("NLOCBackendBase: box constraint is nullptr");
+
+ boxConstraints_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ boxConstraints_[i] = typename OptConProblem_t::ConstraintPtr_t(con->clone());
+ }
+
+ // TODO can we do this multi-threaded?
+ if (iteration_ > 0 && settings_.lineSearchSettings.active)
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeGeneralConstraints(
+ const typename OptConProblem_t::ConstraintPtr_t& con)
+{
+ if (con == nullptr)
+ throw std::runtime_error("NLOCBackendBase: general constraint is nullptr");
+
+ generalConstraints_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ generalConstraints_[i] = typename OptConProblem_t::ConstraintPtr_t(con->clone());
+ }
+
+ // TODO can we do this multi-threaded?
+ if (iteration_ > 0 && settings_.lineSearchSettings.active)
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_ENABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initializeSymplecticIntegrators(
+ size_t i)
+{
+ if (systems_[i]->isSymplectic())
+ {
+ //! it only makes sense to compile the following code, if V_DIM > 0 and P_DIM > 0
+ integratorsEulerSymplectic_[i] =
+ std::shared_ptr<ct::core::IntegratorSymplecticEuler<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ new ct::core::IntegratorSymplecticEuler<P_DIM, V_DIM, CONTROL_DIM, SCALAR>(
+ std::static_pointer_cast<ct::core::SymplecticSystem<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ systems_[i]),
+ substepRecorders_[i]));
+ integratorsRkSymplectic_[i] =
+ std::shared_ptr<ct::core::IntegratorSymplecticRk<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ new ct::core::IntegratorSymplecticRk<P_DIM, V_DIM, CONTROL_DIM, SCALAR>(
+ std::static_pointer_cast<ct::core::SymplecticSystem<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>(
+ systems_[i])));
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_DISABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initializeSymplecticIntegrators(
+ size_t i)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeLinearSystem(
+ const typename OptConProblem_t::LinearPtr_t& lin)
+{
+ linearSystems_.resize(settings_.nThreads + 1);
+
+ for (int i = 0; i < settings_.nThreads + 1; i++)
+ {
+ // make a deep copy
+ linearSystems_[i] = typename OptConProblem_t::LinearPtr_t(lin->clone());
+
+ sensitivity_[i]->setLinearSystem(linearSystems_[i]);
+ }
+ // technically a linear system change does not require a new rollout. Hence, we do not reset.
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::checkProblem()
+{
+ if (K_ == 0)
+ throw std::runtime_error("Time horizon too small resulting in 0 steps");
+
+ if (u_ff_.size() < (size_t)K_)
+ {
+ std::cout << "Provided initial feed forward controller too short, should be at least " << K_ << " but is "
+ << u_ff_.size() << " long." << std::endl;
+ throw(std::runtime_error("Provided initial feed forward controller too short"));
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ if (!settings.parametersOk())
+ {
+ throw(std::runtime_error("Settings are incorrect. Aborting."));
+ }
+
+ if (settings.nThreads != settings_.nThreads)
+ {
+ throw(std::runtime_error("Number of threads cannot be changed after instance has been created."));
+ }
+
+ // update system discretizer with new settings
+ for (int i = 0; i < settings.nThreads + 1; i++)
+ {
+ if (!sensitivity_[i])
+ break;
+
+ sensitivity_[i]->setApproximation(settings.discretization);
+
+ if (settings.useSensitivityIntegrator)
+ sensitivity_[i]->setTimeDiscretization(settings.getSimulationTimestep());
+ else
+ sensitivity_[i]->setTimeDiscretization(settings.dt);
+ }
+
+ if (settings.integrator != settings_.integrator)
+ {
+ throw std::runtime_error("Cannot change integration type.");
+ }
+
+ // select the linear quadratic solver based on settings file
+ if (settings.lqocp_solver == NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ lqocSolver_ = std::shared_ptr<GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>());
+ }
+ else if (settings.lqocp_solver == NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER)
+ {
+#ifdef HPIPM
+ lqocSolver_ =
+ std::shared_ptr<HPIPMInterface<STATE_DIM, CONTROL_DIM>>(new HPIPMInterface<STATE_DIM, CONTROL_DIM>());
+#else
+ throw std::runtime_error("HPIPM selected but not built.");
+#endif
+ }
+ else
+ throw std::runtime_error("Solver for Linear Quadratic Optimal Control Problem wrongly specified.");
+
+ // set number of Eigen Threads (requires -fopenmp)
+ if (settings_.nThreadsEigen > 1)
+ {
+ Eigen::setNbThreads(settings.nThreadsEigen);
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP] Eigen using " << Eigen::nbThreads() << " threads." << std::endl;
+#endif
+ }
+
+ lqocSolver_->configure(settings);
+
+ settings_ = settings;
+
+ reset();
+
+ configured_ = true;
+}
+
+
+/*
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::simpleRollout(
+ const size_t threadId,
+ const ControlVectorArray& uff,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& x_local,
+ ControlVectorArray& u_recorded
+ )const
+{
+ const double& dt = settings_.dt;
+ const double dt_sim = settings_.getSimulationTimestep();
+ const size_t subSteps = settings_.K_sim;
+ const int K_local = K_;
+
+ u_recorded.clear();
+
+ x_local.resize(K_+1);
+ x_local.front() = x_ref_lqr.front();
+
+ if(uff.size() < (size_t)K_) throw std::runtime_error("simpleRollout: u_local is too short.");
+
+
+ for (int i = 1; i<K_local+1; i++)
+ {
+ x_local[i] = x_local[i-1];
+
+ u_recorded.push_back(uff[i-1] + L_[i-1] * (x_local[i-1] - x_ref_lqr[i-1]));
+
+ controller_[threadId]->setControl(u_recorded.back());
+
+
+ if(settings_.integrator == ct::core::IntegrationType::EULER_SYM || settings_.integrator == ct::core::IntegrationType::RK_SYM)
+ {
+ integrateSymplectic<V_DIM, P_DIM, STATE_DIM>(threadId, x_local[i], 0, subSteps, dt_sim);
+ } else
+ {
+ integrators_[threadId]->integrate_n_steps(x_local[i], 0, subSteps, dt_sim);
+ }
+
+
+ // check if nan
+ for (size_t k=0; k<STATE_DIM; k++)
+ {
+ if (isnan(x_local[i](k)))
+ {
+ x_local.resize(K_local+1, ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_recorded.resize(K_local, ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ return false;
+ }
+ }
+ for (size_t k=0; k<CONTROL_DIM; k++)
+ {
+ if (isnan(u_recorded.back()(k)))
+ {
+ x_local.resize(K_local+1, ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_recorded.resize(K_local, ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ std::cout << "control unstable" << std::endl;
+ return false;
+ }
+ }
+ }
+ return true;
+}
+*/
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutSingleShot(const size_t threadId,
+ const size_t k, //! the starting index of the shot
+ ControlVectorArray& u_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot, //! the value at the end of each integration interval
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag) const
+{
+ const double& dt = settings_.dt;
+ const double dt_sim = settings_.getSimulationTimestep();
+ const size_t subSteps = settings_.K_sim;
+ const int K_local = K_;
+
+ if (u_local.size() < (size_t)K_)
+ throw std::runtime_error("rolloutSingleShot: u_local is too short.");
+ if (x_local.size() < (size_t)K_ + 1)
+ throw std::runtime_error("rolloutSingleShot: x_local is too short.");
+ if (xShot.size() < (size_t)K_ + 1)
+ throw std::runtime_error("rolloutSingleShot: xShot is too short.");
+
+ xShot[k] = x_local[k]; // initialize
+
+ //! determine index where to stop at the latest
+ int K_stop = k + settings_.K_shot;
+ if (K_stop > K_local)
+ K_stop = K_local;
+
+ if (settings_.nlocp_algorithm == NLOptConSettings::NLOCP_ALGORITHM::ILQR)
+ K_stop = K_local; //! @todo this is not elegant - need to improve.
+
+ // for each control step
+ for (int i = (int)k; i < K_stop; i++)
+ {
+ if (terminationFlag && *terminationFlag)
+ return false;
+
+ substepRecorders_[threadId]->reset();
+
+ if (i > (int)k)
+ {
+ xShot[i] = xShot[i - 1]; //! initialize integration variable
+ }
+
+ // Todo: the order here is not optimal. In some cases, we will overwrite x_ref_lqr immediately in the next step!
+ if (settings_.closedLoopShooting) // overwrite control
+ u_local[i] += L_[i] * (xShot[i] - x_ref_lqr[i]);
+
+ //! @todo: here we override the state trajectory directly (as passed by reference). This is bad.
+ if (i > (int)k)
+ {
+ x_local[i] = xShot[i - 1]; //!"overwrite" x_local
+ }
+
+
+ controller_[threadId]->setControl(u_local[i]);
+
+
+ if (settings_.integrator == ct::core::IntegrationType::EULER_SYM ||
+ settings_.integrator == ct::core::IntegrationType::RK_SYM)
+ {
+ integrateSymplectic<V_DIM, P_DIM, STATE_DIM>(threadId, xShot[i], i * dt, subSteps, dt_sim);
+ }
+ else
+ {
+ integrators_[threadId]->integrate_n_steps(xShot[i], i * dt, subSteps, dt_sim);
+ }
+
+
+ if (i == K_local - 1)
+ x_local[K_local] = xShot[K_local - 1]; //! fill in terminal state if required
+
+
+ // check if nan
+ for (size_t j = 0; j < STATE_DIM; j++)
+ {
+ if (std::isnan(x_local[i](j)))
+ {
+ x_local.resize(K_local + 1,
+ ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_local.resize(K_local,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ return false;
+ }
+ }
+ for (size_t j = 0; j < CONTROL_DIM; j++)
+ {
+ if (std::isnan(u_local[i](j)))
+ {
+ x_local.resize(K_local + 1,
+ ct::core::StateVector<STATE_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ u_local.resize(K_local,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>::Constant(std::numeric_limits<SCALAR>::quiet_NaN()));
+ std::cout << "control unstable" << std::endl;
+ return false;
+ }
+ }
+
+ // get substeps for sensitivities
+ substepsX[i] = substepRecorders_[threadId]->getSubstates();
+ substepsU[i] = substepRecorders_[threadId]->getSubcontrols();
+ }
+
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_ENABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const
+{
+ if (!systems_[threadId]->isSymplectic())
+ throw std::runtime_error("Trying to integrate using symplectic integrator, but system is not symplectic.");
+
+ if (settings_.integrator == ct::core::IntegrationType::EULER_SYM)
+ {
+ integratorsEulerSymplectic_[threadId]->integrate_n_steps(x0, t, steps, dt_sim);
+ }
+ else if (settings_.integrator == ct::core::IntegrationType::RK_SYM)
+ {
+ integratorsRkSymplectic_[threadId]->integrate_n_steps(x0, t, steps, dt_sim);
+ }
+ else
+ {
+ throw std::runtime_error("invalid symplectic integrator specified");
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SYMPLECTIC_DISABLED NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const
+{
+ throw std::runtime_error("Symplectic integrator selected but invalid dimensions for it. Check V_DIM>1, P_DIM>1");
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShotsSingleThreaded(size_t threadId,
+ size_t firstIndex,
+ size_t lastIndex,
+ ControlVectorArray& u_ff_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot,
+ StateVectorArray& d,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU) const
+{
+ //! make sure all intermediate entries in the defect trajectory are zero
+ d.setConstant(state_vector_t::Zero());
+
+ for (size_t k = firstIndex; k <= lastIndex; k = k + settings_.K_shot)
+ {
+ // first rollout the shot
+ bool dynamicsGood = rolloutSingleShot(threadId, k, u_ff_local, x_local, x_ref_lqr, xShot, substepsX, substepsU);
+
+ if (!dynamicsGood)
+ return false;
+
+ // then compute the corresponding defect
+ computeSingleDefect(k, x_local, xShot, d);
+ }
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeSingleDefect(size_t k,
+ const StateVectorArray& x_local,
+ const StateVectorArray& xShot,
+ StateVectorArray& d) const
+{
+ //! compute the index where the next shot starts (respect total number of stages)
+ int k_next = std::min(K_, (int)k + settings_.K_shot);
+
+ if (k_next < K_)
+ {
+ d[k_next - 1] = xShot[k_next - 1] - x_local[k_next];
+ }
+ //! else ... all other entries of d remain zero.
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeCostsOfTrajectory(size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost) const
+{
+ intermediateCost = 0;
+
+ for (size_t k = 0; k < (size_t)K_; k++)
+ {
+ // feed current state and control to cost function
+ costFunctions_[threadId]->setCurrentStateAndControl(x_local[k], u_local[k], settings_.dt * k);
+
+ // derivative of cost with respect to state
+ intermediateCost += costFunctions_[threadId]->evaluateIntermediate();
+ }
+ intermediateCost *= settings_.dt;
+
+ costFunctions_[threadId]->setCurrentStateAndControl(x_local[K_], control_vector_t::Zero(), settings_.dt * K_);
+ finalCost = costFunctions_[threadId]->evaluateTerminal();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeBoxConstraintErrorOfTrajectory(
+ size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const
+{
+ e_tot = 0;
+
+ // intermediate constraints
+ for (size_t k = 0; k < (size_t)K_; k++)
+ {
+ if (boxConstraints_[threadId] != nullptr)
+ {
+ if (boxConstraints_[threadId]->getIntermediateConstraintsCount() > 0)
+ {
+ boxConstraints_[threadId]->setCurrentStateAndControl(x_local[k], u_local[k], settings_.dt * k);
+ Eigen::Matrix<SCALAR, -1, 1> box_err = boxConstraints_[threadId]->getTotalBoundsViolationIntermediate();
+ e_tot += box_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+ }
+
+ // normalize integrated violation by time
+ e_tot *= settings_.dt;
+
+ // terminal constraint violation
+ if (boxConstraints_[threadId] != nullptr)
+ {
+ if (boxConstraints_[threadId]->getTerminalConstraintsCount() > 0)
+ {
+ boxConstraints_[threadId]->setCurrentStateAndControl(
+ x_local[K_], control_vector_t::Zero(), settings_.dt * K_);
+ Eigen::Matrix<SCALAR, -1, 1> box_err = boxConstraints_[threadId]->getTotalBoundsViolationTerminal();
+ e_tot += box_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeGeneralConstraintErrorOfTrajectory(
+ size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const
+{
+ e_tot = 0;
+
+ // intermediate constraints
+ for (size_t k = 0; k < (size_t)K_; k++)
+ {
+ if (generalConstraints_[threadId] != nullptr)
+ {
+ if (generalConstraints_[threadId]->getIntermediateConstraintsCount() > 0)
+ {
+ generalConstraints_[threadId]->setCurrentStateAndControl(x_local[k], u_local[k], settings_.dt * k);
+ Eigen::Matrix<SCALAR, -1, 1> gen_err =
+ generalConstraints_[threadId]->getTotalBoundsViolationIntermediate();
+ e_tot += gen_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+ }
+
+ // normalize integrated violation by time
+ e_tot *= settings_.dt;
+
+ if (generalConstraints_[threadId] != nullptr)
+ {
+ if (generalConstraints_[threadId]->getTerminalConstraintsCount() > 0)
+ {
+ generalConstraints_[threadId]->setCurrentStateAndControl(
+ x_local[K_], control_vector_t::Zero(), settings_.dt * K_);
+ Eigen::Matrix<SCALAR, -1, 1> gen_err = generalConstraints_[threadId]->getTotalBoundsViolationTerminal();
+ e_tot += gen_err.norm(); // TODO check if we should use different norms here
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLinearizedDynamics(size_t threadId, size_t k)
+{
+ LQOCProblem_t& p = *lqocProblem_;
+
+ /*!
+ * @todo
+ * Note the little 'hack' that is applied here. We need a control input to linearize around for the last stage N.
+ * Should it be zero? We currently set it to be the second-to-last control input.
+ */
+ const core::ControlVector<CONTROL_DIM, SCALAR> u_last = u_ff_[std::min((int)k + 1, K_ - 1)];
+
+ assert(lqocProblem_ != nullptr);
+
+ assert(&lqocProblem_->A_[k] != nullptr);
+ assert(&lqocProblem_->B_[k] != nullptr);
+
+ assert(lqocProblem_->A_.size() > k);
+ assert(lqocProblem_->B_.size() > k);
+
+ sensitivity_[threadId]->setSubstepTrajectoryReference(substepsX_.get(), substepsU_.get());
+ sensitivity_[threadId]->getAandB(x_[k], u_ff_[k], xShot_[k], (int)k, settings_.K_sim, p.A_[k], p.B_[k]);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setBoxConstraintsForLQOCProblem()
+{
+ // set box constraints if there are any
+ if (boxConstraints_[settings_.nThreads] != nullptr)
+ {
+ // temp vars
+ Eigen::VectorXi foo, u_sparsity_intermediate, x_sparsity_intermediate, x_sparsity_terminal;
+
+ // intermediate box constraints
+ const int nb_ux_intermediate = boxConstraints_[settings_.nThreads]->getJacobianStateNonZeroCountIntermediate() +
+ boxConstraints_[settings_.nThreads]->getJacobianInputNonZeroCountIntermediate();
+
+ if (nb_ux_intermediate > 0)
+ {
+ boxConstraints_[settings_.nThreads]->sparsityPatternInputIntermediate(foo, u_sparsity_intermediate);
+ boxConstraints_[settings_.nThreads]->sparsityPatternStateIntermediate(foo, x_sparsity_intermediate);
+
+ Eigen::VectorXi ux_sparsity_intermediate(nb_ux_intermediate);
+ x_sparsity_intermediate.array() += CONTROL_DIM; // shift indices to match combined decision vector [u, x]
+ ux_sparsity_intermediate << u_sparsity_intermediate, x_sparsity_intermediate;
+
+ lqocProblem_->setIntermediateBoxConstraints(nb_ux_intermediate,
+ boxConstraints_[settings_.nThreads]->getLowerBoundsIntermediate(),
+ boxConstraints_[settings_.nThreads]->getUpperBoundsIntermediate(), ux_sparsity_intermediate);
+ }
+
+ // terminal box constraints
+ const int nb_x_terminal = boxConstraints_[settings_.nThreads]->getJacobianStateNonZeroCountTerminal();
+
+ if (nb_x_terminal > 0)
+ {
+ boxConstraints_[settings_.nThreads]->sparsityPatternStateTerminal(foo, x_sparsity_terminal);
+
+ lqocProblem_->setTerminalBoxConstraints(nb_x_terminal,
+ boxConstraints_[settings_.nThreads]->getLowerBoundsTerminal(),
+ boxConstraints_[settings_.nThreads]->getUpperBoundsTerminal(), x_sparsity_terminal);
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLinearizedConstraints(size_t threadId,
+ size_t k)
+{
+ // set general if there are any
+ if (generalConstraints_[threadId] != nullptr)
+ {
+ LQOCProblem_t& p = *lqocProblem_;
+ const scalar_t& dt = settings_.dt;
+
+ // treat general constraints
+ generalConstraints_[threadId]->setCurrentStateAndControl(x_[k], u_ff_[k], dt * k);
+
+ p.ng_[k] = generalConstraints_[threadId]->getIntermediateConstraintsCount();
+ if (p.ng_[k] > 0)
+ {
+ p.hasGenConstraints_ = true;
+ p.C_[k] = generalConstraints_[threadId]->jacobianStateIntermediate();
+ p.D_[k] = generalConstraints_[threadId]->jacobianInputIntermediate();
+
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> g_eval = generalConstraints_[threadId]->evaluateIntermediate();
+
+ // rewrite constraint in absolute coordinates as required by LQOC problem
+ p.d_lb_[k] = generalConstraints_[threadId]->getLowerBoundsIntermediate() - g_eval + p.C_[k] * x_[k] +
+ p.D_[k] * u_ff_[k];
+ p.d_ub_[k] = generalConstraints_[threadId]->getUpperBoundsIntermediate() - g_eval + p.C_[k] * x_[k] +
+ p.D_[k] * u_ff_[k];
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeQuadraticCosts(size_t threadId, size_t k)
+{
+ LQOCProblem_t& p = *lqocProblem_;
+ const scalar_t& dt = settings_.dt;
+
+ // feed current state and control to cost function
+ costFunctions_[threadId]->setCurrentStateAndControl(x_[k], u_ff_[k], dt * k);
+
+ // derivative of cost with respect to state
+ p.q_[k] = costFunctions_[threadId]->evaluateIntermediate() * dt;
+
+ p.qv_[k] = costFunctions_[threadId]->stateDerivativeIntermediate() * dt;
+
+ p.Q_[k] = costFunctions_[threadId]->stateSecondDerivativeIntermediate() * dt;
+
+ // derivative of cost with respect to control and state
+ p.P_[k] = costFunctions_[threadId]->stateControlDerivativeIntermediate() * dt;
+
+ // derivative of cost with respect to control
+ p.rv_[k] = costFunctions_[threadId]->controlDerivativeIntermediate() * dt;
+
+ p.R_[k] = costFunctions_[threadId]->controlSecondDerivativeIntermediate() * dt;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initializeCostToGo()
+{
+ LQOCProblem_t& p = *lqocProblem_;
+
+ // feed current state and control to cost function
+ costFunctions_[settings_.nThreads]->setCurrentStateAndControl(x_[K_], control_vector_t::Zero(), settings_.dt * K_);
+
+ // derivative of terminal cost with respect to state
+ p.q_[K_] = costFunctions_[settings_.nThreads]->evaluateTerminal();
+ p.qv_[K_] = costFunctions_[settings_.nThreads]->stateDerivativeTerminal();
+ p.Q_[K_] = costFunctions_[settings_.nThreads]->stateSecondDerivativeTerminal();
+
+ // init terminal general constraints, if any
+ if (generalConstraints_[settings_.nThreads] != nullptr)
+ {
+ p.ng_[K_] = generalConstraints_[settings_.nThreads]->getTerminalConstraintsCount();
+ if (p.ng_[K_] > 0)
+ {
+ p.hasGenConstraints_ = true;
+ p.C_[K_] = generalConstraints_[settings_.nThreads]->jacobianStateTerminal();
+ p.D_[K_] = generalConstraints_[settings_.nThreads]->jacobianInputTerminal();
+
+ Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> g_eval =
+ generalConstraints_[settings_.nThreads]->evaluateTerminal();
+
+ p.d_lb_[K_] =
+ generalConstraints_[settings_.nThreads]->getLowerBoundsTerminal() - g_eval + p.C_[K_] * x_[K_];
+ p.d_ub_[K_] =
+ generalConstraints_[settings_.nThreads]->getUpperBoundsTerminal() - g_eval + p.C_[K_] * x_[K_];
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::printSummary()
+{
+ SCALAR d_norm_l1 = computeDefectsNorm<1>(lqocProblem_->b_);
+ SCALAR d_norm_l2 = computeDefectsNorm<2>(lqocProblem_->b_);
+ SCALAR totalCost = intermediateCostBest_ + finalCostBest_;
+ SCALAR totalMerit = intermediateCostBest_ + finalCostBest_ + settings_.meritFunctionRho * d_norm_l1;
+
+ SCALAR smallestEigenvalue = 0.0;
+ if (settings_.recordSmallestEigenvalue && settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ smallestEigenvalue = lqocSolver_->getSmallestEigenvalue();
+ }
+
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+
+ summaryAllIterations_.iterations.push_back(iteration_);
+ summaryAllIterations_.defect_l1_norms.push_back(d_norm_l1);
+ summaryAllIterations_.defect_l2_norms.push_back(d_norm_l2);
+ summaryAllIterations_.e_box_norms.push_back(e_box_norm_);
+ summaryAllIterations_.e_gen_norms.push_back(e_gen_norm_);
+ summaryAllIterations_.lx_norms.push_back(lx_norm_);
+ summaryAllIterations_.lu_norms.push_back(lu_norm_);
+ summaryAllIterations_.intermediateCosts.push_back(intermediateCostBest_);
+ summaryAllIterations_.finalCosts.push_back(finalCostBest_);
+ summaryAllIterations_.totalCosts.push_back(totalCost);
+ summaryAllIterations_.merits.push_back(totalMerit);
+ summaryAllIterations_.stepSizes.push_back(alphaBest_);
+ summaryAllIterations_.smallestEigenvalues.push_back(smallestEigenvalue);
+
+ if (settings_.printSummary)
+ summaryAllIterations_.printSummaryLastIteration();
+
+ //! @todo the printing of the smallest eigenvalue is hacky
+ if (settings_.printSummary && settings_.recordSmallestEigenvalue &&
+ settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ std::cout << std::setprecision(15) << "smallest eigenvalue this iteration: " << smallestEigenvalue << std::endl;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logToMatlab(const size_t& iteration)
+{
+#ifdef MATLAB_FULL_LOG
+
+ std::cout << "Logging to Matlab" << std::endl;
+
+ LQOCProblem_t& p = *lqocProblem_;
+
+ matFile_.open(settings_.loggingPrefix + "Log" + std::to_string(iteration) + ".mat");
+
+ matFile_.put("iteration", iteration);
+ matFile_.put("K", K_);
+ matFile_.put("dt", settings_.dt);
+ matFile_.put("K_sim", settings_.K_sim);
+ matFile_.put("x", x_.toImplementation());
+ matFile_.put("u_ff", u_ff_.toImplementation());
+ matFile_.put("t", t_.toEigenTrajectory());
+ matFile_.put("d", lqocProblem_->b_.toImplementation());
+ matFile_.put("xShot", xShot_.toImplementation());
+
+ matFile_.put("A", p.A_.toImplementation());
+ matFile_.put("B", p.B_.toImplementation());
+ matFile_.put("qv", p.qv_.toImplementation());
+ matFile_.put("Q", p.Q_.toImplementation());
+ matFile_.put("P", p.P_.toImplementation());
+ matFile_.put("rv", p.rv_.toImplementation());
+ matFile_.put("R", p.R_.toImplementation());
+ matFile_.put("q", p.q_.toEigenTrajectory());
+
+ matFile_.put("lx_norm", lx_norm_);
+ matFile_.put("lu_norm", lu_norm_);
+ matFile_.put("cost", getCost());
+ matFile_.put("alphaStep", alphaBest_);
+
+ d_norm_ = computeDefectsNorm<1>(lqocProblem_->b_);
+ matFile_.put("d_norm", d_norm_);
+
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+ matFile_.put("e_box_norm", e_box_norm_);
+ matFile_.put("e_gen_norm", e_gen_norm_);
+
+ matFile_.close();
+#endif //MATLAB_FULL_LOG
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logInitToMatlab()
+{
+#ifdef MATLAB
+
+ std::cout << "Logging init guess to Matlab" << std::endl;
+
+ matFile_.open(settings_.loggingPrefix + "LogInit.mat");
+
+ matFile_.put("K", K_);
+ matFile_.put("dt", settings_.dt);
+ matFile_.put("K_sim", settings_.K_sim);
+
+ matFile_.put("x", x_.toImplementation());
+ matFile_.put("u_ff", u_ff_.toImplementation());
+ matFile_.put("d", lqocProblem_->b_.toImplementation());
+ matFile_.put("cost", getCost());
+
+ d_norm_ = computeDefectsNorm<1>(lqocProblem_->b_);
+ matFile_.put("d_norm", d_norm_);
+
+ computeBoxConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_box_norm_);
+ computeGeneralConstraintErrorOfTrajectory(settings_.nThreads, x_, u_ff_, e_gen_norm_);
+ matFile_.put("e_box_norm", e_box_norm_);
+ matFile_.put("e_gen_norm", e_gen_norm_);
+
+ matFile_.close();
+#endif
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::ControlTrajectory<CONTROL_DIM, SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getControlTrajectory() const
+{
+ // \todo this method currently copies the time array (suboptimal)
+
+ core::tpl::TimeArray<SCALAR> t_control = t_;
+ t_control.pop_back();
+
+ return core::ControlTrajectory<CONTROL_DIM, SCALAR>(t_control, u_ff_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::StateTrajectory<STATE_DIM, SCALAR>
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getStateTrajectory() const
+{
+ //! \todo this method currently copies the time array (suboptimal)
+
+ core::tpl::TimeArray<SCALAR> t_control = t_;
+
+ return core::StateTrajectory<STATE_DIM, SCALAR>(t_control, x_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCost() const
+{
+ return intermediateCostBest_ + finalCostBest_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::lineSearchSingleShooting()
+{
+ // lowest cost is cost of last rollout
+ lowestCost_ = intermediateCostBest_ + finalCostBest_;
+ scalar_t lowestCostPrevious = lowestCost_;
+
+ //! backup controller that led to current trajectory
+ u_ff_prev_ = u_ff_; // todo note this might be redundant
+ x_prev_ = x_; // todo note this might be redundant
+
+ if (!settings_.lineSearchSettings.active)
+ {
+ StateVectorArray x_ref_lqr_local(K_ + 1);
+ ControlVectorArray uff_local(K_);
+
+ uff_local = u_ff_ + lu_; // add lu
+ x_ref_lqr_local = x_prev_ + lx_; // stabilize around current solution candidate
+
+ bool dynamicsGood = rolloutSingleShot(
+ settings_.nThreads, 0, uff_local, x_, x_ref_lqr_local, xShot_, *this->substepsX_, *this->substepsU_);
+
+ if (dynamicsGood)
+ {
+ intermediateCostBest_ = std::numeric_limits<scalar_t>::max();
+ finalCostBest_ = std::numeric_limits<scalar_t>::max();
+ computeCostsOfTrajectory(settings_.nThreads, x_, uff_local, intermediateCostBest_, finalCostBest_);
+ lowestCost_ = intermediateCostBest_ + finalCostBest_;
+
+ if (settings_.printSummary)
+ {
+ //! compute l2 norms of state and control update
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, uff_local);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+ }
+ else
+ {
+#ifdef MATLAB // in case of no debug printing but still logging, need to compute them \
+ //! compute l2 norms of state and control update
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, uff_local);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+#endif
+ }
+
+ x_prev_ = x_;
+ u_ff_.swap(uff_local);
+ alphaBest_ = 1;
+ }
+ else
+ {
+ if (settings_.debugPrint)
+ {
+ std::cout << "CONVERGED: System became unstable!" << std::endl;
+ }
+ return false;
+ }
+ }
+ else
+ {
+ // merit of previous trajectory
+ lowestCost_ = intermediateCostBest_ + finalCostBest_ +
+ (e_box_norm_ + e_gen_norm_) * settings_.meritFunctionRhoConstraints;
+ lowestCostPrevious = lowestCost_;
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Starting line search." << std::endl;
+ std::cout << "[LineSearch]: Merit last rollout: " << lowestCost_ << std::endl;
+ }
+
+ alphaBest_ = performLineSearch();
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Best control found at alpha: " << alphaBest_ << " . Will use this control."
+ << std::endl;
+ }
+
+ if (alphaBest_ == 0.0)
+ {
+ if (settings_.debugPrint)
+ {
+ {
+ std::cout << "WARNING: No better control found. Converged." << std::endl;
+ }
+ return false;
+ }
+ }
+ }
+
+ if ((fabs((lowestCostPrevious - lowestCost_) / lowestCostPrevious)) > settings_.min_cost_improvement)
+ {
+ return true; //! found better cost
+ }
+
+ if (settings_.debugPrint)
+ {
+ std::cout << "CONVERGED: Cost last iteration: " << lowestCostPrevious << ", current cost: " << lowestCost_
+ << std::endl;
+ std::cout << "CONVERGED: Cost improvement ratio was: "
+ << (lowestCostPrevious - lowestCost_) / lowestCostPrevious
+ << "x, which is lower than convergence criteria: " << settings_.min_cost_improvement << std::endl;
+ }
+ return false;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::executeLineSearchSingleShooting(
+ const size_t threadId,
+ const scalar_t alpha,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag) const
+{
+ intermediateCost = std::numeric_limits<scalar_t>::infinity();
+ finalCost = std::numeric_limits<scalar_t>::infinity();
+ e_box_norm = 0.0;
+ e_gen_norm = 0.0;
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ StateVectorArray x_ref_lqr;
+ StateVectorArray xShot_local(K_ + 1); //! note this is currently only a dummy (\todo nicer solution?)
+
+ //! if stabilizing about new solution candidate, choose lu as feedforward increment and also increment x_prev_ by lx
+ u_local = lu_ * alpha + u_ff_prev_;
+ x_ref_lqr = lx_ * alpha + x_prev_;
+
+ bool dynamicsGood =
+ rolloutSingleShot(threadId, 0, u_local, x_local, x_ref_lqr, xShot_local, substepsX, substepsU, terminationFlag);
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ if (dynamicsGood)
+ {
+ // compute cost specific to this alpha
+ computeCostsOfTrajectory(threadId, x_local, u_local, intermediateCost, finalCost);
+
+ // compute constraint violations specific to this alpha
+ if (boxConstraints_[threadId] != nullptr)
+ computeBoxConstraintErrorOfTrajectory(threadId, x_local, u_local, e_box_norm);
+ if (generalConstraints_[threadId] != nullptr)
+ computeGeneralConstraintErrorOfTrajectory(threadId, x_local, u_local, e_gen_norm);
+ }
+ else
+ {
+ if (settings_.debugPrint)
+ {
+ std::string msg = std::string("dynamics not good, thread: ") + std::to_string(threadId);
+ std::cout << msg << std::endl;
+ }
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::lineSearchMultipleShooting()
+{
+ // lowest cost
+ scalar_t lowestCostPrevious;
+
+ //! backup controller that led to current trajectory
+ u_ff_prev_ = u_ff_;
+ x_prev_ = x_;
+
+
+ if (!settings_.lineSearchSettings.active) //! do full step updates
+ {
+ //! lowest cost is cost of last rollout
+ lowestCostPrevious = intermediateCostBest_ + finalCostBest_;
+
+ //! update control and states
+ doFullStepUpdate();
+
+ resetDefects();
+
+ rolloutShots(0, K_ - 1);
+
+ updateCosts();
+
+ lowestCost_ = intermediateCostBest_ + finalCostBest_;
+
+ if (settings_.printSummary)
+ {
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, u_ff_);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+ }
+ else
+ {
+#ifdef MATLAB
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, u_ff_);
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
+#endif
+ }
+ x_prev_ = x_;
+ alphaBest_ = 1;
+ }
+ else //! do line search over a merit function trading off costs and constraint violations
+ {
+ // merit of previous trajectory
+ lowestCost_ = intermediateCostBest_ + finalCostBest_ + d_norm_ * settings_.meritFunctionRho;
+ lowestCostPrevious = lowestCost_;
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Starting line search." << std::endl;
+ std::cout << "[LineSearch]: Cost of last rollout:\t" << intermediateCostBest_ + finalCostBest_ << std::endl;
+ std::cout << "[LineSearch]: Defect norm last rollout:\t" << d_norm_ << std::endl;
+ std::cout << "[LineSearch]: Merit of last rollout:\t" << lowestCost_ << std::endl;
+ }
+
+ alphaBest_ = performLineSearch();
+
+ if (settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: Best control found at alpha: " << alphaBest_ << ", with trade-off "
+ << std::endl;
+ std::cout << "[LineSearch]: Cost:\t" << intermediateCostBest_ + finalCostBest_ << std::endl;
+ std::cout << "[LineSearch]: Defect:\t" << d_norm_ << std::endl;
+ }
+
+ if (alphaBest_ == 0.0)
+ {
+ if (settings_.debugPrint || settings_.printSummary)
+ {
+ std::cout << "WARNING: No better control found during line search. Converged." << std::endl;
+ }
+ return false;
+ }
+ }
+
+ if ((fabs((lowestCostPrevious - lowestCost_) / lowestCostPrevious)) > settings_.min_cost_improvement)
+ {
+ return true; //! found better cost
+ }
+
+ if (settings_.debugPrint)
+ {
+ std::cout << "CONVERGED: Cost last iteration: " << lowestCostPrevious << ", current cost: " << lowestCost_
+ << std::endl;
+ std::cout << "CONVERGED: Cost improvement ratio was: "
+ << fabs(lowestCostPrevious - lowestCost_) / lowestCostPrevious
+ << "x, which is lower than convergence criteria: " << settings_.min_cost_improvement << std::endl;
+ }
+ return false;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::executeLineSearchMultipleShooting(
+ const size_t threadId,
+ const scalar_t alpha,
+ const ControlVectorArray& u_ff_update,
+ const StateVectorArray& x_update,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_alpha,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_shot_alpha,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& defects_recorded,
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_alpha,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& defectNorm,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag) const
+{
+ intermediateCost = std::numeric_limits<scalar_t>::max();
+ finalCost = std::numeric_limits<scalar_t>::max();
+ defectNorm = std::numeric_limits<scalar_t>::max();
+ e_box_norm = 0.0;
+ e_gen_norm = 0.0;
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ //! update feedforward
+ u_alpha = u_ff_update * alpha + u_ff_prev_;
+
+ //! update state decision variables
+ x_alpha = x_update * alpha + x_prev_;
+
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ bool dynamicsGood = rolloutShotsSingleThreaded(
+ threadId, 0, K_ - 1, u_alpha, x_alpha, x_alpha, x_shot_alpha, defects_recorded, substepsX, substepsU);
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ //! compute defects norm
+ defectNorm = computeDefectsNorm<1>(defects_recorded);
+
+ if (terminationFlag && *terminationFlag)
+ return;
+
+ //! compute costs
+ if (dynamicsGood)
+ {
+ computeCostsOfTrajectory(threadId, x_alpha, u_alpha, intermediateCost, finalCost);
+
+ // compute constraint violations specific to this alpha
+ if (boxConstraints_[threadId] != nullptr)
+ computeBoxConstraintErrorOfTrajectory(threadId, x_alpha, u_alpha, e_box_norm);
+ if (generalConstraints_[threadId] != nullptr)
+ computeGeneralConstraintErrorOfTrajectory(threadId, x_alpha, u_alpha, e_gen_norm);
+ }
+ else
+ {
+ if (settings_.debugPrint)
+ {
+ std::string msg = std::string("dynamics not good, thread: ") + std::to_string(threadId);
+ std::cout << msg << std::endl;
+ }
+ }
+
+ if (terminationFlag && *terminationFlag)
+ return;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareSolveLQProblem(size_t startIndex)
+{
+ lqpCounter_++;
+
+ // if solver is HPIPM, there's nothing to prepare
+ if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::HPIPM_SOLVER)
+ {
+ }
+ // if solver is GNRiccati - we iterate backward up to the first stage
+ else if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ lqocProblem_->x_ = x_;
+ lqocProblem_->u_ = u_ff_;
+ lqocSolver_->setProblem(lqocProblem_);
+
+ //iterate backward up to first stage
+ for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= startIndex; i--)
+ lqocSolver_->solveSingleStage(i);
+ }
+ else
+ throw std::runtime_error("unknown solver type in prepareSolveLQProblem()");
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishSolveLQProblem(size_t endIndex)
+{
+ lqpCounter_++;
+
+ // if solver is HPIPM, solve the full problem
+ if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::HPIPM_SOLVER)
+ {
+ solveFullLQProblem();
+ }
+ else if (settings_.lqocp_solver == Settings_t::LQOCP_SOLVER::GNRICCATI_SOLVER)
+ {
+ // if solver is GNRiccati, solve the first stage and get solution
+ lqocProblem_->x_ = x_;
+ lqocProblem_->u_ = u_ff_;
+ lqocSolver_->setProblem(lqocProblem_);
+
+ for (int i = endIndex; i >= 0; i--)
+ lqocSolver_->solveSingleStage(i);
+
+ lqocSolver_->computeStateAndControlUpdates();
+ }
+ else
+ throw std::runtime_error("unknown solver type in finishSolveLQProblem()");
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::solveFullLQProblem()
+{
+ lqpCounter_++;
+
+ lqocProblem_->x_ = x_;
+ lqocProblem_->u_ = u_ff_;
+
+ if (lqocProblem_->isBoxConstrained())
+ {
+ if (settings_.debugPrint)
+ std::cout << "LQ Problem has box constraints. Configuring box constraints now. " << std::endl;
+
+ lqocSolver_->configureBoxConstraints(lqocProblem_);
+ }
+ if (lqocProblem_->isGeneralConstrained())
+ {
+ if (settings_.debugPrint)
+ std::cout << "LQ Problem has general constraints. Configuring general constraints now. " << std::endl;
+
+ lqocSolver_->configureGeneralConstraints(lqocProblem_);
+ }
+
+ lqocSolver_->setProblem(lqocProblem_);
+
+ lqocSolver_->solve();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::updateCosts()
+{
+ intermediateCostPrevious_ = intermediateCostBest_;
+ finalCostPrevious_ = finalCostBest_;
+ computeCostsOfTrajectory(settings_.nThreads, x_, u_ff_, intermediateCostBest_, finalCostBest_);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::retrieveLastLinearizedModel(StateMatrixArray& A,
+ StateControlMatrixArray& B)
+{
+ A = lqocProblem_->A_;
+ B = lqocProblem_->B_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Policy_t&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSolution()
+{
+ policy_.update(x_, u_ff_, L_, t_);
+ return policy_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::reset()
+{
+ firstRollout_ = true;
+ iteration_ = 0;
+ d_norm_ = std::numeric_limits<scalar_t>::infinity();
+ lx_norm_ = std::numeric_limits<scalar_t>::infinity();
+ lu_norm_ = std::numeric_limits<scalar_t>::infinity();
+ intermediateCostBest_ = std::numeric_limits<scalar_t>::infinity();
+ finalCostBest_ = std::numeric_limits<scalar_t>::infinity();
+ intermediateCostPrevious_ = std::numeric_limits<scalar_t>::infinity();
+ finalCostPrevious_ = std::numeric_limits<scalar_t>::infinity();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::doFullStepUpdate()
+{
+ u_ff_ += lu_;
+ x_ += lx_;
+
+ alphaBest_ = 1.0;
+
+ if (settings_.debugPrint || settings_.printSummary)
+ {
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(lx_);
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(lu_);
+ }
+ else
+ {
+#ifdef MATLAB
+ lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(lx_);
+ lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(lu_);
+#endif
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logSummaryToMatlab(const std::string& fileName)
+{
+ summaryAllIterations_.logToMatlab(fileName);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const SummaryAllIterations<SCALAR>& NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSummary() const
+{
+ return summaryAllIterations_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getStateUpdates()
+{
+ lx_ = lqocSolver_->getStateUpdates();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getControlUpdates()
+{
+ lu_ = lqocSolver_->getControlUpdates();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getFeedback()
+{
+ if (settings_.closedLoopShooting)
+ lqocSolver_->getFeedback(L_);
+ else
+ L_.setConstant(core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero()); // TODO should eventually go away
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::resetDefects()
+{
+ lqocProblem_->b_.setConstant(state_vector_t::Zero());
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDefectsNorm()
+{
+ d_norm_ = computeDefectsNorm<1>(lqocProblem_->b_);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+size_t& NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::iteration()
+{
+ return iteration_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::nominalRollout()
+{
+ bool success =
+ rolloutSingleShot(settings_.nThreads, 0, u_ff_, x_, x_prev_, xShot_, *this->substepsX_, *this->substepsU_);
+ x_prev_ = x_;
+ u_ff_prev_ = u_ff_;
+ firstRollout_ = false;
+ return success;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::TimeArray&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeArray()
+{
+ return t_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::isConfigured()
+{
+ return configured_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::isInitialized()
+{
+ return initialized_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTotalDefect() const
+{
+ return d_norm_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::testConsistency()
+{
+ return true;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances()
+{
+ return systems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances() const
+{
+ return systems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances()
+{
+ return linearSystems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances() const
+{
+ return linearSystems_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances()
+{
+ return costFunctions_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances() const
+{
+ return costFunctions_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances()
+{
+ return boxConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances() const
+{
+ return boxConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances()
+{
+ return generalConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances() const
+{
+ return generalConstraints_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeHorizon()
+{
+ return K_ * settings_.dt;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+int NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNumSteps()
+{
+ return K_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+int NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNumStepsPerShot()
+{
+ return settings_.K_shot;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Settings_t&
+NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSettings() const
+{
+ return settings_;
+}
+
+} // namespace optcon
+} // namespace ct
+
+
+#undef SYMPLECTIC_ENABLED
+#undef SYMPLECTIC_DISABLED
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp
new file mode 100644
index 0000000..2c7a641
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp
@@ -0,0 +1,724 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <atomic>
+
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+#include <ct/optcon/solver/OptConSolver.h>
+
+#include <ct/optcon/problem/LQOCProblem.hpp>
+
+#include <ct/optcon/solver/lqp/GNRiccatiSolver.hpp>
+#include <ct/optcon/solver/lqp/HPIPMInterface.hpp>
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+#include "NLOCResults.hpp"
+
+#ifdef MATLAB
+#include <ct/optcon/matlab.hpp>
+#endif
+
+#define SYMPLECTIC_ENABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V > 0 && P > 0 && (V+P==ST)), void>::type
+#define SYMPLECTIC_DISABLED \
+ template <size_t V, size_t P, size_t ST> \
+ typename std::enable_if<(V <= 0 || P <= 0 || (V+P!=ST)), void>::type
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * \ingroup GNMS
+ *
+ * \brief C++ implementation of GNMS.
+ *
+ * The implementation and naming is based on the reference below. In general, the code follows this convention:
+ * X <- Matrix (upper-case in paper)
+ * xv <- vector (lower-case bold in paper)
+ * x <- scalar (lower-case in paper)
+ *
+ */
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCBackendBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t state_dim = STATE_DIM;
+ static const size_t control_dim = CONTROL_DIM;
+
+ typedef NLOptConSettings Settings_t;
+ typedef core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+ typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+ typedef LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR> LQOCSolver_t;
+
+ typedef core::Sensitivity<STATE_DIM, CONTROL_DIM, SCALAR> Sensitivity_t;
+
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> StateVectorArray;
+ typedef std::shared_ptr<StateVectorArray> StateVectorArrayPtr;
+
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
+ typedef std::shared_ptr<ControlVectorArray> ControlVectorArrayPtr;
+
+ typedef std::vector<StateVectorArrayPtr, Eigen::aligned_allocator<StateVectorArrayPtr>> StateSubsteps;
+ typedef std::shared_ptr<StateSubsteps> StateSubstepsPtr;
+
+ typedef std::vector<ControlVectorArrayPtr, Eigen::aligned_allocator<ControlVectorArrayPtr>> ControlSubsteps;
+ typedef std::shared_ptr<ControlSubsteps> ControlSubstepsPtr;
+
+ using ControlMatrix = ct::core::ControlMatrix<CONTROL_DIM, SCALAR>;
+ using ControlMatrixArray = ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR>;
+ using StateMatrixArray = ct::core::StateMatrixArray<STATE_DIM, SCALAR>;
+ using StateControlMatrixArray = ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>;
+ using FeedbackArray = ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>;
+ using TimeArray = ct::core::tpl::TimeArray<SCALAR>;
+
+ using state_matrix_t = Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM>;
+ using control_matrix_t = Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM>;
+ using control_state_matrix_t = Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM>;
+ using state_control_matrix_t = Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM>;
+
+ using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
+ using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
+ using feedback_matrix_t = core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>;
+
+ using scalar_t = SCALAR;
+ using scalar_array_t = std::vector<SCALAR, Eigen::aligned_allocator<SCALAR>>;
+
+
+ NLOCBackendBase(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem, const Settings_t& settings);
+
+ NLOCBackendBase(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ virtual ~NLOCBackendBase();
+
+ //! configure the solver
+ /**
+ * Configure the solver
+ * @param settings solver settings
+ */
+ virtual void configure(const Settings_t& settings);
+
+ //! get the current SLQsolver settings
+ const Settings_t& getSettings() const;
+
+ /*!
+ * Set the initial guess used by the solver (not all solvers might support initial guesses)
+ */
+ void setInitialGuess(const Policy_t& initialGuess);
+
+
+ /*!
+ * \brief Change the time horizon the solver operates on.
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct time horizon set.
+ */
+ void changeTimeHorizon(const SCALAR& tf);
+
+ SCALAR getTimeHorizon();
+
+ int getNumSteps();
+
+ int getNumStepsPerShot();
+
+ SYMPLECTIC_ENABLED initializeSymplecticIntegrators(size_t i);
+ SYMPLECTIC_DISABLED initializeSymplecticIntegrators(size_t i);
+
+ SYMPLECTIC_ENABLED integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const;
+ SYMPLECTIC_DISABLED integrateSymplectic(size_t threadId,
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ const double& t,
+ const size_t& steps,
+ const double& dt_sim) const;
+
+
+ /*!
+ * \brief Change the initial state for the optimal control problem
+ */
+ void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0);
+
+ /*!
+ * \brief Change the cost function
+ */
+ void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf);
+
+ /*!
+ * \brief Change the nonlinear system
+ */
+ void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn);
+
+ /*!
+ * \brief Change the linear system
+ */
+ void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin);
+
+ /*!
+ * \brief Change the box constraints
+ */
+ void changeBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t& con);
+
+ /*!
+ * \brief Change the general constraints
+ */
+ void changeGeneralConstraints(const typename OptConProblem_t::ConstraintPtr_t& con);
+
+ /*!
+ * \brief Direct accessor to the system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeNonlinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances();
+
+ const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const;
+
+ /*!
+ * \brief Direct accessor to the linear system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeLinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances();
+
+ const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const;
+
+ /*!
+ * \brief Direct accessor to the cost function instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances();
+
+ const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const;
+
+ /**
+ * @brief Direct accessor to the box constraint instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The box constraint instances
+ */
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances();
+
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const;
+
+ /**
+ * @brief Direct accessor to the general constraints
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The general constraints instances.
+ */
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances();
+
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const;
+
+
+ /*!
+ * Tests consistency of the instance of the dynamics, linear systems and costs. This is not a test for thread safety.
+ * @return returns true if instances are consistent with each other
+ */
+ bool testConsistency();
+
+
+ //! Export all functions to matlab workspace
+ /*!
+ This function can be used for Debugging. It exports all variables to Matlab after each iteration. It also saves
+ the Matlab workspace to a .mat file.
+ */
+ void logToMatlab(const size_t& iteration);
+
+ //! log the initial guess to Matlab
+ void logInitToMatlab();
+
+ //! return the cost of the solution of the current iteration
+ SCALAR getCost() const;
+
+ //! return the sum of the L2-norm of the defects along the solution candidate
+ SCALAR getTotalDefect() const;
+
+ void reset();
+
+ const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const;
+
+ const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const;
+
+
+ const Policy_t& getSolution();
+
+ const TimeArray& getTimeArray();
+
+ bool isConfigured();
+
+ bool isInitialized();
+
+
+ //! Retrieve Last Linearized Model
+ /*!
+ Retrieve the linearized model computed during the last iteration
+ */
+ void retrieveLastLinearizedModel(StateMatrixArray& A, StateControlMatrixArray& B);
+
+ /*!
+ * the prepare Solve LQP Problem method is intended for a special use-case: unconstrained GNMS with pre-solving of the
+ */
+ virtual void prepareSolveLQProblem(size_t startIndex);
+
+
+ virtual void finishSolveLQProblem(size_t endIndex);
+
+ /*!
+ * solve Full LQProblem, e.g. to be used with HPIPM or if we have a constrained problem
+ */
+ virtual void solveFullLQProblem();
+
+ //! compute costs of solution candidate
+ void updateCosts();
+
+ //! nominal rollout using default thread and member variables for the results. // todo maybe rename (initial rollout?)
+ bool nominalRollout();
+
+ //! check problem for consistency
+ void checkProblem();
+
+ //! return the current iteration number
+ size_t& iteration();
+
+ //! Print iteration summary
+ /*!
+ * This function is automatically called if the printSummary settings is on. It prints out important information
+ * like cost etc. after each iteration.
+ */
+ void printSummary();
+
+ //! perform line-search and update controller for single shooting
+ bool lineSearchSingleShooting();
+
+ //! perform line-search and update controller for multiple shooting
+ bool lineSearchMultipleShooting();
+
+ //! build LQ approximation around trajectory (linearize dynamics and general constraints, quadratize cost)
+ virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) = 0;
+
+ //! sets the box constraints for the entire time horizon including terminal stage
+ void setBoxConstraintsForLQOCProblem();
+
+ //! obtain state update from lqoc solver
+ void getStateUpdates();
+
+ //! obtain control update from lqoc solver
+ void getControlUpdates();
+
+ //! obtain feedback update from lqoc solver, if provided
+ void getFeedback();
+
+ //! reset all defects to zero
+ void resetDefects();
+
+ //! update the nominal defects
+ void computeDefectsNorm();
+
+ //! integrates the specified shots and computes the corresponding defects
+ virtual void rolloutShots(size_t firstIndex, size_t lastIndex) = 0;
+
+ //! do a single threaded rollout and defect computation of the shots - useful for line-search
+ bool rolloutShotsSingleThreaded(size_t threadId,
+ size_t firstIndex,
+ size_t lastIndex,
+ ControlVectorArray& u_ff_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot,
+ StateVectorArray& d,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU) const;
+
+ //! performLineSearch: execute the line search, possibly with different threading schemes
+ virtual SCALAR performLineSearch() = 0;
+
+ //! simple full-step update for state and feedforward control (used for MPC-mode!)
+ void doFullStepUpdate();
+
+ void logSummaryToMatlab(const std::string& fileName);
+
+ const SummaryAllIterations<SCALAR>& getSummary() const;
+
+protected:
+ //! integrate the individual shots
+ bool rolloutSingleShot(const size_t threadId,
+ const size_t k,
+ ControlVectorArray& u_ff_local,
+ StateVectorArray& x_local,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& xShot,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag = nullptr) const;
+
+ /*
+ bool simpleRollout(
+ const size_t threadId,
+ const ControlVectorArray& uff,
+ const StateVectorArray& x_ref_lqr,
+ StateVectorArray& x_local,
+ ControlVectorArray& u_recorded
+ )const;
+ */
+
+ //! computes the defect between shot and trajectory
+ /*!
+ * @param k index of the shot under consideration
+ * @param x_local the state trajectory
+ * @param xShot the shot trajectory
+ * @param d the defect trajectory
+ */
+ void computeSingleDefect(size_t k,
+ const StateVectorArray& x_local,
+ const StateVectorArray& xShot,
+ StateVectorArray& d) const;
+
+
+ //! Computes the linearized Dynamics at a specific point of the trajectory
+ /*!
+ This function calculates the linearization, i.e. matrices A and B in \f$ \dot{x} = A(x(k)) x + B(x(k)) u \f$
+ at a specific point of the trajectory
+
+ \param threadId the id of the worker thread
+ \param k step k
+ */
+ void computeLinearizedDynamics(size_t threadId, size_t k);
+
+
+ //! Computes the linearized general constraints at a specific point of the trajectory
+ /*!
+ This function calculates the linearization, i.e. matrices d, C and D in \f$ d_{lb} \leq C x + D u \leq d_{ub}\f$
+ at a specific point of the trajectory
+
+ \param threadId the id of the worker thread
+ \param k step k
+
+ \note the box constraints do not need to be linearized
+ */
+ void computeLinearizedConstraints(size_t threadId, size_t k);
+
+ //! Computes the quadratic costs
+ /*!
+ This function calculates the quadratic costs as provided by the costFunction pointer.
+
+ * \param threadId id of worker thread
+ * \param k step k
+ */
+ void computeQuadraticCosts(size_t threadId, size_t k);
+
+ //! Initializes cost to go
+ /*!
+ * This function initializes the cost-to-go function at time K.
+ *
+ */
+ void initializeCostToGo();
+
+ //! Computes cost to go
+ /*!
+ * This function computes the cost-to-go function for all times t<t_K
+ *
+ * \param k step k
+ */
+ void computeCostToGo(size_t k);
+
+ //! Design controller
+ /*!
+ * This function designes the LQR and feedforward controller at time k.
+ *
+ * \param k step k
+ */
+ void designController(size_t k);
+
+ //! Compute cost for a given set of state and input trajectory
+ /*!
+ * Compute cost for a given set of state and input trajectory
+ *
+ * \param threadId the ID of the thread
+ * \param x_local the state trajectory
+ * \param u_local the control trajectory
+ * \param intermediateCost the accumulated intermediate cost
+ * \param finalCost the accumulated final cost
+ */
+ void computeCostsOfTrajectory(size_t threadId,
+ const core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost) const;
+
+ /*!
+ * @brief Compute box constraint violations for a given set of state and input trajectory
+ *
+ * \param threadId the ID of the thread
+ * \param x_local the state trajectory
+ * \param u_local the control trajectory
+ * \param e_tot the total accumulated box constraint violation
+ */
+ void computeBoxConstraintErrorOfTrajectory(size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const;
+
+ /*!
+ * @brief Compute general constraint violations for a given set of state and input trajectory
+ *
+ * \param threadId the ID of the thread
+ * \param x_local the state trajectory
+ * \param u_local the control trajectory
+ * \param e_tot the total accumulated general constraint violation
+ */
+ void computeGeneralConstraintErrorOfTrajectory(size_t threadId,
+ const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
+ const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
+ scalar_t& e_tot) const;
+
+ //! Check if controller with particular alpha is better
+ void executeLineSearchSingleShooting(const size_t threadId,
+ const scalar_t alpha,
+ StateVectorArray& x_local,
+ ControlVectorArray& u_local,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag = nullptr) const;
+
+
+ void executeLineSearchMultipleShooting(const size_t threadId,
+ const scalar_t alpha,
+ const ControlVectorArray& u_ff_update,
+ const StateVectorArray& x_update,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_recorded,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_shot_recorded,
+ ct::core::StateVectorArray<STATE_DIM, SCALAR>& defects_recorded,
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_recorded,
+ scalar_t& intermediateCost,
+ scalar_t& finalCost,
+ scalar_t& defectNorm,
+ scalar_t& e_box_norm,
+ scalar_t& e_gen_norm,
+ StateSubsteps& substepsX,
+ ControlSubsteps& substepsU,
+ std::atomic_bool* terminationFlag = nullptr) const;
+
+
+ //! Update feedforward controller
+ /*!
+ * This function updates the feedforward Controller based on the previous calculation.
+ *
+ * \param k step k
+ */
+ void updateFFController(size_t k);
+
+
+ //! Send a std::vector of Eigen to Matlab
+ /*!
+ * This is a helper function to efficiently send std::vectors to Matlab.
+ */
+ // template <class V>
+ // void matrixToMatlab(V& matrix, std::string variableName);
+
+ //! compute norm of a discrete array (todo move to core)
+ template <typename ARRAY_TYPE, size_t ORDER = 1>
+ SCALAR computeDiscreteArrayNorm(const ARRAY_TYPE& d) const;
+
+ //! compute norm of difference between two discrete arrays (todo move to core)
+ template <typename ARRAY_TYPE, size_t ORDER = 1>
+ SCALAR computeDiscreteArrayNorm(const ARRAY_TYPE& a, const ARRAY_TYPE& b) const;
+
+ //! compute the norm of the defects trajectory
+ /*!
+ * Note that different kind of norms might be favorable for different cases.
+ * According to Nocedal and Wright, the l1-norm is "exact" (p.435), the l2-norm is smooth.
+ */
+ template <size_t ORDER = 1>
+ SCALAR computeDefectsNorm(const StateVectorArray& d) const;
+
+ typedef std::shared_ptr<ct::core::SubstepRecorder<STATE_DIM, CONTROL_DIM, SCALAR>> SubstepRecorderPtr;
+ std::vector<SubstepRecorderPtr, Eigen::aligned_allocator<SubstepRecorderPtr>> substepRecorders_;
+
+ typedef std::shared_ptr<ct::core::Integrator<STATE_DIM, SCALAR>> IntegratorPtr;
+ std::vector<IntegratorPtr, Eigen::aligned_allocator<IntegratorPtr>> integrators_; //! Runge-Kutta-4 Integrators
+
+ typedef std::shared_ptr<Sensitivity_t> SensitivityPtr;
+ std::vector<SensitivityPtr, Eigen::aligned_allocator<SensitivityPtr>>
+ sensitivity_; //! the ct sensitivity integrators
+
+ typedef std::shared_ptr<ct::core::IntegratorSymplecticEuler<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>
+ IntegratorSymplecticEulerPtr;
+ std::vector<IntegratorSymplecticEulerPtr, Eigen::aligned_allocator<IntegratorSymplecticEulerPtr>>
+ integratorsEulerSymplectic_;
+
+ typedef std::shared_ptr<ct::core::IntegratorSymplecticRk<P_DIM, V_DIM, CONTROL_DIM, SCALAR>>
+ IntegratorSymplecticRkPtr;
+ std::vector<IntegratorSymplecticRkPtr, Eigen::aligned_allocator<IntegratorSymplecticRkPtr>>
+ integratorsRkSymplectic_;
+
+ typedef std::shared_ptr<core::ConstantController<STATE_DIM, CONTROL_DIM, SCALAR>> ConstantControllerPtr;
+ std::vector<ConstantControllerPtr, Eigen::aligned_allocator<ConstantControllerPtr>>
+ controller_; //! the constant controller for forward-integration during one time-step
+
+
+ //! The policy. currently only for returning the result, should eventually replace L_ and u_ff_ (todo)
+ NLOCBackendBase::Policy_t policy_;
+
+ ct::core::tpl::TimeArray<SCALAR> t_; //! the time trajectory
+
+ bool initialized_;
+ bool configured_;
+
+ size_t iteration_; /*!< current iteration */
+
+ Settings_t settings_;
+
+ int K_; //! the number of stages in the overall OptConProblem
+
+ StateVectorArray lx_;
+ StateVectorArray x_;
+ StateVectorArray xShot_;
+ StateVectorArray x_prev_;
+
+ ControlVectorArray lu_;
+ ControlVectorArray u_ff_;
+ ControlVectorArray u_ff_prev_;
+
+ FeedbackArray L_;
+
+ SCALAR d_norm_; //! sum of the norms of all defects (internal constraint)
+ SCALAR e_box_norm_; //! sum of the norms of all box constraint violations
+ SCALAR e_gen_norm_; //! sum of the norms of all general constraint violations
+ SCALAR lx_norm_; //! sum of the norms of state update
+ SCALAR lu_norm_; //! sum of the norms of control update
+
+ //! shared pointer to the linear-quadratic optimal control problem
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>> lqocProblem_;
+
+ //! shared pointer to the linear-quadratic optimal control solver
+ std::shared_ptr<LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>> lqocSolver_;
+
+ StateSubstepsPtr substepsX_;
+ ControlSubstepsPtr substepsU_;
+
+ scalar_t intermediateCostBest_;
+ scalar_t finalCostBest_;
+ scalar_t lowestCost_;
+
+ //! costs of the previous iteration, required to determine convergence
+ scalar_t intermediateCostPrevious_;
+ scalar_t finalCostPrevious_;
+
+
+//! if building with MATLAB support, include matfile
+#ifdef MATLAB
+ matlab::MatFile matFile_;
+#endif //MATLAB
+
+
+ /*!
+ * of the following objects, we have nThreads+1 instantiations in form of a vector.
+ * Every instantiation is dedicated to a certain thread in the multi-thread implementation
+ */
+ std::vector<typename OptConProblem_t::DynamicsPtr_t> systems_;
+ std::vector<typename OptConProblem_t::LinearPtr_t> linearSystems_;
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t> costFunctions_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> boxConstraints_;
+ std::vector<typename OptConProblem_t::ConstraintPtr_t> generalConstraints_;
+
+
+ bool firstRollout_;
+ scalar_t alphaBest_;
+
+ //! a counter used to identify lqp problems in derived classes, i.e. for thread management in MP
+ size_t lqpCounter_;
+
+ SummaryAllIterations<SCALAR> summaryAllIterations_;
+};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+template <typename ARRAY_TYPE, size_t ORDER>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDiscreteArrayNorm(
+ const ARRAY_TYPE& d) const
+{
+ SCALAR norm = 0.0;
+
+ for (size_t k = 0; k < d.size(); k++)
+ {
+ norm += d[k].template lpNorm<ORDER>();
+ }
+ return norm;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+template <typename ARRAY_TYPE, size_t ORDER>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDiscreteArrayNorm(const ARRAY_TYPE& a,
+ const ARRAY_TYPE& b) const
+{
+ assert(a.size() == b.size());
+
+ SCALAR norm = 0.0;
+
+ for (size_t k = 0; k < a.size(); k++)
+ {
+ norm += (a[k] - b[k]).template lpNorm<ORDER>();
+ }
+ return norm;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+template <size_t ORDER>
+SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeDefectsNorm(
+ const StateVectorArray& d) const
+{
+ return computeDiscreteArrayNorm<StateVectorArray, ORDER>(d);
+}
+
+
+} // namespace optcon
+} // namespace ct
+
+
+#undef SYMPLECTIC_ENABLED
+#undef SYMPLECTIC_DISABLED
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp
new file mode 100644
index 0000000..d9f6c71
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp
@@ -0,0 +1,628 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendMP(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const NLOptConSettings& settings)
+ : Base(optConProblem, settings)
+{
+ startupRoutine();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendMP(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : Base(optConProblem, settingsFile, verbose, ns)
+{
+ startupRoutine();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOCBackendMP()
+{
+ shutdownRoutine();
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::startupRoutine()
+{
+ launchWorkerThreads();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::shutdownRoutine()
+{
+ workersActive_ = false;
+ workerTask_ = SHUTDOWN;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "Shutting down workers" << std::endl;
+#endif // DEBUG_PRINT_MP
+
+ for (size_t i = 0; i < workerThreads_.size(); i++)
+ {
+ workerThreads_[i].join();
+ }
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "All workers shut down" << std::endl;
+#endif // DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::threadWork(size_t threadId)
+{
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: launched");
+#endif // DEBUG_PRINT_MP
+
+
+ // local variables
+ int workerTask_local = IDLE;
+ size_t uniqueProcessID = 0;
+ size_t iteration_local = this->iteration_;
+ size_t lqpCounter_local = this->lqpCounter_;
+
+
+ while (workersActive_)
+ {
+ workerTask_local = workerTask_.load();
+ iteration_local = this->iteration_;
+ lqpCounter_local = this->lqpCounter_;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: previous procId: " + std::to_string(uniqueProcessID) +
+ ", current procId: " +
+ std::to_string(generateUniqueProcessID(iteration_local, (int)workerTask_local, lqpCounter_local)));
+#endif
+
+
+ /*!
+ * We want to put the worker to sleep if
+ * - the workerTask_ is IDLE
+ * - or we are finished but workerTask_ is not yet reset, thus the process ID is still the same
+ * */
+ if (workerTask_local == IDLE ||
+ uniqueProcessID == generateUniqueProcessID(iteration_local, (int)workerTask_local, lqpCounter_local))
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: going to sleep !");
+#endif
+
+ // sleep until the state is not IDLE any more and we have a different process ID than before
+ std::unique_lock<std::mutex> waitLock(workerWakeUpMutex_);
+ while (workerTask_ == IDLE || (uniqueProcessID == generateUniqueProcessID(this->iteration_,
+ (int)workerTask_.load(), this->lqpCounter_)))
+ {
+ workerWakeUpCondition_.wait(waitLock);
+ }
+ waitLock.unlock();
+
+ workerTask_local = workerTask_.load();
+ iteration_local = this->iteration_;
+ lqpCounter_local = this->lqpCounter_;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: woke up !");
+#endif // DEBUG_PRINT_MP
+ }
+
+ if (!workersActive_)
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("Breaking - workers are not active !");
+#endif // DEBUG_PRINT_MP
+ break;
+ }
+
+
+ switch (workerTask_local)
+ {
+ case LINE_SEARCH:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now busy with LINE_SEARCH !");
+#endif // DEBUG_PRINT_MP
+ lineSearchWorker(threadId);
+ uniqueProcessID = generateUniqueProcessID(iteration_local, LINE_SEARCH, lqpCounter_local);
+ break;
+ }
+ case ROLLOUT_SHOTS:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now doing shot rollouts !");
+#endif // DEBUG_PRINT_MP
+ rolloutShotWorker(threadId);
+ uniqueProcessID = generateUniqueProcessID(iteration_local, ROLLOUT_SHOTS, lqpCounter_local);
+ break;
+ }
+ case COMPUTE_LQ_PROBLEM:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now doing LQ approximation !");
+#endif // DEBUG_PRINT_MP
+ computeLQProblemWorker(threadId);
+ uniqueProcessID = generateUniqueProcessID(iteration_local, COMPUTE_LQ_PROBLEM, lqpCounter_local);
+ break;
+ }
+ case SHUTDOWN:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: now shutting down !");
+#endif // DEBUG_PRINT_MP
+ return;
+ break;
+ }
+ case IDLE:
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: is idle. Now going to sleep.");
+#endif // DEBUG_PRINT_MP
+ break;
+ }
+ default:
+ {
+ printString("[Thread " + std::to_string(threadId) + "]: WARNING: Worker has unknown task !");
+ break;
+ }
+ }
+ }
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: shut down.");
+#endif // DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::launchWorkerThreads()
+{
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Going to launch worker threads!");
+ std::cout << workersActive_.load() << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ workersActive_ = true;
+ workerTask_ = IDLE;
+
+ for (int i = 0; i < (int)this->settings_.nThreads; i++)
+ {
+ workerThreads_.push_back(std::thread(&NLOCBackendMP::threadWork, this, i));
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLQApproximation(size_t firstIndex,
+ size_t lastIndex)
+{
+ // fill terminal cost
+ if (lastIndex == (this->K_ - 1))
+ this->initializeCostToGo();
+
+ /*
+ * In special cases, this function may be called for a single index, e.g. for the unconstrained GNMS real-time iteration scheme.
+ * Then, don't wake up workers, but do single-threaded computation for that single index, and return.
+ */
+ if (lastIndex == firstIndex)
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: do single threaded LQ approximation for single index " + std::to_string(firstIndex) +
+ ". Not waking up workers.");
+#endif //DEBUG_PRINT_MP
+ this->computeLinearizedDynamics(this->settings_.nThreads, firstIndex);
+ this->computeQuadraticCosts(this->settings_.nThreads, firstIndex);
+ if (this->generalConstraints_[this->settings_.nThreads] != nullptr)
+ this->computeLinearizedConstraints(this->settings_.nThreads, firstIndex);
+ return;
+ }
+
+ /*
+ * In case of multiple points to perform LQ-approximation, start multi-threading:
+ */
+ Eigen::setNbThreads(1); // disable Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restricting Eigen to " + std::to_string(Eigen::nbThreads()) + " threads.");
+#endif //DEBUG_PRINT_MP
+
+ kTaken_ = 0;
+ kCompleted_ = 0;
+ KMax_ = lastIndex;
+ KMin_ = firstIndex;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Waking up workers to do LQ approximation.");
+#endif //DEBUG_PRINT_MP
+ workerTask_ = COMPUTE_LQ_PROBLEM;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Will sleep now until done with LQ approximation.");
+#endif //DEBUG_PRINT_MP
+
+ std::unique_lock<std::mutex> waitLock(kCompletedMutex_);
+ kCompletedCondition_.wait(waitLock, [this] { return kCompleted_.load() > KMax_ - KMin_; });
+ waitLock.unlock();
+ workerTask_ = IDLE;
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Woke up again, should have computed LQ approximation now.");
+#endif //DEBUG_PRINT_MP
+
+
+ Eigen::setNbThreads(this->settings_.nThreadsEigen); // restore Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restoring " + std::to_string(Eigen::nbThreads()) + " Eigen threads.");
+#endif //DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLQProblemWorker(size_t threadId)
+{
+ while (true)
+ {
+ const size_t k = kTaken_++;
+
+ if (k > KMax_ - KMin_)
+ {
+#ifdef DEBUG_PRINT_MP
+ if ((k + 1) % 100 == 0)
+ printString("k > KMax_ - KMin_ with k = " + std::to_string(k) + " and KMax_ is " +
+ std::to_string(KMax_) + " and KMin_ is " + std::to_string(KMin_));
+#endif
+
+ //kCompleted_++;
+ if (kCompleted_.load() > KMax_ - KMin_)
+ kCompletedCondition_.notify_all();
+ return;
+ }
+
+#ifdef DEBUG_PRINT_MP
+ if ((k + 1) % 100 == 0)
+ printString("[Thread " + std::to_string(threadId) + "]: Building LQ problem for index k " +
+ std::to_string(KMax_ - k));
+#endif
+
+ this->computeQuadraticCosts(threadId, KMax_ - k); // quadratize cost backwards
+
+ this->computeLinearizedDynamics(threadId, KMax_ - k); // linearize dynamics backwards
+
+ if (this->generalConstraints_[threadId] != nullptr)
+ this->computeLinearizedConstraints(threadId, KMax_ - k); // linearize constraints backwards
+
+ kCompleted_++;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShots(size_t firstIndex, size_t lastIndex)
+{
+ /*!
+ * In special cases, this function may be called for a single index, e.g. for the unconstrained GNMS real-time iteration scheme.
+ * Then, don't wake up workers, but do single-threaded computation for that single index, and return.
+ */
+ if (lastIndex == firstIndex)
+ {
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: do single threaded shot rollout for single index " + std::to_string(firstIndex) +
+ ". Not waking up workers.");
+#endif //DEBUG_PRINT_MP
+
+ this->rolloutSingleShot(this->settings_.nThreads, firstIndex, this->u_ff_, this->x_, this->x_, this->xShot_,
+ *this->substepsX_, *this->substepsU_);
+
+ this->computeSingleDefect(firstIndex, this->x_, this->xShot_, this->lqocProblem_->b_);
+ return;
+ }
+
+ /*!
+ * In case of multiple points to be linearized, start multi-threading:
+ */
+ Eigen::setNbThreads(1); // disable Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restricting Eigen to " + std::to_string(Eigen::nbThreads()) + " threads.");
+#endif //DEBUG_PRINT_MP
+
+
+ kTaken_ = 0;
+ kCompleted_ = 0;
+ KMax_ = lastIndex;
+ KMin_ = firstIndex;
+
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Waking up workers to do shot rollouts." << std::endl;
+#endif //DEBUG_PRINT_MP
+ workerTask_ = ROLLOUT_SHOTS;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Will sleep now until we have rolled out all shots." << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ std::unique_lock<std::mutex> waitLock(kCompletedMutex_);
+ kCompletedCondition_.wait(waitLock, [this] { return kCompleted_.load() > KMax_ - KMin_; });
+ waitLock.unlock();
+ workerTask_ = IDLE;
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Woke up again, should have rolled out all shots now." << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ Eigen::setNbThreads(this->settings_.nThreadsEigen); // restore Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restoring " + std::to_string(Eigen::nbThreads()) + " Eigen threads.");
+#endif //DEBUG_PRINT_MP
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShotWorker(size_t threadId)
+{
+ while (true)
+ {
+ size_t k = kTaken_++;
+
+ if (k > KMax_ - KMin_)
+ {
+ if (kCompleted_.load() > KMax_ - KMin_)
+ kCompletedCondition_.notify_all();
+ return;
+ }
+
+ size_t kShot = (KMax_ - k);
+ if (kShot % ((size_t)this->settings_.K_shot) == 0) //! only rollout when we're meeting the beginning of a shot
+ {
+#ifdef DEBUG_PRINT_MP
+ if ((k + 1) % 100 == 0)
+ printString("[Thread " + std::to_string(threadId) + "]: rolling out shot with index " +
+ std::to_string(KMax_ - k));
+#endif
+
+ this->rolloutSingleShot(
+ threadId, kShot, this->u_ff_, this->x_, this->x_, this->xShot_, *this->substepsX_, *this->substepsU_);
+
+ this->computeSingleDefect(kShot, this->x_, this->xShot_, this->lqocProblem_->b_);
+ }
+
+ kCompleted_++;
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::performLineSearch()
+{
+ Eigen::setNbThreads(1); // disable Eigen multi-threading
+
+ alphaProcessed_.clear();
+ alphaTaken_ = 0;
+ alphaBestFound_ = false;
+ alphaExpBest_ = this->settings_.lineSearchSettings.maxIterations;
+ alphaExpMax_ = this->settings_.lineSearchSettings.maxIterations;
+ alphaProcessed_.resize(this->settings_.lineSearchSettings.maxIterations, 0);
+ lowestCostPrevious_ = this->lowestCost_;
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Waking up workers." << std::endl;
+#endif //DEBUG_PRINT_MP
+ workerTask_ = LINE_SEARCH;
+ workerWakeUpCondition_.notify_all();
+
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Will sleep now until done line search." << std::endl;
+#endif //DEBUG_PRINT_MP
+ std::unique_lock<std::mutex> waitLock(alphaBestFoundMutex_);
+ alphaBestFoundCondition_.wait(waitLock, [this] { return alphaBestFound_.load(); });
+ waitLock.unlock();
+ workerTask_ = IDLE;
+#ifdef DEBUG_PRINT_MP
+ std::cout << "[MP]: Woke up again, should have results now." << std::endl;
+#endif //DEBUG_PRINT_MP
+
+ double alphaBest = 0.0;
+ if (alphaExpBest_ != alphaExpMax_)
+ {
+ alphaBest = this->settings_.lineSearchSettings.alpha_0 *
+ std::pow(this->settings_.lineSearchSettings.n_alpha, alphaExpBest_);
+ }
+
+ Eigen::setNbThreads(this->settings_.nThreadsEigen); // restore Eigen multi-threading
+#ifdef DEBUG_PRINT_MP
+ printString("[MP]: Restoring " + std::to_string(Eigen::nbThreads()) + " Eigen threads.");
+#endif //DEBUG_PRINT_MP
+
+ if (this->settings_.printSummary)
+ {
+ this->lu_norm_ = this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ this->u_ff_, this->u_ff_prev_);
+ this->lx_norm_ = this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ this->x_, this->x_prev_);
+ }
+ else
+ {
+#ifdef MATLAB
+ this->lu_norm_ = this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ this->u_ff_, this->u_ff_prev_);
+ this->lx_norm_ = this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ this->x_, this->x_prev_);
+#endif
+ }
+ this->x_prev_ = this->x_;
+ this->u_ff_prev_ = this->u_ff_;
+
+ return alphaBest;
+
+} // end linesearch
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::lineSearchWorker(size_t threadId)
+{
+ while (true)
+ {
+ size_t alphaExp = alphaTaken_++;
+
+#ifdef DEBUG_PRINT_MP
+ printString("[Thread " + std::to_string(threadId) + "]: Taking alpha index " + std::to_string(alphaExp));
+#endif
+
+ if (alphaExp >= alphaExpMax_ || alphaBestFound_)
+ {
+ return;
+ }
+
+ //! convert to real alpha
+ double alpha =
+ this->settings_.lineSearchSettings.alpha_0 * std::pow(this->settings_.lineSearchSettings.n_alpha, alphaExp);
+
+ //! local variables
+ SCALAR cost = std::numeric_limits<SCALAR>::max();
+ SCALAR intermediateCost = std::numeric_limits<SCALAR>::max();
+ SCALAR finalCost = std::numeric_limits<SCALAR>::max();
+ SCALAR defectNorm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_box_norm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_gen_norm = std::numeric_limits<SCALAR>::max();
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_shot_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> defects_recorded(
+ this->K_ + 1, ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u_recorded(this->K_);
+ typename Base::StateSubstepsPtr substepsX =
+ typename Base::StateSubstepsPtr(new typename Base::StateSubsteps(this->K_ + 1));
+ typename Base::ControlSubstepsPtr substepsU =
+ typename Base::ControlSubstepsPtr(new typename Base::ControlSubsteps(this->K_ + 1));
+
+ //! set init state
+ x_search[0] = this->x_prev_[0];
+
+
+ switch (this->settings_.nlocp_algorithm)
+ {
+ case NLOptConSettings::NLOCP_ALGORITHM::GNMS:
+ {
+ this->executeLineSearchMultipleShooting(threadId, alpha, this->lu_, this->lx_, x_search, x_shot_search,
+ defects_recorded, u_recorded, intermediateCost, finalCost, defectNorm, e_box_norm, e_gen_norm,
+ *substepsX, *substepsU, &alphaBestFound_);
+ break;
+ }
+ case NLOptConSettings::NLOCP_ALGORITHM::ILQR:
+ {
+ defectNorm = 0.0;
+ this->executeLineSearchSingleShooting(threadId, alpha, x_search, u_recorded, intermediateCost,
+ finalCost, e_box_norm, e_gen_norm, *substepsX, *substepsU, &alphaBestFound_);
+ break;
+ }
+ default:
+ throw std::runtime_error("Algorithm type unknown in performLineSearch()!");
+ }
+
+ // compute merit
+ cost = intermediateCost + finalCost + this->settings_.meritFunctionRho * defectNorm +
+ this->settings_.meritFunctionRhoConstraints * (e_box_norm + e_gen_norm);
+
+ lineSearchResultMutex_.lock();
+ if (cost < lowestCostPrevious_ && !std::isnan(cost))
+ {
+ // make sure we do not alter an existing result
+ if (alphaBestFound_)
+ {
+ lineSearchResultMutex_.unlock();
+ break;
+ }
+
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ printString("[LineSearch, Thread " + std::to_string(threadId) + "]: Lower cost/merit found at alpha:" +
+ std::to_string(alpha));
+ printString("[LineSearch]: Cost:\t" + std::to_string(intermediateCost + finalCost));
+ printString("[LineSearch]: Defect:\t" + std::to_string(defectNorm));
+ printString("[LineSearch]: err box constr:\t" + std::to_string(e_box_norm));
+ printString("[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm));
+ printString("[LineSearch]: Merit:\t" + std::to_string(cost));
+ }
+
+ alphaExpBest_ = alphaExp;
+ this->intermediateCostBest_ = intermediateCost;
+ this->finalCostBest_ = finalCost;
+ this->d_norm_ = defectNorm;
+ this->e_box_norm_ = e_box_norm;
+ this->e_gen_norm_ = e_gen_norm;
+ this->lowestCost_ = cost;
+ this->x_.swap(x_search);
+ this->xShot_.swap(x_shot_search);
+ this->u_ff_.swap(u_recorded);
+ this->lqocProblem_->b_.swap(defects_recorded);
+ this->substepsX_ = substepsX;
+ this->substepsU_ = substepsU;
+ }
+ else
+ {
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ printString("[LineSearch, Thread " + std::to_string(threadId) +
+ "]: NO lower cost/merit found at alpha:" + std::to_string(alpha));
+ printString("[LineSearch]: Cost:\t" + std::to_string(intermediateCost + finalCost));
+ printString("[LineSearch]: Defect:\t" + std::to_string(defectNorm));
+ printString("[LineSearch]: err box constr:\t" + std::to_string(e_box_norm));
+ printString("[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm));
+ printString("[LineSearch]: Merit:\t" + std::to_string(cost));
+ }
+ }
+
+ alphaProcessed_[alphaExp] = 1;
+
+ // we now check if all alphas prior to the best have been processed
+ // this also covers the case that there is no better alpha
+ bool allPreviousAlphasProcessed = true;
+ for (size_t i = 0; i < alphaExpBest_; i++)
+ {
+ if (alphaProcessed_[i] != 1)
+ {
+ allPreviousAlphasProcessed = false;
+ break;
+ }
+ }
+ if (allPreviousAlphasProcessed)
+ {
+ alphaBestFound_ = true;
+ alphaBestFoundCondition_.notify_all();
+ }
+
+ lineSearchResultMutex_.unlock();
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+size_t NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::generateUniqueProcessID(const size_t& iterateNo,
+ const int workerState,
+ const size_t resetCount)
+{
+ return (10e12 * (resetCount + 1) + 10e6 * (workerState + 1) + iterateNo + 1);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::printString(const std::string& text)
+{
+ std::cout << text << std::endl;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP.hpp
new file mode 100644
index 0000000..fc494ea
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP.hpp
@@ -0,0 +1,138 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+
+#include "NLOCBackendBase.hpp"
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * NLOC Backend for the multi-threaded case
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCBackendMP : public NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Base;
+
+ NLOCBackendMP(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem, const NLOptConSettings& settings);
+
+ NLOCBackendMP(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ //! destructor
+ virtual ~NLOCBackendMP();
+
+protected:
+ virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) override;
+
+ virtual void rolloutShots(size_t firstIndex, size_t lastIndex) override;
+
+ SCALAR performLineSearch() override;
+
+private:
+ enum WORKER_STATE
+ {
+ IDLE,
+ LINE_SEARCH,
+ ROLLOUT_SHOTS,
+ COMPUTE_LQ_PROBLEM,
+ SHUTDOWN
+ };
+
+ void startupRoutine();
+
+ void shutdownRoutine();
+
+ //! Launch all worker thread
+ /*!
+ Initializes and launches all worker threads
+ */
+ void launchWorkerThreads();
+
+ //! Main function of thread worker
+ /*!
+ Includes all tasks that a worker will execute
+ */
+ void threadWork(size_t threadId);
+
+ //! Line search for new controller using multi-threading
+ /*!
+ Line searches for the best controller in update direction. If line search is disabled, it just takes the suggested update step.
+ */
+ void lineSearchWorker(size_t threadId);
+
+ //! Creates the linear quadratic problem
+ /*!
+ This function calculates the quadratic costs as provided by the costFunction pointer as well as the linearized dynamics.
+
+ \param k step k
+ */
+ void computeLQProblemWorker(size_t threadId);
+
+ //! rolls out a shot and computes the defect
+ void rolloutShotWorker(size_t threadId);
+
+ /*! heuristic that generates a unique id for a process, such that we can manage the tasks.
+ * Generates a unique identifiers for task, iteration:
+ * @todo replace by proper hash
+ * */
+ size_t generateUniqueProcessID(const size_t& iterateNo, const int workerState, const size_t resetCount);
+
+ //! wrapper method for nice debug printing
+ void printString(const std::string& text);
+
+ std::vector<std::thread> workerThreads_;
+ std::atomic_bool workersActive_;
+ std::atomic_int workerTask_;
+
+ std::mutex workerWakeUpMutex_;
+ std::condition_variable workerWakeUpCondition_;
+
+ std::mutex kCompletedMutex_;
+ std::condition_variable kCompletedCondition_;
+
+ std::mutex kCompletedMutexCost_;
+ std::condition_variable kCompletedConditionCost_;
+
+ std::mutex lineSearchResultMutex_;
+ std::mutex alphaBestFoundMutex_;
+ std::condition_variable alphaBestFoundCondition_;
+
+ std::atomic_size_t alphaTaken_;
+ size_t alphaMax_;
+ size_t alphaExpBest_;
+ size_t alphaExpMax_;
+ std::atomic_bool alphaBestFound_;
+ std::vector<size_t> alphaProcessed_;
+
+ std::atomic_size_t kTaken_;
+ std::atomic_size_t kCompleted_;
+
+ size_t KMax_;
+ size_t KMin_;
+
+ SCALAR lowestCostPrevious_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp
new file mode 100644
index 0000000..bc0eaab
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp
@@ -0,0 +1,212 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendST(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const NLOptConSettings& settings)
+ : Base(optConProblem, settings)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOCBackendST(
+ const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : Base(optConProblem, settingsFile, verbose, ns)
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOCBackendST(){};
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLQApproximation(size_t firstIndex,
+ size_t lastIndex)
+{
+ if (lastIndex == this->K_ - 1)
+ this->initializeCostToGo();
+
+ for (size_t k = firstIndex; k <= lastIndex; k++)
+ {
+ this->computeLinearizedDynamics(this->settings_.nThreads, k);
+
+ this->computeQuadraticCosts(this->settings_.nThreads, k);
+
+ if (this->generalConstraints_[this->settings_.nThreads] != nullptr)
+ this->computeLinearizedConstraints(this->settings_.nThreads, k);
+ }
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::rolloutShots(size_t firstIndex, size_t lastIndex)
+{
+ for (size_t k = firstIndex; k <= lastIndex; k = k + this->settings_.K_shot)
+ {
+ // rollout the shot
+ this->rolloutSingleShot(this->settings_.nThreads, k, this->u_ff_, this->x_, this->x_, this->xShot_,
+ *this->substepsX_, *this->substepsU_);
+
+ this->computeSingleDefect(k, this->x_, this->xShot_, this->lqocProblem_->b_);
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::performLineSearch()
+{
+ // we start with extrapolation
+ double alpha = this->settings_.lineSearchSettings.alpha_0;
+ double alphaBest = 0.0;
+ size_t iterations = 0;
+
+ this->lx_norm_ = 0.0;
+ this->lu_norm_ = 0.0;
+
+
+ while (iterations < this->settings_.lineSearchSettings.maxIterations)
+ {
+ if (this->settings_.lineSearchSettings.debugPrint)
+ std::cout << "[LineSearch]: Iteration: " << iterations << ", try alpha: " << alpha << " out of maximum "
+ << this->settings_.lineSearchSettings.maxIterations << " iterations. " << std::endl;
+
+ iterations++;
+
+ SCALAR cost = std::numeric_limits<SCALAR>::max();
+ SCALAR intermediateCost = std::numeric_limits<SCALAR>::max();
+ SCALAR finalCost = std::numeric_limits<SCALAR>::max();
+ SCALAR defectNorm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_box_norm = std::numeric_limits<SCALAR>::max();
+ SCALAR e_gen_norm = std::numeric_limits<SCALAR>::max();
+
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_shot_search(this->K_ + 1);
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> defects_recorded(
+ this->K_ + 1, ct::core::StateVector<STATE_DIM, SCALAR>::Zero());
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u_recorded(this->K_);
+
+ typename Base::StateSubstepsPtr substepsX =
+ typename Base::StateSubstepsPtr(new typename Base::StateSubsteps(this->K_ + 1));
+ typename Base::ControlSubstepsPtr substepsU =
+ typename Base::ControlSubstepsPtr(new typename Base::ControlSubsteps(this->K_ + 1));
+
+ // set init state
+ x_search[0] = this->x_[0];
+
+
+ switch (this->settings_.nlocp_algorithm)
+ {
+ case NLOptConSettings::NLOCP_ALGORITHM::GNMS:
+ {
+ this->executeLineSearchMultipleShooting(this->settings_.nThreads, alpha, this->lu_, this->lx_, x_search,
+ x_shot_search, defects_recorded, u_recorded, intermediateCost, finalCost, defectNorm, e_box_norm,
+ e_gen_norm, *substepsX, *substepsU);
+
+ break;
+ }
+ case NLOptConSettings::NLOCP_ALGORITHM::ILQR:
+ {
+ defectNorm = 0.0;
+ this->executeLineSearchSingleShooting(this->settings_.nThreads, alpha, x_search, u_recorded,
+ intermediateCost, finalCost, e_box_norm, e_gen_norm, *substepsX, *substepsU);
+ break;
+ }
+ default:
+ throw std::runtime_error("Algorithm type unknown in performLineSearch()!");
+ }
+
+ // compute merit
+ cost = intermediateCost + finalCost + this->settings_.meritFunctionRho * defectNorm +
+ this->settings_.meritFunctionRhoConstraints * (e_box_norm + e_gen_norm);
+
+ // catch the case that a rollout might be unstable
+ if (std::isnan(cost) ||
+ cost >= this->lowestCost_) // TODO: alternatively cost >= (this->lowestCost_*(1 - 1e-3*alpha)), study this
+ {
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "[LineSearch]: No better cost/merit found at alpha " << alpha << ":" << std::endl;
+ std::cout << "[LineSearch]: Cost:\t" << intermediateCost + finalCost << std::endl;
+ std::cout << "[LineSearch]: Defect:\t" << defectNorm << std::endl;
+ std::cout << "[LineSearch]: err box constr:\t" + std::to_string(e_box_norm) << std::endl;
+ std::cout << "[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm) << std::endl;
+ std::cout << "[LineSearch]: Merit:\t" << cost << std::endl;
+ }
+
+ // compute new alpha
+ alpha = alpha * this->settings_.lineSearchSettings.n_alpha;
+ }
+ else
+ {
+ // cost < this->lowestCost_ , better merit/cost found!
+
+ if (this->settings_.lineSearchSettings.debugPrint)
+ {
+ std::cout << "Lower cost/merit found at alpha: " << alpha << ":" << std::endl;
+ std::cout << "[LineSearch]: Cost:\t" << intermediateCost + finalCost << std::endl;
+ std::cout << "[LineSearch]: Defect:\t" << defectNorm << std::endl;
+ std::cout << "[LineSearch]: err box constr:\t" + std::to_string(e_box_norm) << std::endl;
+ std::cout << "[LineSearch]: err gen constr:\t" + std::to_string(e_gen_norm) << std::endl;
+ std::cout << "[LineSearch]: Merit:\t" << cost << std::endl;
+ }
+
+
+ if (this->settings_.printSummary)
+ {
+ this->lu_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ u_recorded, this->u_ff_prev_);
+ this->lx_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ x_search, this->x_prev_);
+ }
+ else
+ {
+#ifdef MATLAB
+ this->lu_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>, 2>(
+ u_recorded, this->u_ff_prev_);
+ this->lx_norm_ =
+ this->template computeDiscreteArrayNorm<ct::core::StateVectorArray<STATE_DIM, SCALAR>, 2>(
+ x_search, this->x_prev_);
+#endif
+ }
+
+ alphaBest = alpha;
+ this->intermediateCostBest_ = intermediateCost;
+ this->finalCostBest_ = finalCost;
+ this->d_norm_ = defectNorm;
+ this->e_box_norm_ = e_box_norm;
+ this->e_gen_norm_ = e_gen_norm;
+ this->x_prev_ = x_search;
+ this->lowestCost_ = cost;
+ this->x_.swap(x_search);
+ this->xShot_.swap(x_shot_search);
+ this->u_ff_.swap(u_recorded);
+ this->lqocProblem_->b_.swap(defects_recorded);
+ this->substepsX_ = substepsX;
+ this->substepsU_ = substepsU;
+ break;
+ }
+ } // end while
+
+ return alphaBest;
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCBackendST.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST.hpp
new file mode 100644
index 0000000..d24285e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCBackendST.hpp
@@ -0,0 +1,47 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include "NLOCBackendBase.hpp"
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * NLOC Backend for Single-Threaded case
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class NLOCBackendST : public NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Base;
+
+ NLOCBackendST(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem, const NLOptConSettings& settings);
+
+ NLOCBackendST(const OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ virtual ~NLOCBackendST();
+
+protected:
+ virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) override;
+
+ virtual void rolloutShots(size_t firstIndex, size_t lastIndex) override;
+
+ SCALAR performLineSearch() override;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/NLOCResults.hpp b/ct_optcon/include/ct/optcon/nloc/NLOCResults.hpp
new file mode 100644
index 0000000..9ef9e26
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/NLOCResults.hpp
@@ -0,0 +1,96 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef MATLAB
+#include <ct/optcon/matlab.hpp>
+#endif
+
+/*!
+ * to do log every xth iter
+ *
+ * \todo also log settings here
+ */
+template <typename SCALAR = double>
+struct SummaryAllIterations
+{
+ //! log of the iterations
+ std::vector<size_t> iterations;
+
+ //! different overall defect norms
+ std::vector<SCALAR> defect_l1_norms;
+ std::vector<SCALAR> defect_l2_norms;
+ std::vector<SCALAR> e_box_norms;
+ std::vector<SCALAR> e_gen_norms;
+
+ //! step update norms
+ std::vector<SCALAR> lx_norms;
+ std::vector<SCALAR> lu_norms;
+
+ //! costs and merits
+ std::vector<SCALAR> intermediateCosts;
+ std::vector<SCALAR> finalCosts;
+ std::vector<SCALAR> totalCosts;
+ std::vector<SCALAR> merits;
+
+ //! step size
+ std::vector<SCALAR> stepSizes;
+
+ //! smallest eigenvalues
+ std::vector<SCALAR> smallestEigenvalues;
+
+ //! print summary of the last iteration with desired numeric precision
+ template <int NUM_PRECISION = 12>
+ void printSummaryLastIteration()
+ {
+ std::cout << "NLOC Summary of iteration " << iterations.back() << std::endl;
+ std::cout << "==============================" << std::endl;
+
+ std::cout << std::setprecision(NUM_PRECISION) << "interm. cost:\t" << intermediateCosts.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "final cost:\t" << finalCosts.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total cost:\t" << totalCosts.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total merit:\t" << merits.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "tot. defect L1:\t" << defect_l1_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "tot. defect L2:\t" << defect_l2_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "box cstr.err:\t" << e_box_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "gen cstr.err:\t" << e_gen_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total lx norm:\t" << lx_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "total lu norm:\t" << lu_norms.back() << std::endl;
+ std::cout << std::setprecision(NUM_PRECISION) << "step-size:\t" << stepSizes.back() << std::endl;
+ std::cout << " ===========" << std::endl;
+ std::cout << std::endl;
+ }
+
+
+ void logToMatlab(const std::string& fileName)
+ {
+#ifdef MATLAB
+ std::cout << "Logging NLOC summary to Matlab" << std::endl;
+ matFile_.open(fileName + ".mat");
+
+ matFile_.put("iterations", iterations);
+ matFile_.put("defect_l1_norms", defect_l1_norms);
+ matFile_.put("defect_l2_norms", defect_l2_norms);
+ matFile_.put("box_constr_norms", e_box_norms);
+ matFile_.put("gen_constr_norms", e_gen_norms);
+ matFile_.put("lx_norms", lx_norms);
+ matFile_.put("lu_norms", lu_norms);
+ matFile_.put("intermediateCosts", intermediateCosts);
+ matFile_.put("finalCosts", finalCosts);
+ matFile_.put("totalCosts", totalCosts);
+ matFile_.put("merits", merits);
+ matFile_.put("stepSizes", stepSizes);
+ matFile_.put("smallestEigenvalues", smallestEigenvalues);
+ matFile_.close();
+#endif
+ }
+
+//! if building with MATLAB support, include matfile
+#ifdef MATLAB
+ matlab::MatFile matFile_;
+#endif //MATLAB
+};
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp
new file mode 100644
index 0000000..88994cf
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp
@@ -0,0 +1,302 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::GNMS(std::shared_ptr<Backend_t>& backend_,
+ const Settings_t& settings)
+ : BASE(backend_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~GNMS()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ this->backend_->configure(settings);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ this->backend_->setInitialGuess(initialGuess);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::runIteration()
+{
+ prepareIteration();
+
+ return finishIteration();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareIteration()
+{
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startPrepare = std::chrono::steady_clock::now();
+
+ if (!this->backend_->isInitialized())
+ throw std::runtime_error("GNMS is not initialized!");
+
+ if (!this->backend_->isConfigured())
+ throw std::runtime_error("GNMS is not configured!");
+
+ this->backend_->checkProblem();
+
+ int K = this->backend_->getNumSteps();
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ // if first iteration, compute shots and rollout and cost!
+ if (this->backend_->iteration() == 0)
+ {
+ this->backend_->rolloutShots(K_shot, K - 1);
+ }
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->setBoxConstraintsForLQOCProblem();
+ this->backend_->computeLQApproximation(K_shot, K - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: computing LQ Approximation from index " << K_shot << " to N-1 took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS]: Solving prepare stage of LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->prepareSolveLQProblem(K_shot);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: Prepare phase of LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ auto endPrepare = std::chrono::steady_clock::now();
+ if (debugPrint)
+ std::cout << "[GNMS]: prepareIteration() took "
+ << std::chrono::duration<double, std::milli>(endPrepare - startPrepare).count() << " ms" << std::endl;
+
+} //! prepareIteration()
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishIteration()
+{
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startFinish = std::chrono::steady_clock::now();
+
+ // if first iteration, compute shots and rollout and cost!
+ if (this->backend_->iteration() == 0)
+ {
+ this->backend_->rolloutShots(0, K_shot - 1);
+ this->backend_->updateCosts();
+ this->backend_->computeDefectsNorm();
+ }
+
+#ifdef MATLAB_FULL_LOG
+ if (this->backend_->iteration() == 0)
+ this->backend_->logInitToMatlab();
+#endif
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->computeLQApproximation(0, K_shot - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: computing LQ approximation for first multiple-shooting interval took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS]: Finish phase LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->finishSolveLQProblem(K_shot - 1);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: Finish solving LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+
+ //! update solutions
+ this->backend_->getFeedback();
+ this->backend_->getControlUpdates();
+ this->backend_->getStateUpdates();
+
+
+ start = std::chrono::steady_clock::now();
+ bool foundBetter = this->backend_->lineSearchMultipleShooting();
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS]: Line search took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+
+ if (debugPrint)
+ {
+ auto endFinish = std::chrono::steady_clock::now();
+ std::cout << "[GNMS]: finishIteration() took "
+ << std::chrono::duration<double, std::milli>(endFinish - startFinish).count() << " ms" << std::endl;
+ }
+
+
+ this->backend_->printSummary();
+
+#ifdef MATLAB_FULL_LOG
+ this->backend_->logToMatlab(this->backend_->iteration());
+#endif //MATLAB_FULL_LOG
+
+ this->backend_->iteration()++;
+
+ return foundBetter;
+
+} //! finishIteration()
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareMPCIteration()
+{
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startPrepare = std::chrono::steady_clock::now();
+
+ if (!this->backend_->isInitialized())
+ throw std::runtime_error("GNMS is not initialized!");
+
+ if (!this->backend_->isConfigured())
+ throw std::runtime_error("GNMS is not configured!");
+
+ this->backend_->checkProblem();
+
+ int K = this->backend_->getNumSteps();
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ this->backend_->resetDefects();
+
+ this->backend_->rolloutShots(K_shot, K - 1);
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->setBoxConstraintsForLQOCProblem();
+ this->backend_->computeLQApproximation(K_shot, K - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: computing LQ approximation from index " << K_shot << " to N-1 took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Solving prepare stage of LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->prepareSolveLQProblem(K_shot);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Prepare phase of LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+
+ auto endPrepare = std::chrono::steady_clock::now();
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: prepareIteration() took "
+ << std::chrono::duration<double, std::milli>(endPrepare - startPrepare).count() << " ms" << std::endl;
+
+
+} //! prepareMPCIteration()
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishMPCIteration()
+{
+ int K_shot = this->backend_->getNumStepsPerShot();
+
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ auto startFinish = std::chrono::steady_clock::now();
+
+
+ this->backend_->rolloutShots(0, K_shot - 1);
+ this->backend_->updateCosts(); //! todo: replace by a simple sum after computeQuadraticCostsAround....
+ this->backend_->computeDefectsNorm(); //! todo: we might not need this in MPC
+
+
+#ifdef MATLAB_FULL_LOG
+ if (this->backend_->iteration() == 0)
+ this->backend_->logInitToMatlab();
+#endif
+
+ auto start = std::chrono::steady_clock::now();
+ this->backend_->computeLQApproximation(0, K_shot - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: computing LQ approximation for first multiple-shooting interval took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Finish phase LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->finishSolveLQProblem(K_shot - 1);
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Finish solving LQOC problem took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+
+ //! update solutions
+ start = std::chrono::steady_clock::now();
+ this->backend_->getFeedback();
+ this->backend_->getControlUpdates();
+ this->backend_->getStateUpdates();
+
+ //!update state and controls, no line-search, overwriting happens only at next rollout
+ this->backend_->doFullStepUpdate();
+
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[GNMS-MPC]: Solution update took " << std::chrono::duration<double, std::milli>(diff).count()
+ << " ms" << std::endl;
+
+
+ if (debugPrint)
+ {
+ auto endFinish = std::chrono::steady_clock::now();
+ std::cout << "[GNMS-MPC]: finishIteration() took "
+ << std::chrono::duration<double, std::milli>(endFinish - startFinish).count() << " ms" << std::endl;
+ }
+
+ this->backend_->printSummary();
+
+#ifdef MATLAB_FULL_LOG
+ this->backend_->logToMatlab(this->backend_->iteration());
+#endif //MATLAB_FULL_LOG
+
+ this->backend_->iteration()++;
+
+ return true; // note: will always return foundBetter
+
+} //! finishIteration()
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS.hpp
new file mode 100644
index 0000000..4c92f84
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS.hpp
@@ -0,0 +1,80 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+#include <ct/optcon/nloc/NLOCAlgorithm.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class GNMS : public NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ typedef ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+
+ typedef NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> BASE;
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Backend_t;
+
+ //! constructor
+ GNMS(std::shared_ptr<Backend_t>& backend_, const Settings_t& settings);
+
+ //! destructor
+ virtual ~GNMS();
+
+ //! configure the solver
+ virtual void configure(const Settings_t& settings) override;
+
+ //! set an initial guess
+ virtual void setInitialGuess(const Policy_t& initialGuess) override;
+
+ //! runIteration combines prepareIteration and finishIteration
+ /*!
+ * @return foundBetter (false if converged)
+ */
+ virtual bool runIteration() override;
+
+
+ /*!
+ * - linearize dynamics for the stages 1 to N-1
+ * - quadratize cost for stages 1 to N-1
+ */
+ virtual void prepareIteration() override;
+
+
+ //! finish iteration for unconstrained GNMS
+ /*!
+ * - linearize dynamcs for the first stage
+ * - quadratize cost for the first stage
+ * @return
+ */
+ virtual bool finishIteration() override;
+
+
+ //! prepare iteration, dedicated to MPC.
+ /*!
+ * requirements: no line-search, end with update-step of controls and state, no rollout after update steps.
+ * Therefore: rollout->linearize->solve
+ */
+ virtual void prepareMPCIteration() override;
+
+
+ //! finish iteration, dedicated to MPC
+ virtual bool finishMPCIteration() override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/matlab/gnmsPlot.m b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/matlab/gnmsPlot.m
new file mode 100644
index 0000000..3f0c060
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/gnms/matlab/gnmsPlot.m
@@ -0,0 +1,49 @@
+clear all
+close all
+load ../../../../../../../ct/GNMSLog0.mat
+
+%reformat
+t = squeeze(t);
+lv = squeeze(lv);
+
+figure()
+subplot(2,2,1)
+plot(t, x(1,:), 'kx', 'MarkerSize',1)
+hold on;
+plot(t, xShot(1,:), 'bx', 'MarkerSize',1)
+legend('x*', 'x init');
+xlabel('t [sec]')
+ylabel('x [m]')
+title('positions');
+
+subplot(2,2,2)
+plot(t, x(2,:), 'k')
+hold on;
+plot(t, xShot(2,:), 'b')
+legend('v*', 'v init');
+xlabel('t [sec]')
+ylabel('v [m/sec]')
+title('velocities');
+
+
+subplot(2,2,3)
+plot(t(1:end-1), lv)
+title('control update')
+xlabel('t [sec]')
+ylabel('F [N]')
+
+subplot(2,2,4)
+plot(t, lx(1,:)); hold on
+plot(t, lx(2,:)); hold on
+title('state update')
+xlabel('t [sec]')
+ylabel('dx [m]')
+legend('x update', 'v update')
+
+
+%%
+figure();
+plot(t(1:end-1), d(1,:)); hold on;
+%plot(t(1:end-1), d(2,:))
+title('defect')
+
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp
new file mode 100644
index 0000000..064060c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp
@@ -0,0 +1,156 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+#include <ct/optcon/nloc/NLOCAlgorithm.hpp>
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::iLQR(std::shared_ptr<Backend_t>& backend_,
+ const Settings_t& settings)
+ : BASE(backend_)
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~iLQR()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ this->backend_->configure(settings);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ this->backend_->setInitialGuess(initialGuess);
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::runIteration()
+{
+ prepareIteration();
+
+ return finishIteration();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareIteration()
+{
+ if (!this->backend_->isInitialized())
+ throw std::runtime_error("iLQR is not initialized!");
+
+ if (!this->backend_->isConfigured())
+ throw std::runtime_error("iLQR is not configured!");
+
+ this->backend_->checkProblem();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishIteration()
+{
+ int K = this->backend_->getNumSteps();
+
+ bool debugPrint = this->backend_->getSettings().debugPrint;
+
+ // if first iteration, compute shots and rollout and cost!
+ if (this->backend_->iteration() == 0)
+ {
+ if (!this->backend_->nominalRollout())
+ throw std::runtime_error("Rollout failed. System became unstable");
+
+ this->backend_->updateCosts();
+ }
+
+#ifdef MATLAB_FULL_LOG
+ if (this->backend_->iteration() == 0)
+ this->backend_->logInitToMatlab();
+#endif
+
+ auto start = std::chrono::steady_clock::now();
+ auto startEntire = start;
+
+ // set box constraints and do LQ approximation
+ this->backend_->setBoxConstraintsForLQOCProblem();
+ this->backend_->computeLQApproximation(0, K - 1);
+ auto end = std::chrono::steady_clock::now();
+ auto diff = end - start;
+ if (debugPrint)
+ std::cout << "[iLQR]: Computing LQ approximation took "
+ << std::chrono::duration<double, std::milli>(diff).count() << " ms" << std::endl;
+
+ end = std::chrono::steady_clock::now();
+ diff = end - startEntire;
+ if (debugPrint)
+ std::cout << "[iLQR]: Forward pass took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+ if (debugPrint)
+ std::cout << "[iLQR]: #2 Solve LQOC Problem" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ this->backend_->solveFullLQProblem();
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[iLQR]: Solving LQOC problem took " << std::chrono::duration<double, std::milli>(diff).count()
+ << " ms" << std::endl;
+
+ // update solutions
+ this->backend_->getFeedback();
+ this->backend_->getControlUpdates();
+ this->backend_->getStateUpdates();
+
+ // line-search
+ if (debugPrint)
+ std::cout << "[iLQR]: #3 LineSearch" << std::endl;
+
+ start = std::chrono::steady_clock::now();
+ bool foundBetter = this->backend_->lineSearchSingleShooting();
+ end = std::chrono::steady_clock::now();
+ diff = end - start;
+ if (debugPrint)
+ std::cout << "[iLQR]: Line search took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+ diff = end - startEntire;
+ if (debugPrint)
+ std::cout << "[iLQR]: finishIteration took " << std::chrono::duration<double, std::milli>(diff).count() << " ms"
+ << std::endl;
+
+ this->backend_->printSummary();
+
+#ifdef MATLAB_FULL_LOG
+ this->backend_->logToMatlab(this->backend_->iteration());
+#endif
+
+ this->backend_->iteration()++;
+
+ return foundBetter;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareMPCIteration()
+{
+ prepareIteration();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishMPCIteration()
+{
+ finishIteration();
+ return true; //! \todo : in MPC always returning true. Unclear how user wants to deal with varying costs, etc.
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR.hpp b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR.hpp
new file mode 100644
index 0000000..7aafca5
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR.hpp
@@ -0,0 +1,80 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+#include <ct/optcon/nloc/NLOCAlgorithm.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR = double>
+class iLQR : public NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ typedef ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+
+ typedef NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> BASE;
+ typedef NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Backend_t;
+
+ //! constructor
+ iLQR(std::shared_ptr<Backend_t>& backend_, const Settings_t& settings);
+
+ //! destructor
+ virtual ~iLQR();
+
+ //! configure the solver
+ virtual void configure(const Settings_t& settings) override;
+
+ //! set an initial guess
+ virtual void setInitialGuess(const Policy_t& initialGuess) override;
+
+
+ //! runIteration combines prepareIteration and finishIteration
+ /*!
+ * For iLQR the separation between prepareIteration and finishIteration would actually not be necessary
+ * @return
+ */
+ virtual bool runIteration() override;
+
+
+ /*!
+ * for iLQR, as it is a purely sequential approach, we cannot prepare anything prior to solving,
+ */
+ virtual void prepareIteration() override;
+
+
+ /*!
+ * for iLQR, finishIteration contains the whole main iLQR iteration.
+ * @return
+ */
+ virtual bool finishIteration() override;
+
+
+ /*!
+ * for iLQR, as it is a purely sequential approach, we cannot prepare anything prior to solving,
+ */
+ virtual void prepareMPCIteration() override;
+
+
+ /*!
+ * for iLQR, finishIteration contains the whole main iLQR iteration.
+ * @return
+ */
+ virtual bool finishMPCIteration() override;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintBase.h b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintBase.h
new file mode 100644
index 0000000..3591dab
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintBase.h
@@ -0,0 +1,200 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup DMS
+ *
+ * @brief Implements an abstract base class from which all the discrete
+ * custom NLP constraints should derive
+ */
+template <typename SCALAR>
+class DiscreteConstraintBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+
+ /**
+ * @brief Default constructor
+ */
+ DiscreteConstraintBase() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~DiscreteConstraintBase() {}
+ /**
+ * @brief Evaluates the constraint violation
+ *
+ * @return A vector of the evaluated constraint violation
+ */
+ virtual VectorXs eval() = 0;
+
+ /**
+ * @brief Returns the non zero elements of the eval method with respect
+ * to the optimization variables
+ *
+ * @return A vector of the non zero elements of the constraint jacobian
+ */
+ virtual VectorXs evalSparseJacobian() = 0;
+
+ /**
+ * @brief Returns size of the constraint vector
+ *
+ * @return The size of the constraint vector (should be equal to the
+ * size of the return value of the eval method)
+ */
+ virtual size_t getConstraintSize() = 0;
+
+ /**
+ * @brief Returns the number of non zero elements of the jacobian
+ *
+ * @return The number of non zero elements of the jacobian (which should
+ * be equal to the return value of evalSparseJacobian)
+ */
+ virtual size_t getNumNonZerosJacobian() = 0;
+
+ /**
+ * @brief Returns the sparsity structure of the constraint jacobian
+ *
+ * @param[out] iRow_vec A vector containing the row indices of the non zero
+ * elements of the constraint jacobian
+ * @param[out] jCol_vec A vector containing the column indices of the non
+ * zero elements of the constraint jacobian
+ */
+ virtual void genSparsityPattern(Eigen::VectorXi& iRow_vec, Eigen::VectorXi& jCol_vec) = 0;
+
+ /**
+ * @brief Returns the lower bound of the constraint
+ *
+ * @return The lower constraint bound
+ */
+ virtual VectorXs getLowerBound() = 0;
+
+ /**
+ * @brief Returns the upper bound of the constraint
+ *
+ * @return The upper constraint bound
+ */
+ virtual VectorXs getUpperBound() = 0;
+
+protected:
+ /**
+ * @brief This method generates Row and Column vectors which indicate
+ * the sparsity pattern of the constraint jacobian for a
+ * quadratic matrix block containing diagonal entries only
+ *
+ * @param[in] col_start The starting column of the jCol vec
+ * @param[in] num_elements The size of the matrix block
+ * @param[out] iRow_vec The resulting row vector
+ * @param[out] jCol_vec The resuling column vector
+ * @param[in] indexNumber The starting inserting index for iRow and jCol
+ *
+ * @return indexnumber plus num_elements
+ */
+ size_t genDiagonalIndices(const size_t col_start,
+ const size_t num_elements,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber);
+
+ /**
+ * @brief This method generates Row and Column vectors which indicate
+ * the sparsity pattern of the constraint jacobian for an
+ * arbitrary dense matrix block
+ *
+ * @param[in] col_start The starting column of the jCol vec
+ * @param[in] num_rows The number of rows of the matrix block
+ * @param[in] num_cols The number of columns of the matrix block
+ * @param[out] iRow_vec The resulting row vector
+ * @param[out] jCol_vec The resuling column vector
+ * @param[in] indexNumber The starting inserting index for iRow and jCol
+ *
+ * @return The indexnumber plus the number of elements contained in the
+ * matrix block
+ */
+ size_t genBlockIndices(const size_t col_start,
+ const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber);
+};
+
+template <typename SCALAR>
+inline size_t DiscreteConstraintBase<SCALAR>::genDiagonalIndices(const size_t col_start,
+ const size_t num_elements,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber)
+{
+ Eigen::VectorXi new_row_indices;
+ Eigen::VectorXi new_col_indices;
+ new_row_indices.resize(num_elements);
+ new_col_indices.resize(num_elements);
+
+ size_t count = 0;
+
+ for (size_t i = 0; i < num_elements; ++i)
+ {
+ new_row_indices(count) = i;
+ new_col_indices(count) = col_start + i;
+ count++;
+ }
+
+ assert(count == num_elements);
+
+ iRow_vec.segment(indexNumber, num_elements) = new_row_indices;
+ jCol_vec.segment(indexNumber, num_elements) = new_col_indices;
+
+ return count;
+}
+
+template <typename SCALAR>
+inline size_t DiscreteConstraintBase<SCALAR>::genBlockIndices(const size_t col_start,
+ const size_t num_rows,
+ const size_t num_cols,
+ Eigen::VectorXi& iRow_vec,
+ Eigen::VectorXi& jCol_vec,
+ const size_t indexNumber)
+{
+ size_t num_gen_indices = num_rows * num_cols;
+
+ Eigen::VectorXi new_row_indices;
+ Eigen::VectorXi new_col_indices;
+ new_row_indices.resize(num_gen_indices);
+ new_col_indices.resize(num_gen_indices);
+
+ size_t count = 0;
+
+ for (size_t row = 0; row < num_rows; ++row)
+ {
+ for (size_t col = col_start; col < col_start + num_cols; ++col)
+ {
+ new_row_indices(count) = row;
+ new_col_indices(count) = col;
+ count++;
+ }
+ }
+
+ assert(count == num_gen_indices);
+
+ iRow_vec.segment(indexNumber, num_gen_indices) = new_row_indices;
+ jCol_vec.segment(indexNumber, num_gen_indices) = new_col_indices;
+
+ return num_gen_indices;
+}
+}
+
+typedef tpl::DiscreteConstraintBase<double> DiscreteConstraintBase;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintContainerBase.h b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintContainerBase.h
new file mode 100644
index 0000000..d288b4e
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/DiscreteConstraintContainerBase.h
@@ -0,0 +1,201 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+
+#include "DiscreteConstraintBase.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief An abstract base class which serves as a container for all the
+ * discrete constraints used in the NLP
+ */
+template <typename SCALAR>
+class DiscreteConstraintContainerBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<VectorXi> MapVecXi;
+
+ /**
+ * @brief Default constructor
+ */
+ DiscreteConstraintContainerBase() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~DiscreteConstraintContainerBase() {}
+ /**
+ * @brief Gets called before the constraint evaluation. This method
+ * should contain all the calculations needed to evaluate the
+ * constraints
+ */
+ virtual void prepareEvaluation() = 0;
+
+ /**
+ * @brief Gets called before the constraint jacobian evaluation. This
+ * method should contain all the calculations needed to evaluate
+ * the constraint jacobian
+ */
+ virtual void prepareJacobianEvaluation() = 0;
+
+
+ /**
+ * @brief Writes the constraint evaluations into the large constraint
+ * optimization vector
+ *
+ * @param[out] c_nlp The constraint vector used in the NLP
+ */
+ void evalConstraints(MapVecXs& c_nlp)
+ {
+ prepareEvaluation();
+ size_t ind = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t cSize = constraint->getConstraintSize();
+ c_nlp.segment(ind, cSize) = constraint->eval();
+ ind += cSize;
+ }
+
+ assert(ind == c_nlp.rows()); // or throw an error
+ }
+
+ void evalConstraints(VectorXs& c_nlp)
+ {
+ prepareEvaluation();
+ size_t ind = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t cSize = constraint->getConstraintSize();
+ c_nlp.segment(ind, cSize) = constraint->eval();
+ ind += cSize;
+ }
+
+ assert(static_cast<int>(ind) == c_nlp.rows()); // or throw an error
+ }
+
+ /**
+ * @brief Evaluates the jacobian of the constraints and writes them
+ * into the nlp vector
+ *
+ * @param[out] jac_nlp The constraint jacobian vector used in NLP
+ * @param[in] nzz_jac_g The number of non zero elements in the jacobian
+ */
+ void evalSparseJacobian(MapVecXs& jac_nlp, const int nzz_jac_g)
+ {
+ prepareJacobianEvaluation();
+ size_t ind = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t nnEle = constraint->getNumNonZerosJacobian();
+ jac_nlp.segment(ind, nnEle) = constraint->evalSparseJacobian();
+ ind += nnEle;
+ }
+
+ assert(static_cast<int>(ind) == nzz_jac_g);
+ }
+
+ /**
+ * @brief Retrieves the sparsity pattern of the constraints and writes
+ * them into the nlp vectors
+ *
+ * @param[out] iRow_nlp The vector containing the row indices of the non
+ * zero entries of the constraint jacobian
+ * @param[out] jCol_nlp The vector containing the column indices of the
+ * non zero entries of the constraint jacobian
+ * @param[in] nnz_jac_g The number of non zero elements in the constraint jacobian
+ */
+ void getSparsityPattern(Eigen::Map<Eigen::VectorXi>& iRow_nlp,
+ Eigen::Map<Eigen::VectorXi>& jCol_nlp,
+ const int nnz_jac_g)
+ {
+ size_t ind = 0;
+ size_t constraintCount = 0;
+
+ for (auto constraint : constraints_)
+ {
+ size_t nnEle = constraint->getNumNonZerosJacobian();
+ size_t cSize = constraint->getConstraintSize();
+ Eigen::VectorXi iRow, jCol;
+ iRow.resize(nnEle);
+ jCol.resize(nnEle);
+ constraint->genSparsityPattern(iRow, jCol);
+ iRow_nlp.segment(ind, nnEle) = iRow.array() + constraintCount;
+ jCol_nlp.segment(ind, nnEle) = jCol;
+ constraintCount += cSize;
+ ind += nnEle;
+ }
+
+ assert(static_cast<int>(ind) == nnz_jac_g);
+ }
+
+ /**
+ * @brief Returns the number of constraints in the NLP
+ *
+ * @return The number of constraint in the NLP
+ */
+ size_t getConstraintsCount() const
+ {
+ size_t count = 0;
+ for (auto constraint : constraints_)
+ count += constraint->getConstraintSize();
+ return count;
+ }
+
+ /**
+ * @brief Returns the number of non zeros in the constraint jacobian
+ *
+ * @return The number of non zeros in the constraint jacobian
+ */
+ size_t getNonZerosJacobianCount() const
+ {
+ size_t count = 0;
+ for (auto constraint : constraints_)
+ count += constraint->getNumNonZerosJacobian();
+ return count;
+ }
+
+ /**
+ * @brief Retrieves the constraint bounds and writes them into the
+ * vectors used in the NLP
+ *
+ * @param[out] lowerBound The lower constraint bound
+ * @param[out] upperBound The lower constraint bound
+ */
+ void getBounds(MapVecXs& lowerBound, MapVecXs& upperBound)
+ {
+ size_t ind = 0;
+ for (auto constraint : constraints_)
+ {
+ size_t cSize = constraint->getConstraintSize();
+ lowerBound.segment(ind, cSize) = constraint->getLowerBound();
+ upperBound.segment(ind, cSize) = constraint->getUpperBound();
+ ind += cSize;
+ }
+ }
+
+protected:
+ std::vector<std::shared_ptr<DiscreteConstraintBase<SCALAR>>>
+ constraints_; /*!< contains all the constraints of the NLP */
+};
+}
+
+typedef tpl::DiscreteConstraintContainerBase<double> DiscreteConstraintContainerBase;
+}
+}
diff --git a/ct_optcon/include/ct/optcon/nlp/DiscreteCostEvaluatorBase.h b/ct_optcon/include/ct/optcon/nlp/DiscreteCostEvaluatorBase.h
new file mode 100644
index 0000000..1fc0cb6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/DiscreteCostEvaluatorBase.h
@@ -0,0 +1,57 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Implements an abstract base class which evaluates the cost
+ * function and its gradient in the NLP
+ */
+template <typename SCALAR>
+class DiscreteCostEvaluatorBase
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /**
+ * @brief Default constructor
+ */
+ DiscreteCostEvaluatorBase(){};
+
+ /**
+ * @brief Destructor.
+ */
+ virtual ~DiscreteCostEvaluatorBase(){};
+
+
+ /**
+ * @brief Evaluates the cost function
+ *
+ * @return The evaluates cost function
+ */
+ virtual SCALAR eval() = 0;
+
+ /**
+ * @brief Evaluates the cost gradient
+ *
+ * @param[in] grad_length The size of the gradient vector
+ * @param[out] grad The values of the gradient
+ */
+ virtual void evalGradient(size_t grad_length, Eigen::Map<Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>>& grad) = 0;
+};
+}
+
+typedef tpl::DiscreteCostEvaluatorBase<double> DiscreteCostEvaluatorBase;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/Nlp b/ct_optcon/include/ct/optcon/nlp/Nlp
new file mode 100644
index 0000000..0c2c7b8
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/Nlp
@@ -0,0 +1,8 @@
+#pragma once
+
+#include "Nlp.h"
+#include "solver/NlpSolver.h"
+#include "solver/IpoptSolver.h"
+#include "solver/SnoptSolver.h"
+#include "solver/NlpSolverSettings.h"
+
diff --git a/ct_optcon/include/ct/optcon/nlp/Nlp.h b/ct_optcon/include/ct/optcon/nlp/Nlp.h
new file mode 100644
index 0000000..7442050
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/Nlp.h
@@ -0,0 +1,493 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+#include <Eigen/Sparse>
+#include <ct/core/math/DerivativesCppadJIT.h>
+#include "OptVector.h"
+#include "DiscreteConstraintContainerBase.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+
+/** @defgroup NLP NLP
+ *
+ * @brief The nonlinear program module
+ */
+
+/**
+ * @ingroup NLP
+ *
+ * @brief The NLP base class. This class serves as abstract base class to
+ * use as an interface to the NLP solver IPOPT and SNOPT
+ */
+template <typename SCALAR>
+class Nlp
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<VectorXi> MapVecXi;
+ typedef Eigen::Map<const VectorXs> MapConstVecXs;
+
+ /**
+ * @brief Default constructor
+ */
+ Nlp() {}
+ /**
+ * @brief Destructor
+ */
+ virtual ~Nlp() {}
+ /**
+ * @brief { This method gets called at each update of the Optimization
+ * variables. This can be used to distribute or rearrange the
+ * optimization variables appropriately }
+ */
+ virtual void updateProblem() = 0;
+
+ /**
+ * @brief { Evaluates the costfunction at the current nlp iteration }
+ *
+ * @return { Scalar value of the resulting cost }
+ */
+ SCALAR evaluateCostFun()
+ {
+ if (!costCodegen_ && !costEvaluator_)
+ throw std::runtime_error("Error in evaluateCostFun. Costevaluator not initialized");
+
+ if (costCodegen_)
+ return costCodegen_->forwardZero(optVariables_->getOptimizationVars())(0);
+ else
+ return costEvaluator_->eval();
+ }
+
+
+ /**
+ * @brief { Evaluates the gradient of the costfunction}
+ *
+ * @param[in] n { size of the gradient }
+ * @param[out] grad The gradient of the cost function
+ */
+ void evaluateCostGradient(const size_t n, MapVecXs& grad)
+ {
+ if (!costCodegen_ && !costEvaluator_)
+ throw std::runtime_error("Error in evaluateCostGradient. Costevaluator not initialized");
+
+ if (costCodegen_)
+ grad = costCodegen_->jacobian(optVariables_->getOptimizationVars());
+ else
+ costEvaluator_->evalGradient(n, grad);
+ }
+
+ /**
+ * @brief { Evaluates the constraints }
+ *
+ * @param[out] values The values of the constraint violations, wrapped as a
+ * vector
+ */
+ void evaluateConstraints(MapVecXs& values)
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in evaluateConstraints. Constraints not initialized");
+
+ if (constraintsCodegen_)
+ values = constraintsCodegen_->forwardZero(optVariables_->getOptimizationVars());
+ else
+ constraints_->evalConstraints(values);
+ }
+
+ /**
+ * @brief { Evaluates the constraint jacobian }
+ *
+ * @param[in] nele_jac The number of non zeros in the constraint jacobian
+ * @param[out] jac The non zero values of the jacobian
+ */
+ void evaluateConstraintJacobian(const int nele_jac, MapVecXs& jac)
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in evaluateConstraintJacobian. Constraints not initialized");
+
+ if (constraintsCodegen_)
+ jac = constraintsCodegen_->sparseJacobianValues(optVariables_->getOptimizationVars());
+ else
+ constraints_->evalSparseJacobian(jac, nele_jac);
+ }
+
+ /**
+ * @brief Evaluates the hessian of the lagrangian
+ *
+ * @param[in] nele_hes The number of non zeros in the hessian
+ * @param hes The values of the non-zeros of the hessian
+ * @param[in] obj_fac The costfunction multiplier
+ * @param lambda The constraint multipliers
+ */
+ void evaluateHessian(const int nele_hes, MapVecXs& hes, const SCALAR obj_fac, MapConstVecXs& lambda)
+ {
+ if (!constraintsCodegen_ || !costCodegen_)
+ throw std::runtime_error(
+ "Error in evaluateHessian. Hessian Evaluation only implemented for codegeneration");
+
+ hes.setZero();
+ Eigen::Matrix<double, 1, 1> omega;
+ omega << obj_fac;
+
+ Eigen::VectorXd hessianCostValues =
+ costCodegen_->sparseHessianValues(optVariables_->getOptimizationVars(), omega);
+ Eigen::VectorXd hessianConstraintsValues =
+ constraintsCodegen_->sparseHessianValues(optVariables_->getOptimizationVars(), lambda);
+
+ for (size_t i = 0; i < hessianCostValues.rows(); ++i)
+ hessianCost_.coeffRef(iRowHessianCost_(i), jColHessianCost_(i)) = hessianCostValues(i);
+
+ for (size_t i = 0; i < hessianConstraintsValues.rows(); ++i)
+ hessianConstraints_.coeffRef(iRowHessianConstraints_(i), jColHessianConstraints_(i)) =
+ hessianConstraintsValues(i);
+
+ hessianTotal_ = (hessianCost_ + hessianConstraints_).template triangularView<Eigen::Lower>();
+
+ hes = Eigen::Map<Eigen::VectorXd>(hessianTotal_.valuePtr(), nele_hes, 1);
+ }
+
+ /**
+ * @brief Gets the sparsity pattern.
+ *
+ * @param[in] nele_jac The number of non zero elements in the constraint
+ * jacobian
+ * @param[out] iRow The row indices of the location of the non zero
+ * elements of the constraint jacobian
+ * @param[out] jCol The column indices of the location of the non zero
+ * elements of the constraint jacobian
+ */
+ void getSparsityPatternJacobian(const int nele_jac, MapVecXi& iRow, MapVecXi& jCol) const
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in getSparsityPatternJacobian. Constraints not initialized");
+
+ iRow.setZero();
+ jCol.setZero();
+
+ if (constraintsCodegen_)
+ {
+ Eigen::VectorXi iRow1;
+ Eigen::VectorXi jCol1;
+ constraintsCodegen_->getSparsityPatternJacobian(iRow1, jCol1);
+
+ iRow = iRow1;
+ jCol = jCol1;
+ }
+ else
+ constraints_->getSparsityPattern(iRow, jCol, nele_jac);
+ }
+
+ /**
+ * @brief Gets the sparsity pattern hessian of the lagrangian
+ *
+ * @param[in] nele_hes The number of non zero elements in the hessian
+ * @param iRow The row indices
+ * @param jCol The column indices
+ */
+ void getSparsityPatternHessian(const int nele_hes, MapVecXi& iRow, MapVecXi& jCol) const
+ {
+ if (!constraintsCodegen_ || !costCodegen_)
+ throw std::runtime_error(
+ "Error in getSparsityPatternHessian. Hessian Evaluation only implemented for codegeneration");
+
+ iRow.setZero();
+ jCol.setZero();
+
+ iRow = iRowHessian_;
+ jCol = jColHessian_;
+ }
+
+ /**
+ * @brief Returns the number of constraints in the NLP
+ *
+ * @return The number of constraints.
+ */
+ size_t getConstraintsCount() const
+ {
+ if (!constraints_)
+ throw std::runtime_error("Error in getConstraintsCount. Constraints not initialized");
+
+ return constraints_->getConstraintsCount();
+ }
+
+ /**
+ * @brief Returns the number of the non zero elements of the constraint
+ * jacobian.
+ *
+ * @return The number of the non zero elements of the constraint
+ * jacobian.
+ */
+ size_t getNonZeroJacobianCount() const
+ {
+ if (!constraintsCodegen_ && !constraints_)
+ throw std::runtime_error("Error in getNonZeroJacobianCount. Constraints not initialized");
+
+ if (constraintsCodegen_)
+ return constraintsCodegen_->getNumNonZerosJacobian();
+ else
+ return constraints_->getNonZerosJacobianCount();
+ }
+
+ /**
+ * @brief Returns the number of non zeros in the hessian
+ *
+ * @return The number of non zeros in the hessian
+ */
+ size_t getNonZeroHessianCount()
+ {
+ if (!constraintsCodegen_ || !costCodegen_)
+ {
+ std::cerr << "Error: Exact hessians only work with AD hessian codegeneration" << std::endl;
+ throw std::runtime_error("Error in getNonZeroHessianCount. Codegeneration not initialized");
+ }
+
+ costCodegen_->getSparsityPatternHessian(iRowHessianCost_, jColHessianCost_);
+ constraintsCodegen_->getSparsityPatternHessian(iRowHessianConstraints_, jColHessianConstraints_);
+
+ std::vector<Eigen::Triplet<SCALAR>> tripletsCost;
+ std::vector<Eigen::Triplet<SCALAR>> tripletsConstraints;
+
+ for (size_t i = 0; i < iRowHessianCost_.rows(); ++i)
+ tripletsCost.push_back(Eigen::Triplet<SCALAR>(iRowHessianCost_(i), jColHessianCost_(i), SCALAR(0.1)));
+
+ for (size_t i = 0; i < iRowHessianConstraints_.rows(); ++i)
+ tripletsConstraints.push_back(
+ Eigen::Triplet<SCALAR>(iRowHessianConstraints_(i), jColHessianConstraints_(i), SCALAR(0.1)));
+
+ hessianCost_.resize(optVariables_->size(), optVariables_->size());
+ hessianCost_.setFromTriplets(tripletsCost.begin(), tripletsCost.end());
+ hessianConstraints_.resize(optVariables_->size(), optVariables_->size());
+ hessianConstraints_.setFromTriplets(tripletsConstraints.begin(), tripletsConstraints.end());
+
+ hessianTotal_.resize(optVariables_->size(), optVariables_->size());
+ hessianTotal_ = (hessianCost_ + hessianConstraints_).template triangularView<Eigen::Lower>();
+
+ std::vector<int> iRowHessianStdVec;
+ std::vector<int> jColHessianStdVec;
+ for (size_t k = 0; k < hessianTotal_.outerSize(); ++k)
+ for (typename Eigen::SparseMatrix<SCALAR>::InnerIterator it(hessianTotal_, k); it; ++it)
+ {
+ iRowHessianStdVec.push_back(it.row());
+ jColHessianStdVec.push_back(it.col());
+ }
+
+ iRowHessian_ = Eigen::Map<Eigen::VectorXi>(iRowHessianStdVec.data(), iRowHessianStdVec.size(), 1);
+ jColHessian_ = Eigen::Map<Eigen::VectorXi>(jColHessianStdVec.data(), jColHessianStdVec.size(), 1);
+
+ size_t nonZerosHessian = iRowHessian_.rows();
+ return nonZerosHessian;
+ }
+
+ /**
+ * @brief Reads the bounds of the constraints.
+ *
+ * @param[out] lowerBound The lower constraint bound
+ * @param[out] upperBound The upper constraint bound
+ * @param[in] m { The size of the constraints }
+ */
+ void getConstraintBounds(MapVecXs& lowerBound, MapVecXs& upperBound, const size_t m) const
+ {
+ if (!constraints_)
+ throw std::runtime_error("Error in getConstraintBounds. Constraints not initialized");
+
+ constraints_->getBounds(lowerBound, upperBound);
+ }
+
+ /**
+ * @brief Returns the number of Optimization optimization variables
+ *
+ * @return The number of Optimization variables.
+ */
+ size_t getVarCount() const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getVarCount. Optvariables not initialized");
+
+ return optVariables_->size();
+ }
+
+ /**
+ * @brief Reads the bounds on the Optimization optimization variables.
+ *
+ * @param[out] lowerBound The lower optimization variable bound
+ * @param[out] upperBound The upper optimization variable bound
+ * @param[in] n { The number of Optimization variables }
+ */
+ void getVariableBounds(MapVecXs& lowerBound, MapVecXs& upperBound, const size_t n) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getVariableBounds. Optvariables not initialized");
+
+ optVariables_->getLowerBounds(lowerBound);
+ optVariables_->getUpperBounds(upperBound);
+ }
+
+ /**
+ * @brief {Extracts the Optimization optimization variables from the nlp
+ * solvers between nlp iterations}
+ *
+ * @param[in] x { The value of the Optimization variables }
+ * @param[in] isNew Indicates whether x differs from a previous call
+ */
+ void extractOptimizationVars(const MapConstVecXs& x, bool isNew)
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in extractOptimizationVars. Optvariables not initialized");
+
+ if (isNew)
+ {
+ optVariables_->setOptimizationVars(x);
+ updateProblem();
+ }
+ }
+
+ /**
+ * @brief Gets the Optimization variables.
+ *
+ * @param[in] n { The number of Optimization variables }
+ * @param[out] x { The values of the Optimization vars }
+ */
+ void getInitialGuess(const size_t n, MapVecXs& x) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getOptimizationVars. Optvariables not initialized");
+
+ optVariables_->getInitialGuess(n, x);
+ }
+
+ /**
+ * @brief Gets the variable multiplier and the variable state, used in
+ * the NLP solver SNOPT. See the snopt documentation for further
+ * explanations
+ *
+ * @param[in] n { The number of Optimization variables }
+ * @param[out] xMul The optimization variable multiplier
+ * @param[out] xState The optimization variable states
+ */
+ void getOptimizationMultState(const size_t n, MapVecXs& xMul, MapVecXi& xState) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getOptimizationMultState. Optvariables not initialized");
+
+ optVariables_->getOptimizationMultState(n, xMul, xState);
+ }
+
+ /**
+ * @brief Gets the constraint multiplier and state, used in the NLP
+ * solver SNOPT.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] zMul The constraint variable multiplier
+ * @param[out] zState The constraint variable state
+ */
+ void getConstraintsMultState(const size_t m, MapVecXs& zMul, MapVecXi& zState) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getConstraintsMultState. Optvariables not initialized");
+
+ optVariables_->getConstraintsMultState(m, zMul, zState);
+ }
+
+ /**
+ * @brief Gets the bound multipliers used in the NLP solver IPOPT.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] zLow The value for the lower bound multiplier
+ * @param[out] zUp The value for the upper bound multiplier
+ */
+ void getBoundMultipliers(size_t n, MapVecXs& zLow, MapVecXs& zUp) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getBoundMultipliers. Optvariables not initialized");
+
+ optVariables_->getBoundMultipliers(n, zLow, zUp);
+ }
+
+ /**
+ * @brief Gets the values of the constraint multipliers.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] lambda The values of the constraint multipliers
+ */
+ void getLambdaVars(size_t m, MapVecXs& lambda) const
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in getLambdaVars. Optvariables not initialized");
+
+ optVariables_->getLambdaVars(m, lambda);
+ }
+
+ /**
+ * @brief { Extracts the solution values from IPOPT }
+ *
+ * @param[in] x { The values of the optimization variables }
+ * @param[in] zL The value for the lower bound multiplier
+ * @param[in] zU The value for the upper bound multiplier
+ * @param[in] lambda The values of the constraint multipliers
+ */
+ void extractIpoptSolution(const MapConstVecXs& x,
+ const MapConstVecXs& zL,
+ const MapConstVecXs& zU,
+ const MapConstVecXs& lambda)
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in extractIpoptSolution. Optvariables not initialized");
+
+ optVariables_->setNewIpoptSolution(x, zL, zU, lambda);
+ }
+
+ /**
+ * @brief { Extracts the solution values from SNOPT }
+ *
+ * @param[in] x { The values of the optimization variables }
+ * @param[in] xMul The optimization variable multiplier
+ * @param[in] xState The optimization variable state
+ * @param[in] fMul The constraint variable multiplier
+ * @param[in] fState The constraint variable state
+ */
+ void extractSnoptSolution(const MapVecXs& x,
+ const MapVecXs& xMul,
+ const MapVecXi& xState,
+ const MapVecXs& fMul,
+ const MapVecXi& fState)
+ {
+ if (!optVariables_)
+ throw std::runtime_error("Error in extractSnoptSolution. Optvariables not initialized");
+
+ optVariables_->setNewSnoptSolution(x, xMul, xState, fMul, fState);
+ }
+
+
+protected:
+ std::shared_ptr<DiscreteCostEvaluatorBase<SCALAR>>
+ costEvaluator_; //! abstract base class, approximates the cost evaluation for the discrete problem
+ std::shared_ptr<OptVector<SCALAR>>
+ optVariables_; //! base class, containts the optimization variables used in the NLP solvers
+ std::shared_ptr<DiscreteConstraintContainerBase<SCALAR>>
+ constraints_; //! abstract base class, contains the discretized constraints for the problem
+ std::shared_ptr<ct::core::DerivativesCppadJIT<-1, 1>> costCodegen_;
+ std::shared_ptr<ct::core::DerivativesCppadJIT<-1, -1>> constraintsCodegen_;
+
+ Eigen::SparseMatrix<SCALAR> hessianCost_, hessianConstraints_, hessianTotal_;
+
+ Eigen::VectorXi iRowHessianCost_, iRowHessianConstraints_, jColHessianCost_, jColHessianConstraints_;
+ Eigen::VectorXi iRowHessian_;
+ Eigen::VectorXi jColHessian_;
+};
+}
+
+typedef tpl::Nlp<double> Nlp;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/OptVector.h b/ct_optcon/include/ct/optcon/nlp/OptVector.h
new file mode 100644
index 0000000..d2c4dbc
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/OptVector.h
@@ -0,0 +1,322 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Class containing and managing all the optimization variables used
+ * for in the NLP solver IPOPT and SNOPT.
+ */
+template <typename SCALAR>
+class OptVector
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<int, Eigen::Dynamic, 1> VectorXi;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<VectorXi> MapVecXi;
+ typedef Eigen::Map<const VectorXs> MapConstVecXs;
+
+ OptVector() = delete;
+
+ /**
+ * @brief { Constructor resizing the vectors of the optimization
+ * variables to the correct size }
+ *
+ * @param[in] n The number of the optimization variables
+ */
+ OptVector(const size_t n) : updateCount_(0)
+ {
+ x_.resize(n);
+ x_.setZero();
+ xInit_.resize(n);
+ xLb_.resize(n);
+ xUb_.resize(n);
+ xLb_.setConstant(std::numeric_limits<SCALAR>::lowest());
+ xUb_.setConstant(std::numeric_limits<SCALAR>::max());
+ xMul_.resize(n);
+ xMul_.setZero();
+ xState_.resize(n);
+ xState_.setZero();
+ zUpper_.resize(n);
+ zUpper_.setZero();
+ zLow_.resize(n);
+ zLow_.setZero();
+ }
+
+ /**
+ * @brief Resizes the vectors of the constraint variables to the
+ * correct size
+ *
+ * @param[in] m The number of constraints
+ */
+ void resizeConstraintVars(size_t m)
+ {
+ zMul_.resize(m + 1);
+ zMul_.setZero();
+ zState_.resize(m + 1);
+ zState_.setZero();
+ }
+
+ void reset() { updateCount_ = 0; }
+ /**
+ * @brief Destructor
+ */
+ virtual ~OptVector() {}
+ /**
+ * @brief Resizes the vectors of the optimization variables
+ *
+ * @param[in] size The size of the new optimization variables
+ */
+ void resizeOptimizationVars(const size_t size)
+ {
+ x_.resize(size);
+ xInit_.resize(size);
+ xLb_.resize(size);
+ xUb_.resize(size);
+ lambda_.resize(size);
+ zUpper_.resize(size);
+ zLow_.resize(size);
+ }
+
+
+ /**
+ * @brief Resets the optimization variables
+ */
+ void setZero()
+ {
+ x_.setZero();
+ xInit_.setZero();
+ xLb_.setZero();
+ xUb_.setZero();
+ lambda_.setZero();
+ zUpper_.setZero();
+ zLow_.setZero();
+ }
+
+ /**
+ * @brief Checks if the optimization variables have to correct size
+ *
+ * @param[in] n The number of optimization variables
+ *
+ * @return returns true of the dimensions match
+ */
+ bool checkOptimizationVarDimension(const unsigned int n)
+ {
+ bool xDim = x_.size() == n ? true : false;
+ bool xLDim = xLb_.size() == n ? true : false;
+ bool xUDim = xUb_.size() == n ? true : false;
+ return xDim && xLDim && xUDim;
+ }
+
+ /**
+ * @brief Sets the optimization variable bounds.
+ *
+ * @param[in] xLb The lower optimization variable bound
+ * @param[in] xUb The upper optimization variable bound
+ */
+ void setBounds(const VectorXs& xLb, const VectorXs& xUb)
+ {
+ xLb_ = xLb;
+ xUb_ = xUb;
+ }
+
+
+ /**
+ * @brief Gets the lower bounds of the optimization variables.
+ *
+ * @param[out] x Lower bound
+ */
+ void getLowerBounds(MapVecXs& x) const { x = xLb_; }
+ /**
+ * @brief Gets the upper bounds of the optimization variables.
+ *
+ * @param[out] x The upper bound
+ */
+ void getUpperBounds(MapVecXs& x) const { x = xUb_; }
+ /**
+ * @brief Return the state and the multiplier of the optimization
+ * variables, used in the NLP solver SNOPT.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] xMul The optimization variables multiplier
+ * @param[out] xState The optimization variables state
+ */
+ void getOptimizationMultState(const size_t n, MapVecXs& xMul, MapVecXi& xState) const
+ {
+ assert(n == xMul_.size());
+ assert(n == xState_.size());
+ xMul = xMul_;
+ xState = xState_;
+ }
+
+ /**
+ * @brief Gets the constraint multiplier and state, used in the NLP
+ * solver SNOPT.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] zMul The constraint variable multiplier
+ * @param[out] zState The constraint variable state
+ */
+ void getConstraintsMultState(const size_t m, MapVecXs& zMul, MapVecXi& zState) const
+ {
+ assert(m == zMul_.size());
+ assert(m == zState_.size());
+ zMul = zMul_;
+ zState = zState_;
+ }
+
+ /**
+ * @brief Returns the number of optimization variables
+ *
+ * @return the number of optimization variables
+ */
+ size_t size() const { return x_.size(); }
+ /**
+ * @brief Gets the bound multipliers used in the NLP solver IPOPT.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] low The value for the lower bound multiplier
+ * @param[out] up The value for the upper bound multiplier
+ */
+ void getBoundMultipliers(size_t n, MapVecXs& low, MapVecXs& up) const
+ {
+ assert(n == zLow_.size());
+ low = zLow_;
+ up = zUpper_;
+ }
+
+ /**
+ * @brief Gets the values of the constraint multipliers.
+ *
+ * @param[in] m { The number of constraints }
+ * @param[out] x The values of the constraint multipliers
+ */
+ void getLambdaVars(size_t m, MapVecXs& x) const
+ {
+ assert(m == lambda_.size());
+ x = lambda_;
+ }
+
+ /**
+ * @brief Gets the optimization variables.
+ *
+ * @param[in] n { The number of optimization variables }
+ * @param[out] x The optimization variables
+ */
+ void getOptimizationVars(size_t n, MapVecXs& x) const
+ {
+ assert(n == x_.size());
+ x = x_;
+ }
+
+ VectorXs getOptimizationVars() const { return x_; }
+ void getInitialGuess(size_t n, MapVecXs& x) const
+ {
+ assert(n == xInit_.size());
+ x = xInit_;
+ }
+
+ /**
+ * @brief Extracts the solution from ipopt and stores them into class
+ * variables
+ *
+ * @param[in] x The optimization variables
+ * @param[in] zL The lower bound multiplier
+ * @param[in] zU The upper bound multiplier
+ * @param[in] lambda The constraint multiplier
+ */
+ void setNewIpoptSolution(const MapConstVecXs& x,
+ const MapConstVecXs& zL,
+ const MapConstVecXs& zU,
+ const MapConstVecXs& lambda)
+ {
+ x_ = x;
+ zLow_ = zL;
+ zUpper_ = zU;
+ lambda_ = lambda;
+ }
+
+ /**
+ * @brief Extracts the solution from snopt and stores it into class
+ * variables
+ *
+ * @param[in] x The optimization variables
+ * @param[in] xMul The optimization variables multiplier
+ * @param[in] xState The optimization variables state
+ * @param[in] fMul The constraints multiplier
+ * @param[in] fState The constraints state
+ */
+ void setNewSnoptSolution(const MapVecXs& x,
+ const MapVecXs& xMul,
+ const MapVecXi& xState,
+ const MapVecXs& fMul,
+ const MapVecXi& fState)
+ {
+ x_ = x;
+ xMul_ = xMul;
+ xState_ = xState;
+ zMul_ = fMul;
+ zState_ = fState;
+ }
+
+ /**
+ * @brief Sets the updates optimization variables from the NLP solver
+ * and updates the counter
+ *
+ * @param[in] x The updates primal variables
+ */
+ void setOptimizationVars(const MapConstVecXs& x)
+ {
+ x_ = x;
+ updateCount_++;
+ }
+
+ void setOptimizationVars(const VectorXs& x)
+ {
+ x_ = x;
+ updateCount_++;
+ }
+
+
+ /**
+ * @brief Returns the update counter
+ *
+ * @return The update counter
+ */
+ size_t getUpdateCount() const { return updateCount_; }
+protected:
+ VectorXs x_; /*!< The optimization variables */
+ VectorXs xInit_;
+ VectorXs xLb_; /*!< lower bound on optimization vector */
+ VectorXs xUb_; /*!< upper bound on optimization vector */
+
+ VectorXs zUpper_; /*!< The upper bound multiplier, used in IPOPT */
+ VectorXs zLow_; /*!< The lower bound multiplier, used in IPOPT */
+ VectorXs lambda_; /*!< The constraint multiplier, used in IPOPT */
+
+ // Snopt variables
+ VectorXs xMul_; /*!< The optimization variable multiplier, used in SNOPT */
+ VectorXi xState_; /*!< The optimization variable state, used in SNOPT */
+ VectorXs zMul_; /*!< The constraint multiplier, used in SNOPT */
+ VectorXi zState_; /*!< The constraint state, used in SNOPT */
+
+ size_t updateCount_; /*!< The number of optimization variable updates */
+};
+}
+
+typedef tpl::OptVector<double> OptVector;
+}
+}
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/IpoptSolver.h b/ct_optcon/include/ct/optcon/nlp/solver/IpoptSolver.h
new file mode 100644
index 0000000..2ec1366
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/IpoptSolver.h
@@ -0,0 +1,213 @@
+/**********************************************************************************************************************
+Copyright (c) 2016, Agile & Dexterous Robotics Lab, ETH ZURICH. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name of ETH ZURICH nor the names of its contributors may be used
+ to endorse or promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/nlp/Nlp.h>
+#include "NlpSolver.h"
+
+#ifdef BUILD_WITH_IPOPT_SUPPORT // build IPOPT interface
+
+#include "IpTNLP.hpp"
+#include "IpIpoptApplication.hpp"
+#include "IpSolveStatistics.hpp"
+
+#endif // BUILD_WITH_IPOPT_SUPPORT
+
+// #define DEBUG_PRINT
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+#ifdef BUILD_WITH_IPOPT_SUPPORT // build IPOPT interface
+
+/**
+ * @ingroup NLP
+ *
+ * @brief The interface to the NLP solver IPOPT
+ *
+ * For the implementation see ct/ct_optcon/src/nlp/solver/IpoptSolver.cpp
+ */
+template <typename SCALAR>
+class IpoptSolver : public Ipopt::TNLP, public NlpSolver<SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef NlpSolver<SCALAR> BASE;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Map<VectorXs> MapVecXs;
+ typedef Eigen::Map<const VectorXs> MapConstVecXs;
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] nlp The nlp
+ * @param[in] settings The nlp settings
+ */
+ IpoptSolver(std::shared_ptr<tpl::Nlp<SCALAR>> nlp, const NlpSolverSettings& settings);
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~IpoptSolver();
+
+ virtual bool solve() override;
+
+ virtual void prepareWarmStart(size_t maxIterations) override;
+
+ virtual void configureDerived(const NlpSolverSettings& settings) override;
+
+ /**@name Overloaded from TNLP */
+ //@{
+ /** Method to return some info about the nlp */
+ virtual bool get_nlp_info(Ipopt::Index& n,
+ Ipopt::Index& m,
+ Ipopt::Index& nnz_jac_g,
+ Ipopt::Index& nnz_h_lag,
+ IndexStyleEnum& index_style);
+
+ /** Method to return the bounds for my problem */
+ virtual bool get_bounds_info(Ipopt::Index n, SCALAR* x_l, SCALAR* x_u, Ipopt::Index m, SCALAR* g_l, SCALAR* g_u);
+
+ /** Method to return the starting point for the algorithm */
+ virtual bool get_starting_point(Ipopt::Index n,
+ bool init_x,
+ SCALAR* x,
+ bool init_z,
+ SCALAR* z_L,
+ SCALAR* z_U,
+ Ipopt::Index m,
+ bool init_lambda,
+ SCALAR* lambda);
+
+ /** Method to return the objective value */
+ virtual bool eval_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR& obj_value);
+
+ /** Method to return the gradient of the objective */
+ virtual bool eval_grad_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR* grad_f);
+
+ /** Method to return the constraint residuals */
+ virtual bool eval_g(Ipopt::Index n, const SCALAR* x, bool new_x, Ipopt::Index m, SCALAR* g);
+
+ /** Method to return:
+ * 1) The structure of the jacobian (if "values" is NULL)
+ * 2) The values of the jacobian (if "values" is not NULL)
+ */
+ virtual bool eval_jac_g(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ Ipopt::Index m,
+ Ipopt::Index nele_jac,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values);
+
+ /** Method to return:
+ * 1) The structure of the hessian of the lagrangian (if "values" is NULL)
+ * 2) The values of the hessian of the lagrangian (if "values" is not NULL)
+ */
+ virtual bool eval_h(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ SCALAR obj_factor,
+ Ipopt::Index m,
+ const SCALAR* lambda,
+ bool new_lambda,
+ Ipopt::Index nele_hess,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values);
+
+ //@}
+
+ /** @name Solution Methods */
+ //@{
+ /** This method is called when the algorithm is complete so the TNLP can store/write the solution */
+ virtual void finalize_solution(Ipopt::SolverReturn status,
+ Ipopt::Index n,
+ const SCALAR* x,
+ const SCALAR* z_L,
+ const SCALAR* z_U,
+ Ipopt::Index m,
+ const SCALAR* g,
+ const SCALAR* lambda,
+ SCALAR obj_value,
+ const Ipopt::IpoptData* ip_data,
+ Ipopt::IpoptCalculatedQuantities* ip_cq);
+ //@}
+
+private:
+ /**
+ * @brief Sets the IPOPT solver options.
+ */
+ void setSolverOptions();
+ std::shared_ptr<Ipopt::IpoptApplication> ipoptApp_; /*!< A pointer to ipopt*/
+ Ipopt::ApplicationReturnStatus status_; /*!< The return status of IPOPT*/
+ IpoptSettings settings_; /*!< Contains the IPOPT settings*/
+
+ /**@name Methods to block default compiler methods.
+ * The compiler automatically generates the following three methods.
+ * Since the default compiler implementation is generally not what
+ * you want (for all but the most simple classes), we usually
+ * put the declarations of these methods in the private section
+ * and never implement them. This prevents the compiler from
+ * implementing an incorrect "default" behavior without us
+ * knowing. (See Scott Meyers book, "Effective C++")
+ *
+ */
+ //@{
+ IpoptSolver(const IpoptSolver&);
+ IpoptSolver& operator=(const IpoptSolver&);
+ //@}
+};
+
+#include "implementation/IpoptSolver-impl.h"
+
+#else // BUILD_WITH_IPOPT_SUPPORT -- not building with IPOPT support, create dummy class
+
+template <typename SCALAR>
+class IpoptSolver : public NlpSolver<SCALAR>
+{
+public:
+ IpoptSolver() { throw(std::runtime_error("Error - IPOPT interface not compiled.")); }
+ IpoptSolver(std::shared_ptr<tpl::Nlp<SCALAR>> nlp, NlpSolverSettings settings)
+ {
+ throw(std::runtime_error("Error - IPOPT interface not compiled."));
+ }
+
+ virtual bool solve() override { return false; }
+ virtual void prepareWarmStart(size_t maxIterations) override {}
+ virtual void configureDerived(const NlpSolverSettings& settings) override {}
+};
+
+#endif // BUILD_WITH_IPOPT_SUPPORT
+ //
+} // namespace tpl
+
+typedef tpl::IpoptSolver<double> IpoptSolver;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/NlpSolver.h b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolver.h
new file mode 100644
index 0000000..3c01e00
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolver.h
@@ -0,0 +1,97 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/nlp/Nlp.h>
+#include "NlpSolverSettings.h"
+
+namespace ct {
+namespace optcon {
+namespace tpl {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Abstract base class for the NLP solvers
+ */
+template <typename SCALAR>
+class NlpSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /**
+ * @brief Default constructor
+ */
+ NlpSolver() : isInitialized_(false) {}
+ /**
+ * @brief Custom Constructor 1
+ *
+ * @param[in] nlp The nlp
+ */
+ NlpSolver(std::shared_ptr<Nlp<SCALAR>> nlp) : nlp_(nlp) {}
+ /**
+ * @brief Custom constructor 2
+ *
+ * @param[in] nlp The nlp
+ * @param[in] settings Custom user settings
+ */
+ NlpSolver(std::shared_ptr<Nlp<SCALAR>> nlp, NlpSolverSettings settings)
+ : nlp_(nlp), settings_(settings), isInitialized_(false)
+ {
+ }
+
+ /**
+ * @brief Destructor
+ */
+ virtual ~NlpSolver() {}
+ /**
+ * @brief Configures the solver with new settings
+ *
+ * @param[in] settings The nlp solver settings
+ */
+ void configure(const NlpSolverSettings& settings)
+ {
+ settings_ = settings;
+ configureDerived(settings);
+ }
+
+ /**
+ * @brief Forwards the settings to the corresponding nlp solver
+ *
+ * @param[in] settings The nlp settings
+ */
+ virtual void configureDerived(const NlpSolverSettings& settings) = 0;
+
+ /**
+ * @brief Solves the nlp
+ *
+ * @return Returns true of solve succeeded
+ */
+ virtual bool solve() = 0;
+
+ /**
+ * @brief Prepares the solver for a warmstarting scenario with
+ * available (good) initial guess
+ *
+ * @param[in] maxIterations Specifies the maximum number of nlp iteration
+ * the user is willing to spend
+ */
+ virtual void prepareWarmStart(const size_t maxIterations) = 0;
+
+ bool isInitialized() { return isInitialized_; }
+protected:
+ std::shared_ptr<Nlp<SCALAR>> nlp_; /*!< The non linear program*/
+ NlpSolverSettings settings_; /*!< The nlp settings */
+ bool isInitialized_; /*!< Indicates whether the solver is initialized */
+};
+}
+
+typedef tpl::NlpSolver<double> NlpSolver;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/NlpSolverSettings.h b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolverSettings.h
new file mode 100644
index 0000000..48f4072
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/NlpSolverSettings.h
@@ -0,0 +1,327 @@
+/**********************************************************************************************************************
+Copyright (c) 2016, Agile & Dexterous Robotics Lab, ETH ZURICH. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name of ETH ZURICH nor the names of its contributors may be used
+ to endorse or promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <iostream>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/info_parser.hpp>
+
+namespace ct {
+namespace optcon {
+
+/**
+ * @ingroup NLP
+ *
+ * @brief SnoptSolver settings. Details about the parameters can be found
+ * in the SNOPT documentation
+ */
+class SnoptSettings
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /**
+ * @brief Default constructor, sets the parameters to default values
+ */
+ SnoptSettings()
+ : scale_option_param_(0),
+ derivative_option_param_(1),
+ verify_level_param_(-1),
+ major_iteration_limit_param_(500),
+ minor_iteration_limit_param_(500),
+ iteration_limit_param_(5000),
+ major_optimality_tolerance_param_(1e-6),
+ major_feasibility_tolerance_param_(1e-6),
+ minor_feasibility_tolerance_param_(1e-6),
+ print_file_param_(0),
+ minor_print_level_param_(0),
+ major_print_level_param_(0),
+ new_basis_file_param_(0),
+ old_basis_file_param_(0),
+ backup_basis_file_param_(0),
+ line_search_tolerance_param_(0.9),
+ crash_option_(3),
+ hessian_updates_(5)
+ {
+ }
+
+ int scale_option_param_;
+ int derivative_option_param_;
+ int verify_level_param_;
+ int major_iteration_limit_param_;
+ int minor_iteration_limit_param_;
+ int iteration_limit_param_;
+ double major_optimality_tolerance_param_;
+ double major_feasibility_tolerance_param_;
+ double minor_feasibility_tolerance_param_;
+ int print_file_param_;
+ int minor_print_level_param_;
+ int major_print_level_param_;
+ int new_basis_file_param_;
+ int old_basis_file_param_;
+ int backup_basis_file_param_;
+ double line_search_tolerance_param_;
+ int crash_option_;
+ int hessian_updates_;
+
+ /**
+ * @brief Prints out information about the settings
+ */
+ void print()
+ {
+ std::cout << "SNOPT settings:" << std::endl;
+ std::cout << "Max Major Iterations: " << major_iteration_limit_param_ << std::endl;
+ std::cout << "Max Minor Iterations: " << minor_iteration_limit_param_ << std::endl;
+ }
+
+ /**
+ * @brief Checks whether to settings are filled with meaningful values
+ *
+ * @return Returns true of the parameters are ok
+ */
+ bool parametersOk() const { return true; }
+ /**
+ * @brief Loads the settings from a .info file
+ *
+ * @param[in] filename The filename
+ * @param[in] verbose True if parameters to be printed out
+ * @param[in] ns The namespace in the .info file
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "dms.solver.snopt")
+ {
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ minor_iteration_limit_param_ = pt.get<unsigned int>(ns + ".MaxMinorIterations");
+ major_iteration_limit_param_ = pt.get<unsigned int>(ns + ".MaxMajorIterations");
+ minor_print_level_param_ = pt.get<unsigned int>(ns + ".MinorPrintLevelVerbosity");
+ major_print_level_param_ = pt.get<unsigned int>(ns + ".MajorPrintLevelVerbosity");
+ major_optimality_tolerance_param_ = pt.get<double>(ns + ".OptimalityTolerance");
+ }
+};
+
+/**
+ * @ingroup NLP
+ *
+ * @brief IPOPT settings. Details about the parameters can be found
+ * in the IPOPT documentation
+ */
+class IpoptSettings
+{
+public:
+ /**
+ * @brief Default constructor, sets the parameters to default values
+ */
+ IpoptSettings()
+ : tol_(1e-8),
+ constr_viol_tol_(1e-4),
+ max_iter_(200),
+ restoTol_(1e-7),
+ acceptableTol_(1e-6),
+ restoAcceptableTol_(1e-7),
+ linear_scaling_on_demand_("yes"),
+ hessian_approximation_("limited-memory"),
+ nlp_scaling_method_("gradient-based"),
+ printLevel_(5),
+ print_user_options_("no"),
+ print_frequency_iter_(1),
+ printInfoString_("no"),
+ derivativeTest_("none"),
+ derivativeTestTol_(1e-4),
+ derivativeTestPerturbation_(1e-8),
+ point_perturbation_radius_(10),
+ checkDerivativesForNaninf_("no"),
+ derivativeTestPrintAll_("no"),
+ linearSystemScaling_("mc19"),
+ linear_solver_("ma57"),
+ jacobianApproximation_("finite-difference-values")
+ {
+ }
+
+ double tol_;
+ double constr_viol_tol_;
+ int max_iter_;
+ double restoTol_;
+ double acceptableTol_;
+ double restoAcceptableTol_;
+ std::string linear_scaling_on_demand_;
+ std::string hessian_approximation_;
+ std::string nlp_scaling_method_;
+ int printLevel_;
+ std::string print_user_options_;
+ int print_frequency_iter_;
+ std::string printInfoString_;
+ std::string derivativeTest_;
+ double derivativeTestTol_;
+ double derivativeTestPerturbation_;
+ double point_perturbation_radius_;
+ std::string checkDerivativesForNaninf_;
+ std::string derivativeTestPrintAll_;
+ std::string linearSystemScaling_;
+ std::string linear_solver_;
+ std::string jacobianApproximation_;
+
+ /**
+ * @brief Prints out information about the settings
+ */
+ void print()
+ {
+ std::cout << "IPOPT SETTINGS: " << std::endl;
+ std::cout << "Using " << hessian_approximation_ << " hessian approximation" << std::endl;
+ std::cout << "MaxIterations: " << max_iter_ << std::endl;
+ }
+
+ /**
+ * @brief Checks whether to settings are filled with meaningful values
+ *
+ * @return Returns true of the parameters are ok
+ */
+ bool parametersOk() const { return true; }
+ /**
+ * @brief Loads the settings from a .info file
+ *
+ * @param[in] filename The filename
+ * @param[in] verbose True if parameters to be printed out
+ * @param[in] ns The namespace in the .info fil
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "dms.nlp.ipopt")
+ {
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt); //
+ max_iter_ = pt.get<unsigned int>(ns + ".MaxIterations");
+ bool checkDerivatives = pt.get<bool>(ns + ".CheckDerivatives");
+ if (checkDerivatives)
+ derivativeTest_ = "first-order";
+ if (!checkDerivatives)
+ derivativeTest_ = "none";
+ bool exactHessian = pt.get<bool>(ns + ".ExactHessian");
+ if (exactHessian)
+ hessian_approximation_ = "exact";
+ if (!exactHessian)
+ hessian_approximation_ = "limited-memory";
+
+ printLevel_ = pt.get<unsigned int>(ns + ".Verbosity");
+ tol_ = pt.get<double>(ns + ".OptimalityTolerance");
+ }
+};
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Contains the NLP solver settings
+ */
+class NlpSolverSettings
+{
+public:
+ typedef enum SolverType { IPOPT = 0, SNOPT = 1, num_types_solver } SolverType_t;
+
+ /**
+ * @brief Default constructor, set default settings
+ */
+ NlpSolverSettings() : solverType_(IPOPT), useGeneratedCostGradient_(false), useGeneratedConstraintJacobian_(false)
+ {
+ }
+
+ SolverType_t solverType_;
+ bool useGeneratedCostGradient_;
+ bool useGeneratedConstraintJacobian_;
+ SnoptSettings snoptSettings_;
+ IpoptSettings ipoptSettings_;
+
+ /**
+ * @brief Prints out settings
+ */
+ void print()
+ {
+ std::cout << "=============================================================" << std::endl;
+ std::cout << "\tNLP Solver Settings: " << std::endl;
+ std::cout << "=============================================================" << std::endl;
+
+ std::cout << "Using nlp solver: " << solverToString[solverType_] << std::endl;
+ if (useGeneratedCostGradient_)
+ std::cout << "Using generated Cost Gradient" << std::endl;
+ else
+ std::cout << "Using analytical cost Gradient" << std::endl;
+ if (useGeneratedConstraintJacobian_)
+ std::cout << "Using generated Constraints Jacobian" << std::endl;
+ else
+ std::cout << "Using anlyitical Constraints Jacobian" << std::endl;
+
+ if (solverType_ == IPOPT)
+ ipoptSettings_.print();
+ else if (solverType_ == SNOPT)
+ snoptSettings_.print();
+ }
+
+ /**
+ * @brief Checks whether to settings are filled with meaningful values
+ *
+ * @return Returns true of the parameters are ok
+ */
+ bool parametersOk() const
+ {
+ if (solverType_ == IPOPT)
+ return ipoptSettings_.parametersOk();
+ else if (solverType_ == SNOPT)
+ return snoptSettings_.parametersOk();
+ else
+ return false;
+ }
+
+ /**
+ * @brief Loads the settings from a .info file
+ *
+ * @param[in] filename The filename
+ * @param[in] verbose True if parameters to be printed out
+ * @param[in] ns The namespace in the .info fil
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "solver")
+ {
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ solverType_ = (SolverType)pt.get<unsigned int>(ns + ".SolverType");
+ useGeneratedCostGradient_ = pt.get<bool>(ns + ".UseGeneratedCostGradient");
+ useGeneratedConstraintJacobian_ = pt.get<bool>(ns + ".UseGeneratedConstraintJacobian");
+
+ if (solverType_ == IPOPT)
+ ipoptSettings_.load(filename, verbose, ns + ".ipopt");
+ else if (solverType_ == SNOPT)
+ snoptSettings_.load(filename, verbose, ns + ".snopt");
+
+ if (verbose)
+ {
+ std::cout << "Loaded NLP Solver settings from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+
+private:
+ std::map<SolverType, std::string> solverToString = {{IPOPT, "IPOPT"}, {SNOPT, "SNOPT"}};
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/SnoptSolver.h b/ct_optcon/include/ct/optcon/nlp/solver/SnoptSolver.h
new file mode 100644
index 0000000..fb73350
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/SnoptSolver.h
@@ -0,0 +1,314 @@
+/**********************************************************************************************************************
+Copyright (c) 2016, Agile & Dexterous Robotics Lab, ETH ZURICH. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name of ETH ZURICH nor the names of its contributors may be used
+ to endorse or promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**********************************************************************************************************************/
+
+
+#pragma once
+
+
+#include <ct/optcon/nlp/Nlp.h>
+#include "NlpSolver.h"
+
+#ifdef BUILD_WITH_SNOPT_SUPPORT // build SNOPT interface
+#include <snoptProblem.hpp>
+#endif //BUILD_WITH_SNOPT_SUPPORT
+
+namespace ct {
+namespace optcon {
+
+#ifdef BUILD_WITH_SNOPT_SUPPORT // build SNOPT interface
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Forward declaration
+ */
+class SnoptSolver;
+
+
+/**
+ * @ingroup NLP
+ *
+ * @brief Contains all the dynamically allocated memory for SNOPT
+ *
+ * For the implementation see ct/ct_optcon/src/nlp/solver/SnoptSolver.cpp
+ *
+ */
+struct SnoptMemory
+{
+ typedef double Number;
+ const SnoptSolver &self; /*!<A reference the the Snoptsolver the memory points to*/
+
+
+ Number *x_ = nullptr; /*!<Optimization variables*/
+ Number *xlow_ = nullptr; /*!<Lower bound of the optimization variables*/
+ Number *xupp_ = nullptr; /*!<Upper bound of the optimization variables*/
+ Number *xmul_ = nullptr; /*!<The optimization variables multiplier*/
+ int *xstate_ = nullptr; //*!<The state of the optimization variables*/
+
+ Number *F_ = nullptr; /*!<Nonlinear parts of the costfunction and the constraints*/
+ Number *Flow_ = nullptr; /*!<Lower bound on F*/
+ Number *Fupp_ = nullptr; /*!<Upper bound on F*/
+ Number *Fmul_ = nullptr; /*!<The F multiplier*/
+ int *Fstate_ = nullptr; /*!<The F state*/
+
+ Number *A_ = nullptr; /*!<Contains the linear parts of costfunction and the constraints*/
+ int *iAfun_ = nullptr; /*!<Rows of the sparsity pattern of A*/
+ int *jAvar_ = nullptr; /*!<Columns of the sparsity pattern of A*/
+
+ /*!<The sparsity pattern of the costgradient and constraint jacobian*/
+ int *iGfun_ = nullptr; /*!<Sparsity rows*/
+ int *jGvar_ = nullptr; /*!<Sparsity columns*/
+
+ static std::vector<SnoptMemory *> mempool; /*!<Containts all the instances of the snopt memory blocks*/
+ int memind; /*!<The index inside the mempool this instance points to*/
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] self A reference to the Snoptsolver the memory is pointing
+ * to
+ */
+ SnoptMemory(const SnoptSolver &self);
+
+ /// Destructor
+ ~SnoptMemory();
+};
+
+
+/**
+ * @ingroup NLP
+ *
+ * @brief The interface to the NLP solver SNOPT. Currently the SnoptA C++
+ * interface is implemented, which does not require the distinction
+ * between linear and non linear parts of the userfunction
+ */
+class SnoptSolver : public NlpSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef double Number;
+ typedef NlpSolver BASE;
+ typedef Eigen::Matrix<Number, Eigen::Dynamic, 1> VectorXd;
+ typedef Eigen::Map<VectorXd> MapVecXd;
+ typedef Eigen::Map<const VectorXd> MapConstVecXd;
+ typedef Eigen::Map<Eigen::VectorXi> MapVecXi;
+
+ /**
+ * @brief Default constructor
+ */
+ SnoptSolver() {}
+ /**
+ * @brief Destructor, releases the memory
+ */
+ virtual ~SnoptSolver();
+ // {
+ // free_memory(memoryPtr_);
+ // }
+
+
+ /**
+ * @brief Custom constructor
+ *
+ * @param[in] nlp The nlp
+ * @param[in] settings The settings
+ */
+ SnoptSolver(std::shared_ptr<Nlp> nlp, const NlpSolverSettings &settings);
+
+
+ /**
+ * @brief The non static NLP function interfacing with the optimization
+ * module of the control toolbox
+ *
+ * @param m A pointer to the dynamic SNOPT memory
+ * @param Status Indicates the first and last call to the method
+ * @param n The number of optimization variables
+ * @param x The optimization variables
+ * @param needF If true, the method needs to provide f
+ * @param neF The length of the F vector
+ * @param F The F vector, containing the costfunction and the
+ * constraints
+ * @param needG If true, the method needs to provide g
+ * @param neG The length of the G vector
+ * @param G The vector of derivatives of F with respect to the
+ * decision variables
+ * @param cu The character array for external user inputs
+ * @param lencu The length of cu
+ * @param iu The integer array for external user inputs
+ * @param leniu The length of ui
+ * @param ru The real array for external user inputs
+ * @param lenru The length of ru
+ */
+ void NLP_Function(SnoptMemory *m,
+ int *Status,
+ int *n,
+ double x[],
+ int *needF,
+ int *neF,
+ double F[],
+ int *needG,
+ int *neG,
+ double G[],
+ char *cu,
+ int *lencu,
+ int iu[],
+ int *leniu,
+ double ru[],
+ int *lenru) const;
+
+ /**
+ * @brief The static NLP function passed to SNOPT
+ *
+ * @param Status Indicates the first and last call to the method
+ * @param n The number of optimization variables
+ * @param x The optimization variables
+ * @param needF If true, the method needs to provide f
+ * @param neF The length of the F vector
+ * @param F The F vector, containing the costfunction and the
+ * constraints
+ * @param needG If true, the method needs to provide g
+ * @param neG The length of the G vector
+ * @param G The vector of derivatives of F with respect to the
+ * decision variables
+ * @param cu The character array for external user inputs
+ * @param lencu The length of cu
+ * @param iu The integer array for external user inputs
+ * @param leniu The length of ui
+ * @param ru The real array for external user inputs
+ * @param lenru The length of ru
+ */
+ static void NLP_Function(int *Status,
+ int *n,
+ double x[],
+ int *needF,
+ int *neF,
+ double F[],
+ int *needG,
+ int *neG,
+ double G[],
+ char *cu,
+ int *lencu,
+ int iu[],
+ int *leniu,
+ double ru[],
+ int *lenru);
+
+ virtual void configureDerived(const NlpSolverSettings &settings) override;
+
+ virtual bool solve() override;
+
+ virtual void prepareWarmStart(size_t maxIterations) override;
+
+
+private:
+ /**
+ * @brief Allocates memory for the SNOPT interface
+ *
+ * @return Pointer to the memory location
+ */
+ SnoptMemory *alloc_memory() const { return new SnoptMemory(*this); }
+ /**
+ * @brief Frees the allocated memory
+ *
+ * @param[in] mem The memory to be freed
+ */
+ inline void free_memory(SnoptMemory *mem) const { delete mem; }
+ /**
+ * @brief Initializes the memory
+ *
+ * @param mem The memory
+ */
+ void init_memory(SnoptMemory *mem) const;
+
+ /**
+ * @brief Fills the memory with values from NLP, gets called before
+ * solving the NLP
+ *
+ * @param mem The memory
+ */
+ void fill_memory(SnoptMemory *mem) const;
+
+ /**
+ * @brief Provides SNOPT the information from SNOPT memory, gets called
+ * before solving the NLP
+ */
+ void setupSnoptObjects();
+
+ /**
+ * @brief Prints out status messages depending on the snopt status
+ *
+ * @param[in] status The SNOPT solver status
+ */
+ void validateSNOPTStatus(const int &status) const;
+
+ /**
+ * @brief Sets the solver options.
+ */
+ void setSolverOptions();
+
+ SnoptSettings settings_;
+ SnoptMemory *memoryPtr_;
+
+ snoptProblemA snoptApp_;
+
+ int n_ = 0; /*!<Number of optimization variables in optimization problem*/
+ int neF_ = 0; /*!<Number of constraints in optimization problem plus 1 (objective function belongs to constraints)*/
+ int ObjRow_ = 0; /*!<Objective function row*/
+ Number ObjAdd_ = 0.0; /*!<Constant to be added to objective function*/
+
+ const int Cold_ = 0, Basis_ = 1, Warm_ = 2; /*!<Defines the warmstart options for SNOPT*/
+ int currStartOption_ = Cold_; /*!<The option handed to SNOPT*/
+
+
+ int lenA_ =
+ 0; /*!<The dimension of the sparsity pattern for the linear part of the problem. We set it to zero to treat all parts nonlinearly*/
+ int neA_ = 0; /*!<The number of non zeros in A. We set it to zero to treat all parts nonlinearly*/
+
+ int lenG_ = 0; /*!<The dimension of the sparsity pattern for the linear part of the problem. Will be updated later*/
+ int neG_ = 0; /*!<The number of non zeros in G. Will be updated later*/
+
+ int status_ = 0; /*!<The exit status of the SNOPT solver*/
+};
+
+
+#else // BUILD_WITH_SNOPT_SUPPORT -- not building with SNOPT support, create dummy class
+
+class SnoptSolver : public NlpSolver // <STATE_DIM, CONTROL_DIM>
+{
+public:
+ SnoptSolver() { throw(std::runtime_error("Error - SNOPT interface not compiled.")); }
+ SnoptSolver(std::shared_ptr<Nlp> nlp, NlpSolverSettings settings)
+ {
+ throw(std::runtime_error("Error - SNOPT interface not compiled."));
+ }
+
+ virtual void configureDerived(const NlpSolverSettings& settings) override {}
+ virtual bool solve() override { return false; }
+ virtual void prepareWarmStart(size_t maxIterations) override {}
+};
+
+#endif // BUILD_WITH_SNOPT_SUPPORT
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/nlp/solver/implementation/IpoptSolver-impl.h b/ct_optcon/include/ct/optcon/nlp/solver/implementation/IpoptSolver-impl.h
new file mode 100644
index 0000000..45e65ad
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/nlp/solver/implementation/IpoptSolver-impl.h
@@ -0,0 +1,386 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+
+// #define DEBUG_PRINT
+
+
+template <typename SCALAR>
+IpoptSolver<SCALAR>::IpoptSolver(std::shared_ptr<tpl::Nlp<SCALAR>> nlp, const NlpSolverSettings& settings)
+ : BASE(nlp, settings), settings_(BASE::settings_.ipoptSettings_)
+{
+ //Constructor arguments
+ //Argument 1: create console output
+ //Argument 2: create empty
+ ipoptApp_ = std::shared_ptr<Ipopt::IpoptApplication>(new Ipopt::IpoptApplication(true, false));
+}
+
+template <typename SCALAR>
+IpoptSolver<SCALAR>::~IpoptSolver()
+{
+ // Needed to destruct all the IPOPT memory
+ Ipopt::Referencer* t = NULL;
+ this->ReleaseRef(t);
+ delete t;
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::configureDerived(const NlpSolverSettings& settings)
+{
+ std::cout << "calling Ipopt configure derived" << std::endl;
+ settings_ = settings.ipoptSettings_;
+ setSolverOptions();
+ this->isInitialized_ = true;
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::setSolverOptions()
+{
+ ipoptApp_->Options()->SetNumericValue("tol", settings_.tol_);
+ ipoptApp_->Options()->SetNumericValue("constr_viol_tol", settings_.constr_viol_tol_);
+ ipoptApp_->Options()->SetIntegerValue("max_iter", settings_.max_iter_);
+ // ipoptApp_->Options()->SetNumericValue("resto.tol", settings_.restoTol_);
+ // ipoptApp_->Options()->SetNumericValue("acceptable_tol", settings_.acceptableTol_);
+ // ipoptApp_->Options()->SetNumericValue("resto.acceptable_tol", settings_.restoAcceptableTol_);
+ ipoptApp_->Options()->SetStringValueIfUnset("linear_scaling_on_demand", settings_.linear_scaling_on_demand_);
+ ipoptApp_->Options()->SetStringValueIfUnset("hessian_approximation", settings_.hessian_approximation_);
+ // ipoptApp_->Options()->SetStringValueIfUnset("nlp_scaling_method", settings_.nlp_scaling_method_);
+ ipoptApp_->Options()->SetIntegerValue("print_level", settings_.printLevel_); //working now
+ ipoptApp_->Options()->SetStringValueIfUnset("print_user_options", settings_.print_user_options_);
+ // ipoptApp_->Options()->SetIntegerValue("print_frequency_iter", settings_.print_frequency_iter_);
+ ipoptApp_->Options()->SetStringValueIfUnset("derivative_test", settings_.derivativeTest_);
+ ipoptApp_->Options()->SetIntegerValue("print_level", settings_.printLevel_);
+ ipoptApp_->Options()->SetNumericValue("derivative_test_tol", settings_.derivativeTestTol_);
+ ipoptApp_->Options()->SetNumericValue("derivative_test_perturbation", settings_.derivativeTestPerturbation_);
+ ipoptApp_->Options()->SetNumericValue("point_perturbation_radius", settings_.point_perturbation_radius_);
+ ipoptApp_->Options()->SetStringValueIfUnset("linear_system_scaling", settings_.linearSystemScaling_);
+ ipoptApp_->Options()->SetStringValueIfUnset("linear_solver", settings_.linear_solver_);
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::solve()
+{
+ status_ = ipoptApp_->Initialize();
+ if (!(status_ == Ipopt::Solve_Succeeded) && !this->isInitialized_)
+ throw(std::runtime_error("NLP initialization failed"));
+
+ // Ask Ipopt to solve the problem
+ status_ = ipoptApp_->OptimizeTNLP(this);
+
+ if (status_ == Ipopt::Solve_Succeeded || status_ == Ipopt::Solved_To_Acceptable_Level)
+ {
+ // Retrieve some statistics about the solve
+ Ipopt::Index iter_count = ipoptApp_->Statistics()->IterationCount();
+ std::cout << std::endl
+ << std::endl
+ << "*** The problem solved in " << iter_count << " iterations!" << std::endl;
+
+ SCALAR final_obj = ipoptApp_->Statistics()->FinalObjective();
+ std::cout << std::endl
+ << std::endl
+ << "*** The final value of the objective function is " << final_obj << '.' << std::endl;
+ return true;
+ }
+ else
+ {
+ std::cout << " ipopt return value: " << status_ << std::endl;
+ return false;
+ }
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::prepareWarmStart(size_t maxIterations)
+{
+ ipoptApp_->Options()->SetStringValue("warm_start_init_point", "yes");
+ ipoptApp_->Options()->SetNumericValue("warm_start_bound_push", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_bound_frac", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_slack_bound_frac", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_slack_bound_push", 1e-9);
+ ipoptApp_->Options()->SetNumericValue("warm_start_mult_bound_push", 1e-9);
+ ipoptApp_->Options()->SetIntegerValue("max_iter", (int)maxIterations);
+ ipoptApp_->Options()->SetStringValue("derivative_test", "none");
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::get_nlp_info(Ipopt::Index& n,
+ Ipopt::Index& m,
+ Ipopt::Index& nnz_jac_g,
+ Ipopt::Index& nnz_h_lag,
+ IndexStyleEnum& index_style)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_nlp_info()" << std::endl;
+#endif
+ n = this->nlp_->getVarCount();
+ assert(n == n);
+
+ // std::cout << 1 << std::endl;
+
+ m = this->nlp_->getConstraintsCount();
+ assert(m == m);
+
+ nnz_jac_g = this->nlp_->getNonZeroJacobianCount();
+ assert(nnz_jac_g == nnz_jac_g);
+
+ if (settings_.hessian_approximation_ == "exact")
+ nnz_h_lag = this->nlp_->getNonZeroHessianCount();
+
+ index_style = Ipopt::TNLP::C_STYLE;
+
+#ifdef DEBUG_PRINT
+ std::cout << "... number of decision variables = " << n << std::endl;
+ std::cout << "... number of constraints = " << m << std::endl;
+ std::cout << "... nonzeros in jacobian = " << nnz_jac_g << std::endl;
+#endif
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::get_bounds_info(Ipopt::Index n,
+ SCALAR* x_l,
+ SCALAR* x_u,
+ Ipopt::Index m,
+ SCALAR* g_l,
+ SCALAR* g_u)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_bounds_info()" << std::endl;
+#endif //DEBUG_PRINT
+ MapVecXs x_lVec(x_l, n);
+ MapVecXs x_uVec(x_u, n);
+ MapVecXs g_lVec(g_l, m);
+ MapVecXs g_uVec(g_u, m);
+ this->nlp_->getVariableBounds(x_lVec, x_uVec, n);
+ // bounds on optimization vector
+ // x_l <= x <= x_u
+ assert(x_l == x_l);
+ assert(x_u == x_u);
+ assert(n == n);
+
+ // constraints bounds (e.g. for equality constraints = 0)
+ this->nlp_->getConstraintBounds(g_lVec, g_uVec, m);
+ assert(g_l == g_l);
+ assert(g_u == g_u);
+
+#ifdef DEBUG_PRINT
+ std::cout << "... Leaving get_bounds_info()" << std::endl;
+#endif //DEBUG_PRINT
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::get_starting_point(Ipopt::Index n,
+ bool init_x,
+ SCALAR* x,
+ bool init_z,
+ SCALAR* z_L,
+ SCALAR* z_U,
+ Ipopt::Index m,
+ bool init_lambda,
+ SCALAR* lambda)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_starting_point()" << std::endl;
+#endif //DEBUG_PRINT
+ //
+ if (init_x)
+ {
+ MapVecXs xVec(x, n);
+ this->nlp_->getInitialGuess(n, xVec);
+ }
+
+ if (init_z)
+ {
+ MapVecXs z_lVec(z_L, n);
+ MapVecXs z_uVec(z_U, n);
+ this->nlp_->getBoundMultipliers(n, z_lVec, z_uVec);
+ }
+
+ if (init_lambda)
+ {
+ MapVecXs lambdaVec(lambda, m);
+ this->nlp_->getLambdaVars(m, lambdaVec);
+ }
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "... entering get_starting_point()" << std::endl;
+#endif //DEBUG_PRINT
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR& obj_value)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_f()" << std::endl;
+#endif //DEBUG_PRINT
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ obj_value = this->nlp_->evaluateCostFun();
+ assert(obj_value == obj_value);
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_f()" << std::endl;
+#endif //DEBUG_PRINT
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_grad_f(Ipopt::Index n, const SCALAR* x, bool new_x, SCALAR* grad_f)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_grad_f()" << std::endl;
+#endif //DEBUG_PRINT
+ MapVecXs grad_fVec(grad_f, n);
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ this->nlp_->evaluateCostGradient(n, grad_fVec);
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_grad_f()" << std::endl;
+#endif //DEBUG_PRINT
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_g(Ipopt::Index n, const SCALAR* x, bool new_x, Ipopt::Index m, SCALAR* g)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_g()" << std::endl;
+#endif //DEBUG_PRINT
+ assert(m == this->nlp_->getConstraintsCount());
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ MapVecXs gVec(g, m);
+ this->nlp_->evaluateConstraints(gVec);
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "gVec: " << gVec.transpose() << std::endl;
+ std::cout << "... leaving eval_g()" << std::endl;
+#endif //DEBUG_PRINT
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_jac_g(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ Ipopt::Index m,
+ Ipopt::Index nele_jac,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values)
+{
+ if (values == NULL)
+ {
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_jac_g, values == NULL" << std::endl;
+#endif //DEBUG_PRINT
+ // set indices of nonzero elements of the jacobian
+ Eigen::Map<Eigen::VectorXi> iRowVec(iRow, nele_jac);
+ Eigen::Map<Eigen::VectorXi> jColVec(jCol, nele_jac);
+ this->nlp_->getSparsityPatternJacobian(nele_jac, iRowVec, jColVec);
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_jac_g, values == NULL" << std::endl;
+#endif //DEBUG_PRINT
+ }
+ else
+ {
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_jac_g, values != NULL" << std::endl;
+#endif //DEBUG_PRINT
+ MapVecXs valVec(values, nele_jac);
+ MapConstVecXs xVec(x, n);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ this->nlp_->evaluateConstraintJacobian(nele_jac, valVec);
+
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_jac_g, values != NULL" << std::endl;
+#endif //DEBUG_PRINT
+ }
+
+ return true;
+}
+
+template <typename SCALAR>
+bool IpoptSolver<SCALAR>::eval_h(Ipopt::Index n,
+ const SCALAR* x,
+ bool new_x,
+ SCALAR obj_factor,
+ Ipopt::Index m,
+ const SCALAR* lambda,
+ bool new_lambda,
+ Ipopt::Index nele_hess,
+ Ipopt::Index* iRow,
+ Ipopt::Index* jCol,
+ SCALAR* values)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering eval_h()" << std::endl;
+#endif
+ if (values == NULL)
+ {
+ // return the structure. This is a symmetric matrix, fill the lower left
+ // triangle only.
+ Eigen::Map<Eigen::VectorXi> iRowVec(iRow, nele_hess);
+ Eigen::Map<Eigen::VectorXi> jColVec(jCol, nele_hess);
+ this->nlp_->getSparsityPatternHessian(nele_hess, iRowVec, jColVec);
+ }
+ else
+ {
+ // return the values. This is a symmetric matrix, fill the lower left
+ // triangle only
+ MapVecXs valVec(values, nele_hess);
+ MapConstVecXs xVec(x, n);
+ MapConstVecXs lambdaVec(lambda, m);
+ this->nlp_->extractOptimizationVars(xVec, new_x);
+ this->nlp_->evaluateHessian(nele_hess, valVec, obj_factor, lambdaVec);
+ }
+
+// only needed if quasi-newton approximation is not used, hence set to -1 (not used)!
+// ATTENTION: for hard coding of the hessian, one only needs the lower left corner (since it is symmetric) - IPOPT knows that
+//nnz_h_lag = -1;
+
+#ifdef DEBUG_PRINT
+ std::cout << "... leaving eval_h()" << std::endl;
+#endif
+
+ return true;
+}
+
+template <typename SCALAR>
+void IpoptSolver<SCALAR>::finalize_solution(Ipopt::SolverReturn status,
+ Ipopt::Index n,
+ const SCALAR* x,
+ const SCALAR* z_L,
+ const SCALAR* z_U,
+ Ipopt::Index m,
+ const SCALAR* g,
+ const SCALAR* lambda,
+ SCALAR obj_value,
+ const Ipopt::IpoptData* ip_data,
+ Ipopt::IpoptCalculatedQuantities* ip_cq)
+{
+#ifdef DEBUG_PRINT
+ std::cout << "... entering finalize_solution() ..." << std::endl;
+#endif
+ MapConstVecXs xVec(x, n);
+ MapConstVecXs zLVec(z_L, n);
+ MapConstVecXs zUVec(z_U, n);
+ MapConstVecXs lambdaVec(lambda, m);
+
+ this->nlp_->extractIpoptSolution(xVec, zLVec, zUVec, lambdaVec);
+}
diff --git a/ct_optcon/include/ct/optcon/optcon-prespec.h b/ct_optcon/include/ct/optcon/optcon-prespec.h
new file mode 100644
index 0000000..0a1f139
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/optcon-prespec.h
@@ -0,0 +1,51 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_OPTCON_OPTCON_H_
+#define INCLUDE_CT_OPTCON_OPTCON_H_
+
+#include <ct/core/core-prespec.h>
+
+#include "costfunction/costfunction.hpp"
+#include "costfunction/costfunction-impl.hpp" //temporary solution
+
+#include "constraint/constraint.h"
+
+#include "problem/OptConProblem.h"
+#include "problem/LQOCProblem.hpp"
+#include "solver/OptConSolver.h"
+
+#include "nloc/NLOCBackendBase.hpp"
+#include "nloc/NLOCBackendST.hpp"
+#include "nloc/NLOCBackendMP.hpp"
+#include "nloc/algorithms/gnms/GNMS.hpp"
+#include "nloc/algorithms/ilqr/iLQR.hpp"
+
+#include "solver/lqp/HPIPMInterface.hpp"
+#include "solver/lqp/GNRiccatiSolver.hpp"
+#include "solver/NLOptConSolver.hpp"
+#include "solver/NLOptConSettings.hpp"
+
+#include "lqr/riccati/CARE.hpp"
+#include "lqr/riccati/DARE.hpp"
+#include "lqr/FHDTLQR.hpp"
+#include "lqr/LQR.hpp"
+
+#include "dms/dms.h"
+
+#include "mpc/MpcSettings.h"
+#include "mpc/MPC.h"
+#include "mpc/timehorizon/MpcTimeHorizon.h"
+#include "mpc/policyhandler/PolicyHandler.h"
+#include "mpc/policyhandler/default/StateFeedbackPolicyHandler.h"
+
+/*!
+ * \warning{do not include implementation files in optcon-prespec.h}
+ */
+
+// keep standard header guard (easy debugging)
+// header guard is identical to the one in optcon.h
+#endif /* INCLUDE_CT_OPTCON_OPTCON_H_ */
diff --git a/ct_optcon/include/ct/optcon/optcon.h b/ct_optcon/include/ct/optcon/optcon.h
new file mode 100644
index 0000000..224a7d3
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/optcon.h
@@ -0,0 +1,83 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#ifndef INCLUDE_CT_OPTCON_OPTCON_H_
+#define INCLUDE_CT_OPTCON_OPTCON_H_
+
+#include <ct/core/core.h>
+
+#include "costfunction/costfunction.hpp"
+
+#include "constraint/constraint.h"
+
+#include "problem/OptConProblem.h"
+#include "problem/LQOCProblem.hpp"
+#include "solver/OptConSolver.h"
+
+#include "nloc/NLOCBackendBase.hpp"
+#include "nloc/NLOCBackendST.hpp"
+#include "nloc/NLOCBackendMP.hpp"
+#include "nloc/algorithms/gnms/GNMS.hpp"
+#include "nloc/algorithms/ilqr/iLQR.hpp"
+
+#include "solver/lqp/HPIPMInterface.hpp"
+#include "solver/lqp/GNRiccatiSolver.hpp"
+#include "solver/NLOptConSolver.hpp"
+#include "solver/NLOptConSettings.hpp"
+
+#include "lqr/riccati/CARE.hpp"
+#include "lqr/riccati/DARE.hpp"
+#include "lqr/FHDTLQR.hpp"
+#include "lqr/LQR.hpp"
+
+#include "dms/dms.h"
+
+#include "mpc/MpcSettings.h"
+#include "mpc/MPC.h"
+#include "mpc/timehorizon/MpcTimeHorizon.h"
+#include "mpc/policyhandler/PolicyHandler.h"
+#include "mpc/policyhandler/default/StateFeedbackPolicyHandler.h"
+
+
+// implementations
+//costfunction
+#include "costfunction/costfunction-impl.hpp"
+
+//constraints
+#include "constraint/constraint-impl.h"
+
+//problem
+#include "problem/OptConProblem-impl.h"
+#include "problem/LQOCProblem-impl.hpp"
+
+//solver
+#include "solver/lqp/GNRiccatiSolver-impl.hpp"
+#include "solver/lqp/HPIPMInterface-impl.hpp"
+#include "solver/NLOptConSolver-impl.hpp"
+
+//lqr
+#include "lqr/riccati/CARE-impl.hpp"
+#include "lqr/riccati/DARE-impl.hpp"
+#include "lqr/FHDTLQR-impl.hpp"
+#include "lqr/LQR-impl.hpp"
+
+//nloc
+#include "nloc/NLOCBackendBase-impl.hpp"
+#include "nloc/NLOCBackendST-impl.hpp"
+#include "nloc/NLOCBackendMP-impl.hpp"
+#include "nloc/algorithms/gnms/GNMS-impl.hpp"
+#include "nloc/algorithms/ilqr/iLQR-impl.hpp"
+
+//mpc
+#include "mpc/MPC-impl.h"
+#include "mpc/timehorizon/MpcTimeHorizon-impl.h"
+#include "mpc/policyhandler/PolicyHandler-impl.h"
+#include "mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h"
+
+
+// keep standard header guard (easy debugging)
+// header guard is identical to the one in optcon-prespec.h
+#endif /* INCLUDE_CT_OPTCON_OPTCON_H_ */
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
new file mode 100644
index 0000000..d59428a
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp
@@ -0,0 +1,245 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::LQOCProblem(int N)
+ : hasBoxConstraints_(false), // by default, we assume the problem ins unconstrained
+ hasGenConstraints_(false)
+{
+ changeNumStages(N);
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isConstrained() const
+{
+ return (isBoxConstrained() | isGeneralConstrained());
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isBoxConstrained() const
+{
+ return hasBoxConstraints_;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+bool LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::isGeneralConstrained() const
+{
+ return hasGenConstraints_;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+int LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getNumberOfStages()
+{
+ return K_;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumStages(int N)
+{
+ K_ = N;
+
+ A_.resize(N);
+ B_.resize(N);
+ b_.resize(N + 1);
+
+ x_.resize(N + 1);
+ u_.resize(N);
+
+ P_.resize(N);
+ q_.resize(N + 1);
+ qv_.resize(N + 1);
+ Q_.resize(N + 1);
+
+ rv_.resize(N);
+ R_.resize(N);
+
+ ux_lb_.resize(N + 1);
+ ux_ub_.resize(N + 1);
+ ux_I_.resize(N + 1);
+ nb_.resize(N + 1);
+
+ ng_.resize(N + 1);
+ d_lb_.resize(N + 1);
+ d_ub_.resize(N + 1);
+ C_.resize(N + 1);
+ D_.resize(N + 1);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setZero(const int& nGenConstr)
+{
+ A_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Zero());
+ B_.setConstant(core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero());
+ b_.setConstant(core::StateVector<STATE_DIM, SCALAR>::Zero());
+ x_.setConstant(core::StateVector<STATE_DIM, SCALAR>::Zero());
+ u_.setConstant(core::ControlVector<CONTROL_DIM, SCALAR>::Zero());
+ P_.setConstant(core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>::Zero());
+ qv_.setConstant(core::StateVector<STATE_DIM, SCALAR>::Zero());
+ Q_.setConstant(core::StateMatrix<STATE_DIM, SCALAR>::Zero());
+ rv_.setConstant(core::ControlVector<CONTROL_DIM, SCALAR>::Zero());
+ R_.setConstant(core::ControlMatrix<CONTROL_DIM, SCALAR>::Zero());
+ q_.setConstant((SCALAR)0.0);
+
+ // reset the number of box constraints
+ std::fill(nb_.begin(), nb_.end(), 0);
+
+ assert(d_lb_.size() == d_ub_.size());
+ assert(d_lb_.size() == C_.size());
+ assert(d_lb_.size() == D_.size());
+ std::fill(ng_.begin(), ng_.end(), 0);
+ for (size_t i = 0; i < d_lb_.size(); i++)
+ {
+ d_lb_[i].resize(nGenConstr, 1);
+ d_lb_[i].setZero();
+ d_ub_[i].resize(nGenConstr, 1);
+ d_ub_[i].setZero();
+ C_[i].resize(nGenConstr, STATE_DIM);
+ C_[i].setZero();
+ D_[i].resize(nGenConstr, CONTROL_DIM);
+ D_[i].setZero();
+ }
+
+ hasBoxConstraints_ = false;
+ hasGenConstraints_ = false;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateBoxConstraint(const int index,
+ const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp)
+{
+ if ((ux_lb.rows() != ux_ub.rows()) | (ux_lb.size() != nConstr) | (sp.rows() != nConstr) |
+ (sp(sp.rows() - 1) > (STATE_DIM + CONTROL_DIM - 1)))
+ {
+ std::cout << "n.o. constraints : " << nConstr << std::endl;
+ std::cout << "ux_lb : " << ux_lb.transpose() << std::endl;
+ std::cout << "ux_ub : " << ux_ub.transpose() << std::endl;
+ std::cout << "sparsity : " << sp.transpose() << std::endl;
+ throw(std::runtime_error("LQOCProblem setIntermediateBoxConstraint: error in constraint config"));
+ }
+
+ if (index >= K_)
+ throw(std::runtime_error("LQOCProblem cannot set an intermediate Box constraint at time >= K_"));
+
+ nb_[index] = nConstr;
+ ux_lb_[index].template topRows(nConstr) = ux_lb;
+ ux_ub_[index].template topRows(nConstr) = ux_ub;
+ ux_I_[index].template topRows(nConstr) = sp;
+
+ hasBoxConstraints_ = true;
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setIntermediateBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp)
+{
+ for (int i = 0; i < K_; i++)
+ setIntermediateBoxConstraint(i, nConstr, ux_lb, ux_ub, sp);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setTerminalBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp)
+{
+ if (nConstr > 0)
+ {
+ if ((ux_lb.rows() != ux_ub.rows()) | (ux_lb.size() != nConstr) | (sp.rows() != nConstr) |
+ (sp(sp.rows() - 1) > (STATE_DIM - 1)))
+ {
+ std::cout << "n.o. constraints : " << nConstr << std::endl;
+ std::cout << "ux_lb : " << ux_lb.transpose() << std::endl;
+ std::cout << "ux_ub : " << ux_ub.transpose() << std::endl;
+ std::cout << "sparsity : " << sp.transpose() << std::endl;
+ throw(std::runtime_error("LQOCProblem setTerminalBoxConstraint: error in constraint config"));
+ }
+
+ nb_[K_] = nConstr;
+ ux_lb_[K_].template topRows(nConstr) = ux_lb;
+ ux_ub_[K_].template topRows(nConstr) = ux_ub;
+ ux_I_[K_].template topRows(nConstr) = sp;
+
+ hasBoxConstraints_ = true;
+ }
+}
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setGeneralConstraints(const constr_vec_t& d_lb,
+ const constr_vec_t& d_ub,
+ const constr_state_jac_t& C,
+ const constr_control_jac_t& D)
+{
+ d_lb_.setConstant(d_lb);
+ d_ub_.setConstant(d_ub);
+ C_.setConstant(C);
+ D_.setConstant(D);
+
+ hasGenConstraints_ = true;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR>
+void LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setFromTimeInvariantLinearQuadraticProblem(
+ ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>& u0,
+ ct::core::DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>& linearSystem,
+ ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>& costFunction,
+ ct::core::StateVector<STATE_DIM, SCALAR>& stateOffset,
+ double dt)
+{
+ setZero();
+
+ core::StateMatrix<STATE_DIM, SCALAR> A;
+ core::StateControlMatrix<STATE_DIM, CONTROL_DIM, SCALAR> B;
+ linearSystem.getAandB(x0, u0, 0, A, B);
+
+ A_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_, A);
+ B_ = core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, B);
+ b_ = core::StateVectorArray<STATE_DIM, SCALAR>(K_ + 1, stateOffset);
+
+
+ // feed current state and control to cost function
+ costFunction.setCurrentStateAndControl(x0, u0, 0);
+
+ // derivative of cost with respect to state
+ qv_ = core::StateVectorArray<STATE_DIM, SCALAR>(K_ + 1, costFunction.stateDerivativeIntermediate() * dt);
+ Q_ = core::StateMatrixArray<STATE_DIM, SCALAR>(K_ + 1, costFunction.stateSecondDerivativeIntermediate() * dt);
+
+ // derivative of cost with respect to control and state
+ P_ =
+ core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>(K_, costFunction.stateControlDerivativeIntermediate() * dt);
+
+ // derivative of cost with respect to control
+ rv_ = core::ControlVectorArray<CONTROL_DIM, SCALAR>(K_, costFunction.controlDerivativeIntermediate() * dt);
+
+ R_ = core::ControlMatrixArray<CONTROL_DIM, SCALAR>(K_, costFunction.controlSecondDerivativeIntermediate() * dt);
+
+ Q_[K_] = costFunction.stateSecondDerivativeTerminal();
+ qv_[K_] = costFunction.stateDerivativeTerminal();
+
+ x_ = core::StateVectorArray<STATE_DIM, SCALAR>(K_ + 1, x0);
+ u_ = core::ControlVectorArray<CONTROL_DIM, SCALAR>(K_, u0);
+
+ hasBoxConstraints_ = false;
+ hasGenConstraints_ = false;
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
new file mode 100644
index 0000000..2eb5572
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp
@@ -0,0 +1,230 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * \brief Defines a Linear-Quadratic Optimal Control Problem, which is optionally constrained.
+ *
+ * This class defines a Linear Quadratic Optimal Control (LQOC) Problem, consisting of
+ * - affine system dynamics
+ * - reference trajectories (arrays!) for state and control
+ * - LQ approximation of the cost function
+ *
+ * The unconstrained LQ problem hence has the following form:
+ * \f[
+ * \min_{\delta \mathbf{u}_n, \delta \mathbf{x}_n}
+ * \bigg \{
+ * q_N +\delta \mathbf{x}_N^\top \mathbf{q}_N +\frac{1}{2}\delta \mathbf{x}_N^\top \mathbf{Q}_N\delta \mathbf{x}_N
+ * +\sum_{n=0}^{N-1} q_n + \delta \mathbf{x}_n^\top \mathbf{q}_n
+ * + \delta \mathbf{u}_n^\top \mathbf{r}_n
+ * + \frac{1}{2}\delta \mathbf{x}_n^\top\mathbf{Q}_n\delta \mathbf{x}_n
+ * +\frac{1}{2}\delta \mathbf{u}_n^\top \mathbf{R}_n\delta \mathbf{u}_n
+ * + \delta \mathbf{u}_n^\top \mathbf{P}_n\delta \mathbf{x}_n
+ * \bigg \}
+ * \f]
+ * subject to
+ * \f[
+ * \delta \mathbf x_{n+1} = \mathbf A_n \delta \mathbf x_n + \mathbf B_n \delta \mathbf u_n +\mathbf b_n
+ * \f]
+ * with
+ * \f$ \delta \mathbf x_n = \mathbf x_n - \hat \mathbf x_n \f$ and \f$ \delta \mathbf u_n = \mathbf u_n - \hat \mathbf u_n \f$
+ *
+ * The reference trajectories for state and control are here denoted as \f$ \hat \mathbf x_i, \
+ * \hat \mathbf u_i \quad \forall i = 0, 1, \ldots \f$
+ *
+ * The constrained LQ problem additionally implements the box constraints
+ * \f$ \mathbf x_{lb} \leq \mathbf x_n \leq \mathbf x_{ub} \ \forall i=1,2,\ldots,N \f$
+ * and
+ * \f$ \mathbf u_{lb} \leq \mathbf u_n \leq \mathbf u_{ub} \ \forall i=0,1,\ldots,N-1\f$
+ * and the general inequality constraints
+ * \f[
+ * \mathbf d_{lb} \leq \mathbf C_n \mathbf x_n + \mathbf D_n \mathbf u_n \leq \mathbf d_{ub} \ \forall i=0,1,\ldots,N
+ * \f]
+ * which are both always kept in absolute coordinates.
+ *
+ * \note The box constraint containers within this class are made fixed-size. Solvers can get the
+ * actual number of box constraints from a a dedicated container nb_
+ *
+ * \todo refactor all to be in global coordinates
+ * \todo Refactor the initializing methods such that const-references can be handed over.
+ */
+template <int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
+class LQOCProblem
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ using constr_vec_t = Eigen::Matrix<SCALAR, -1, -1>;
+ using constr_state_jac_t = Eigen::Matrix<SCALAR, -1, -1>;
+ using constr_control_jac_t = Eigen::Matrix<SCALAR, -1, -1>;
+
+ using constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t>;
+ using constr_state_jac_array_t = ct::core::DiscreteArray<constr_state_jac_t>;
+ using constr_control_jac_array_t = ct::core::DiscreteArray<constr_control_jac_t>;
+
+ using box_constr_t = Eigen::Matrix<SCALAR, STATE_DIM + CONTROL_DIM, 1>;
+ using box_constr_array_t = ct::core::DiscreteArray<box_constr_t>;
+
+ //! a vector indicating which box constraints are active and which not
+ using box_constr_sparsity_t = Eigen::Matrix<int, STATE_DIM + CONTROL_DIM, 1>;
+ using box_constr_sparsity_array_t = ct::core::DiscreteArray<box_constr_sparsity_t>;
+
+ using VectorXi = Eigen::VectorXi;
+
+ //! constructor
+ LQOCProblem(int N = 0);
+
+ //! returns the number of discrete time steps in the LOCP, including terminal stage
+ int getNumberOfStages();
+
+ //! change the number of discrete time steps in the LOCP
+ void changeNumStages(int N);
+
+ /*!
+ * @brief set all member variables to zero
+ * @param nGenConstr by default, we resize the general constraint containers to zero
+ */
+ void setZero(const int& nGenConstr = 0);
+
+ /*!
+ * \brief set intermediate box constraints at a specific index
+ * @param index the index
+ * @param nConstr the number of constraints
+ * @param ux_lb control lower bound in absolute coordinates, active bounds ordered as [u x]
+ * @param ux_ub control upper bound in absolute coordinates, active bounds ordered as [u x]
+ * @param sp the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
+ */
+ void setIntermediateBoxConstraint(const int index,
+ const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp);
+
+ /*!
+ * \brief set uniform box constraints, with the same constraint being applied at each intermediate stage
+ * @param nConstr the number of constraints
+ * @param ux_lb control lower bound in absolute coordinates, active bounds ordered as [u x]
+ * @param ux_ub control upper bound in absolute coordinates, active bounds ordered as [u x]
+ * @param sp the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
+ */
+ void setIntermediateBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp);
+
+ /*!
+ * \brief set box constraints for terminal stage
+ * @param nConstr the number of constraints
+ * @param ux_lb control lower bound in absolute coordinates, active bounds ordered as [u x]
+ * @param ux_ub control upper bound in absolute coordinates, active bounds ordered as [u x]
+ * @param sp the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
+ */
+ void setTerminalBoxConstraints(const int nConstr,
+ const constr_vec_t& ux_lb,
+ const constr_vec_t& ux_ub,
+ const VectorXi& sp);
+
+ /*!
+ * \brief set general (in)equaltiy constraints, with the same constraint applied at each stage
+ * @param d_lb general constraint lower bound
+ * @param d_ub general constraint upper bound
+ * @param C general constraint state jacobian
+ * @param D general constraint control jacobian
+ */
+ void setGeneralConstraints(const constr_vec_t& d_lb,
+ const constr_vec_t& d_ub,
+ const constr_state_jac_t& C,
+ const constr_control_jac_t& D);
+
+ /*!
+ * \brief a convenience method which constructs an unconstrained LQOC Problem from an LTI system and continuous-time quadratic cost
+ * The discretization of the cost functions happens within this function. It employs an Euler-Discretization
+ * @param x0 the initial state
+ * @param u0 the current (and desired control)
+ * @param linearSystem the discrete-time LTI system
+ * @param costFunction the continuous-time cost function
+ * @param stateOffset the offset for the affine system dynamics demanded by the LQOC Solver
+ * @param dt the sampling time, required for discretization
+ */
+ void setFromTimeInvariantLinearQuadraticProblem(ct::core::StateVector<STATE_DIM, SCALAR>& x0,
+ ct::core::ControlVector<CONTROL_DIM, SCALAR>& u0,
+ ct::core::DiscreteLinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>& linearSystem,
+ ct::optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>& costFunction,
+ ct::core::StateVector<STATE_DIM, SCALAR>& stateOffset,
+ double dt);
+
+ //! return a flag indicating whether this LQOC Problem is constrained or not
+ bool isConstrained() const;
+
+ bool isBoxConstrained() const;
+ bool isGeneralConstrained() const;
+
+ //! affine, time-varying system dynamics in discrete time
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> A_;
+ ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> B_;
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> b_;
+
+ //! reference state trajectory
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x_;
+
+ //! reference control trajectory
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u_;
+
+ //! constant term of in the LQ approximation of the cost function
+ ct::core::ScalarArray<SCALAR> q_;
+
+ //! LQ approximation of the pure state penalty, including terminal state penalty
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> qv_;
+ ct::core::StateMatrixArray<STATE_DIM, SCALAR> Q_;
+
+ //! LQ approximation of the pure control penalty
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> rv_;
+ ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR> R_;
+
+ //! LQ approximation of the cross terms of the cost function
+ ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> P_;
+
+ //! lower bound of box constraints in order [u_lb; x_lb]. Stacked for memory efficiency.
+ box_constr_array_t ux_lb_;
+ //! upper bound of box constraints in order [u_ub; x_ub]. Stacked for memory efficiency.
+ box_constr_array_t ux_ub_;
+ /*!
+ * \brief container for the box constraint sparsity pattern
+ * An example for how an element of this array might look like: [0 1 4 7]
+ * This would mean that box constraints act on elements 0, 1, 4 and 7 of the
+ * combined vector of decision variables [u; x]
+ */
+ box_constr_sparsity_array_t ux_I_;
+ //! the number of box constraints at every stage.
+ std::vector<int> nb_;
+
+ //! general constraint lower bound
+ constr_vec_array_t d_lb_;
+ //! general constraint upper bound
+ constr_vec_array_t d_ub_;
+ //! linear general constraint matrices
+ constr_state_jac_array_t C_;
+ constr_control_jac_array_t D_;
+ //! number of general inequality constraints
+ std::vector<int> ng_;
+
+ //! bool indicating if the optimization problem is box-constrained
+ bool hasBoxConstraints_;
+
+ //! bool indicating if the optimization problem hs general inequality constraints
+ bool hasGenConstraints_;
+
+private:
+ //! the number of discrete time steps in the LOCP, including terminal stage
+ int K_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/problem/OptConProblem-impl.h b/ct_optcon/include/ct/optcon/problem/OptConProblem-impl.h
new file mode 100644
index 0000000..509f95c
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/OptConProblem-impl.h
@@ -0,0 +1,202 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem()
+{
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ LinearPtr_t linearSystem)
+ : tf_(0.0),
+ x0_(state_vector_t::Zero()),
+ controlledSystem_(nonlinDynamics),
+ costFunction_(costFunction),
+ linearizedSystem_(linearSystem),
+ boxConstraints_(nullptr),
+ generalConstraints_(nullptr)
+{
+ if (linearSystem == nullptr) // no linearization provided
+ {
+ linearizedSystem_ = std::shared_ptr<core::SystemLinearizer<STATE_DIM, CONTROL_DIM, SCALAR>>(
+ new core::SystemLinearizer<STATE_DIM, CONTROL_DIM, SCALAR>(controlledSystem_));
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ LinearPtr_t linearSystem)
+ : OptConProblem(nonlinDynamics, costFunction, linearSystem) // delegating constructor
+{
+ tf_ = tf;
+ x0_ = x0;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem)
+ : OptConProblem(nonlinDynamics, costFunction, linearSystem) // delegating constructor
+{
+ boxConstraints_ = boxConstraints;
+ generalConstraints_ = generalConstraints;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem)
+ : OptConProblem(nonlinDynamics,
+ costFunction,
+ boxConstraints,
+ generalConstraints,
+ linearSystem) // delegating constructor
+{
+ tf_ = tf;
+ x0_ = x0;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::verify() const
+{
+ if (!controlledSystem_)
+ {
+ throw std::runtime_error("Dynamic system not set");
+ }
+ if (!linearizedSystem_)
+ {
+ throw std::runtime_error("Linearized system not set");
+ }
+ if (!costFunction_)
+ {
+ throw std::runtime_error("Cost function not set");
+ }
+ if (tf_ < 0.0)
+ {
+ throw std::runtime_error("Time horizon should not be negative");
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::DynamicsPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getNonlinearSystem() const
+{
+ return controlledSystem_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::LinearPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getLinearSystem() const
+{
+ return linearizedSystem_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::CostFunctionPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getCostFunction() const
+{
+ return costFunction_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setNonlinearSystem(const DynamicsPtr_t dyn)
+{
+ controlledSystem_ = dyn;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setLinearSystem(const LinearPtr_t lin)
+{
+ linearizedSystem_ = lin;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setCostFunction(const CostFunctionPtr_t cost)
+{
+ costFunction_ = cost;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setBoxConstraints(const ConstraintPtr_t constraint)
+{
+ boxConstraints_ = constraint;
+ if (!boxConstraints_->isInitialized())
+ boxConstraints_->initialize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setGeneralConstraints(const ConstraintPtr_t constraint)
+{
+ generalConstraints_ = constraint;
+ if (!generalConstraints_->isInitialized())
+ generalConstraints_->initialize();
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getBoxConstraints() const
+{
+ return boxConstraints_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::ConstraintPtr_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getGeneralConstraints() const
+{
+ return generalConstraints_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const typename OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::state_vector_t
+OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getInitialState() const
+{
+ return x0_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setInitialState(const state_vector_t& x0)
+{
+ x0_ = x0;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+const SCALAR& OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::getTimeHorizon() const
+{
+ return tf_;
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>::setTimeHorizon(const SCALAR& tf)
+{
+ tf_ = tf;
+}
+
+
+} // optcon
+} // ct
diff --git a/ct_optcon/include/ct/optcon/problem/OptConProblem.h b/ct_optcon/include/ct/optcon/problem/OptConProblem.h
new file mode 100644
index 0000000..96d6b0b
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/problem/OptConProblem.h
@@ -0,0 +1,225 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/constraint/LinearConstraintContainer.h>
+#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/*!
+ * \defgroup OptConProblem OptConProblem
+ *
+ * \brief Class that defines how to set up an Optimal Control Problem
+ *
+ * An finite-horizon optimal control problem is generally defined through
+ * - nonlinear system dynamics
+ * - cost function (intermediate + terminal cost)
+ * - initial state
+ * - box constraints
+ * - general constraints
+ * - an overall time horizon
+ *
+ * Note that in most cases, the user can also provide a pointer to the linearized system dynamics. This is optional, and
+ * in case it is not provided, numerical differentiation will be applied to approximate the linearized dynamics.
+ *
+ * \warning Using numerical differentiation is inefficient and typically slow.
+ *
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class OptConProblem
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ // typedefs
+ typedef ct::core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef std::shared_ptr<core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> DynamicsPtr_t;
+ typedef std::shared_ptr<core::LinearSystem<STATE_DIM, CONTROL_DIM, SCALAR>> LinearPtr_t;
+ typedef std::shared_ptr<optcon::CostFunctionQuadratic<STATE_DIM, CONTROL_DIM, SCALAR>> CostFunctionPtr_t;
+ typedef std::shared_ptr<optcon::LinearConstraintContainer<STATE_DIM, CONTROL_DIM, SCALAR>> ConstraintPtr_t;
+
+ OptConProblem();
+
+ /*!
+ * @brief Construct a simple unconstrained Optimal Control Problem
+ * \warning time and initial state to be specified later
+ *
+ * @param nonlinDynamics the nonlinear system dynamics
+ * @param costFunction a quadratic cost function
+ * @param linearSystem (optional) the linear system holding the dynamics derivatives. If the
+ * user does not specify the derivatives, they are generated automatically using numerical differentiation. Warning: this is slow
+ */
+ OptConProblem(DynamicsPtr_t nonlinDynamics, CostFunctionPtr_t costFunction, LinearPtr_t linearSystem = nullptr);
+
+ /*!
+ * @brief Construct a simple unconstrained optimal control problem, with initial state and final time as constructor arguments
+ * @param tf The optimal control problem final time horizon
+ * @param x0 The initial system state
+ * @param nonlinDynamics The nonlinear system dynamics
+ * @param costFunction A quadratic cost function
+ * @param linearSystem (optional) Linearized System Dynamics.
+ */
+ OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ LinearPtr_t linearSystem = nullptr);
+
+ /*!
+ * @brief Construct a constrained Optimal Control Problem
+ *
+ * @param nonlinDynamics the nonlinear system dynamics
+ * @param costFunction a quadratic cost function
+ * @param boxConstraints the box constraints
+ * @param generalConstraints the general constraints
+ * @param linearSystem (optional) the linear system holding the dynamics derivatives.
+ *
+ * \warning time and initial state to be specified later
+ * \warning If the user does not specify the derivatives, they are generated automatically using numerical differentiation. This is slow
+ */
+ OptConProblem(DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem = nullptr);
+
+ /*!
+ * @brief Construct a constrained Optimal Control Problem
+ *
+ * @param tf The optimal control problem final time horizon
+ * @param x0 The initial system state
+ * @param nonlinDynamics the nonlinear system dynamics
+ * @param costFunction a quadratic cost function
+ * @param boxConstraints the box constraints
+ * @param generalConstraints the general constraints
+ * @param linearSystem (optional) the linear system holding the dynamics derivatives.
+ *
+ * \warning time and initial state to be specified later
+ * \warning If the user does not specify the derivatives, they are generated automatically using numerical differentiation. This is slow
+ */
+ OptConProblem(const SCALAR& tf,
+ const state_vector_t& x0,
+ DynamicsPtr_t nonlinDynamics,
+ CostFunctionPtr_t costFunction,
+ ConstraintPtr_t boxConstraints,
+ ConstraintPtr_t generalConstraints,
+ LinearPtr_t linearSystem = nullptr);
+
+ //! check if all the ingredients for an unconstrained optimal control problem are there
+ void verify() const;
+
+ /*!
+ * returns a pointer to the controlled system
+ * */
+ const DynamicsPtr_t getNonlinearSystem() const;
+
+ /*!
+ * returns a pointer to the linear system approximation
+ * */
+ const LinearPtr_t getLinearSystem() const;
+
+ /*!
+ * returns a pinter to the cost function
+ * */
+ const CostFunctionPtr_t getCostFunction() const;
+
+ /*!
+ * returns a pointer to the controlled system
+ * */
+ void setNonlinearSystem(const DynamicsPtr_t dyn);
+
+ /*!
+ * returns a pointer to the linear system approximation
+ * */
+ void setLinearSystem(const LinearPtr_t lin);
+
+ /*!
+ * returns a pinter to the cost function
+ * */
+ void setCostFunction(const CostFunctionPtr_t cost);
+
+ /*!
+ * set box constraints
+ * @param constraint pointer to box constraint
+ */
+ void setBoxConstraints(const ConstraintPtr_t constraint);
+
+ /*!
+ * set general constraints
+ * @param constraint pointer to a general constraint
+ */
+ void setGeneralConstraints(const ConstraintPtr_t constraint);
+
+ /**
+ * @brief Retrieve the box constraints
+ *
+ * @return The box constraints.
+ */
+ const ConstraintPtr_t getBoxConstraints() const;
+
+ /**
+ * @brief Retrieves the general constraints
+ *
+ * @return The the general constraints
+ */
+ const ConstraintPtr_t getGeneralConstraints() const;
+
+ /*!
+ * get initial state (called by solvers)
+ * */
+ const state_vector_t getInitialState() const;
+
+ /*!
+ * set initial state for first subsystem
+ * */
+ void setInitialState(const state_vector_t& x0);
+
+ /*!
+ * get the current time horizon
+ * @return Time Horizon
+ */
+ const SCALAR& getTimeHorizon() const;
+
+ /*!
+ * Update the current time horizon in the Opt.Control Problem (required for example for replanning)
+ * @param tf new time horizon
+ */
+ void setTimeHorizon(const SCALAR& tf);
+
+
+private:
+ SCALAR tf_; //! end time
+
+ state_vector_t x0_; //! initial state
+
+ DynamicsPtr_t controlledSystem_; //! the nonlinear system
+ CostFunctionPtr_t costFunction_; //! a quadratic cost function
+ LinearPtr_t linearizedSystem_; //! the linear approximation of the nonlinear system
+
+ /*!
+ * @brief container of all the state and input box constraints of the problem
+ * Expected form:
+ * \f$ u_{lb} \leq u \leq u_{ub} \f$ and \f$ x_{lb} \leq x \leq x_{ub} \f$
+ */
+ ConstraintPtr_t boxConstraints_;
+
+ /*!
+ * @brief container of all the general constraints of the problem
+ * Expected form:
+ * \f$ d_{lb} \leq g(x,u) \leq d_{ub} \f$
+ */
+ ConstraintPtr_t generalConstraints_;
+};
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp
new file mode 100644
index 0000000..5eb7c05
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp
@@ -0,0 +1,612 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <map>
+
+#include <boost/property_tree/info_parser.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+//! GNMS Line Search Settings
+/*!
+ * \ingroup GNMS
+ *
+ * The Line Search Settings are part of the general settings struct and hold parameters to customize the line-search for the NLOptCon controller update.
+ */
+struct LineSearchSettings
+{
+ //! default constructor for the NLOptCon line-search settings
+ LineSearchSettings()
+ : active(true),
+ adaptive(false),
+ maxIterations(10),
+ alpha_0(1.0),
+ alpha_max(1.0),
+ n_alpha(0.5),
+ debugPrint(false)
+ {
+ }
+
+ //! check if the currently set line-search parameters are meaningful
+ bool parametersOk() const { return (alpha_0 > 0.0) && (n_alpha > 0.0) && (n_alpha < 1.0) && (alpha_max > 0.0); }
+ bool active; /*!< Flag whether or not to perform line search */
+ bool adaptive; /*!< Flag whether alpha_0 gets updated based on previous iteration */
+ size_t maxIterations; /*!< Maximum number of iterations during line search */
+ double alpha_0; /*!< Initial step size for line search. Use 1 for step size as suggested by NLOptCon */
+ double alpha_max; /*!< Maximum step size for line search. This is the limit when adapting alpha_0. */
+ double
+ n_alpha; /*!< Factor by which the line search step size alpha gets multiplied with after each iteration. Usually 0.5 is a good value. */
+ bool debugPrint; /*!< Print out debug information during line-search*/
+
+
+ //! print the current line search settings to console
+ void print() const
+ {
+ std::cout << "Line Search Settings: " << std::endl;
+ std::cout << "=====================" << std::endl;
+ std::cout << "active:\t" << active << std::endl;
+ std::cout << "adaptive:\t" << adaptive << std::endl;
+ std::cout << "maxIter:\t" << maxIterations << std::endl;
+ std::cout << "alpha_0:\t" << alpha_0 << std::endl;
+ std::cout << "alpha_max:\t" << alpha_max << std::endl;
+ std::cout << "n_alpha:\t" << n_alpha << std::endl;
+ std::cout << "debugPrint:\t" << debugPrint << std::endl;
+ std::cout << " =======" << std::endl;
+ std::cout << std::endl;
+ }
+
+ //! load line search settings from file
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "line_search")
+ {
+ if (verbose)
+ std::cout << "Trying to load line search settings from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ active = pt.get<bool>(ns + ".active");
+ maxIterations = pt.get<size_t>(ns + ".maxIterations");
+ alpha_0 = pt.get<double>(ns + ".alpha_0");
+ n_alpha = pt.get<double>(ns + ".n_alpha");
+ debugPrint = pt.get<bool>(ns + ".debugPrint");
+ alpha_max = alpha_0;
+ adaptive = false;
+
+ try
+ {
+ adaptive = pt.get<bool>(ns + ".adaptive");
+ alpha_max = pt.get<double>(ns + ".alpha_max");
+ } catch (...)
+ {
+ }
+
+ if (verbose)
+ {
+ std::cout << "Loaded line search settings from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+};
+
+
+//! LQOC Solver settings
+/*!
+ * Settings for solving each linear-quadratic (constrained) sub-problem
+ */
+struct LQOCSolverSettings
+{
+public:
+ LQOCSolverSettings() : num_lqoc_iterations(5), lqoc_debug_print(false) {}
+ int num_lqoc_iterations; //! number of allowed sub-iterations of LQOC solver per NLOC main iteration
+ bool lqoc_debug_print;
+
+ void print() const
+ {
+ std::cout << "======================= LQOCSolverSettings =====================" << std::endl;
+ std::cout << "num_lqoc_iterations: \t" << num_lqoc_iterations << std::endl;
+ std::cout << "lqoc_debug_print: \t" << lqoc_debug_print << std::endl;
+ }
+
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "lqoc_solver_settings")
+ {
+ if (verbose)
+ std::cout << "Trying to load LQOCSolverSettings config from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ try
+ {
+ num_lqoc_iterations = pt.get<int>(ns + ".num_lqoc_iterations");
+ } catch (...)
+ {
+ }
+ try
+ {
+ lqoc_debug_print = pt.get<bool>(ns + ".lqoc_debug_print");
+ } catch (...)
+ {
+ }
+ }
+};
+
+
+/*!
+ * \ingroup NLOptCon
+ *
+ * \brief Settings for the NLOptCon algorithm.
+ */
+class NLOptConSettings
+{
+public:
+ typedef typename core::SensitivityApproximationSettings::APPROXIMATION APPROXIMATION;
+
+ //! the nonlinear optimal control problem solving algorithm
+ enum NLOCP_ALGORITHM
+ {
+ GNMS = 0,
+ ILQR,
+ NUM_TYPES
+ };
+
+ //! the linear optimal control problem solver in the background
+ enum LQOCP_SOLVER
+ {
+ GNRICCATI_SOLVER = 0,
+ HPIPM_SOLVER = 1
+ };
+
+
+ //! NLOptCon Settings default constructor
+ /*!
+ * sets all settings to default values.
+ */
+ NLOptConSettings()
+ : integrator(ct::core::IntegrationType::RK4),
+ discretization(APPROXIMATION::BACKWARD_EULER),
+ timeVaryingDiscretization(false),
+ nlocp_algorithm(GNMS),
+ lqocp_solver(GNRICCATI_SOLVER),
+ loggingPrefix("alg"),
+ closedLoopShooting(false), //! by default, we do open-loop shooting
+ epsilon(1e-5),
+ dt(0.001),
+ K_sim(1), //! by default, there is only one sub-integration step
+ K_shot(1), //! by default the shot length is equal to the control length
+ min_cost_improvement(1e-5), //! cost needs to be at least 1e-5 better before we assume convergence
+ maxDefectSum(1e-5),
+ meritFunctionRho(0.0),
+ meritFunctionRhoConstraints(1.0),
+ max_iterations(100),
+ fixedHessianCorrection(false),
+ recordSmallestEigenvalue(false),
+ nThreads(4),
+ nThreadsEigen(4),
+ lineSearchSettings(),
+ debugPrint(false),
+ printSummary(true),
+ useSensitivityIntegrator(false)
+ {
+ }
+
+ ct::core::IntegrationType integrator; //! which integrator to use during the NLOptCon forward rollout
+ APPROXIMATION discretization;
+ bool timeVaryingDiscretization;
+ NLOCP_ALGORITHM nlocp_algorithm; //! which nonlinear optimal control algorithm is to be used
+ LQOCP_SOLVER lqocp_solver; //! the solver for the linear-quadratic optimal control problem
+ std::string loggingPrefix; //! the prefix to be stored before the matfile name for logging
+ bool closedLoopShooting; //! use feedback gains during forward integration (true) or not (false)
+ double epsilon; //! Eigenvalue correction factor for Hessian regularization
+ double dt; //! sampling time for the control input (seconds)
+ int K_sim; //! number of sub-integration-steps
+ int K_shot; //! duration of a shot as an integer multiple of dt
+ double min_cost_improvement; //! minimum cost improvement between two interations to assume convergence
+ double maxDefectSum; //! maximum sum of squared defects (assume covergence if lower than this number)
+ double meritFunctionRho; //! trade off between internal (defect)constraint violation and cost
+ double meritFunctionRhoConstraints; //! trade off between external (general and path) constraint violation and cost
+ int max_iterations; //! the maximum admissible number of NLOptCon main iterations \warning make sure to select this number high enough allow for convergence
+ bool fixedHessianCorrection; //! perform Hessian regularization by incrementing the eigenvalues by epsilon.
+ bool recordSmallestEigenvalue; //! save the smallest eigenvalue of the Hessian
+ int nThreads; //! number of threads, for MP version
+ size_t
+ nThreadsEigen; //! number of threads for eigen parallelization (applies both to MP and ST) Note. in order to activate Eigen parallelization, compile with '-fopenmp'
+ LineSearchSettings lineSearchSettings; //! the line search settings
+ LQOCSolverSettings lqoc_solver_settings;
+ bool debugPrint;
+ bool printSummary;
+ bool useSensitivityIntegrator;
+
+
+ //! compute the number of discrete time steps for an arbitrary input time interval
+ /*!
+ * @param timeHorizon the time horizon of interest, e.g. overall optimal control time horizon or shot-length
+ * @return the resulting number of steps, minimum 1 steps long
+ *
+ * \todo naming it K is confusing, should better be N.
+ */
+ int computeK(double timeHorizon) const
+ {
+ if (timeHorizon < 0.0)
+ {
+ throw std::runtime_error("time Horizon is negative");
+ }
+ return std::max(1, (int)std::lround(timeHorizon / dt));
+ }
+
+ double getSimulationTimestep() const { return (dt / (double)K_sim); }
+ //! print the current NLOptCon settings to console
+ void print() const
+ {
+ std::cout << "======================= NLOptCon Settings =====================" << std::endl;
+ std::cout << "===============================================================" << std::endl;
+ std::cout << "integrator: " << integratorToString.at(integrator) << std::endl;
+ std::cout << "discretization: " << discretizationToString.at(discretization) << std::endl;
+ std::cout << "time varying discretization: " << timeVaryingDiscretization << std::endl;
+ std::cout << "nonlinear OCP algorithm: " << nlocp_algorithmToString.at(nlocp_algorithm) << std::endl;
+ std::cout << "linear-quadratic OCP solver: " << locp_solverToString.at(lqocp_solver) << std::endl;
+ std::cout << "closed loop shooting:\t" << closedLoopShooting << std::endl;
+ std::cout << "dt:\t" << dt << std::endl;
+ std::cout << "K_sim:\t" << K_sim << std::endl;
+ std::cout << "K_shot:\t" << K_shot << std::endl;
+ std::cout << "maxIter:\t" << max_iterations << std::endl;
+ std::cout << "min cost improvement:\t" << min_cost_improvement << std::endl;
+ std::cout << "max defect sum:\t" << maxDefectSum << std::endl;
+ std::cout << "merit function rho defects:\t" << meritFunctionRho << std::endl;
+ std::cout << "merit function rho constraints:\t" << meritFunctionRhoConstraints << std::endl;
+ std::cout << "fixedHessianCorrection:\t" << fixedHessianCorrection << std::endl;
+ std::cout << "recordSmallestEigenvalue:\t" << recordSmallestEigenvalue << std::endl;
+ std::cout << "epsilon:\t" << epsilon << std::endl;
+ std::cout << "nThreads:\t" << nThreads << std::endl;
+ std::cout << "nThreadsEigen:\t" << nThreadsEigen << std::endl;
+ std::cout << "loggingPrefix:\t" << loggingPrefix << std::endl;
+ std::cout << "debugPrint:\t" << debugPrint << std::endl;
+ std::cout << "printSummary:\t" << printSummary << std::endl;
+ std::cout << "useSensitivityIntegrator:\t" << useSensitivityIntegrator << std::endl;
+ std::cout << std::endl;
+
+ lineSearchSettings.print();
+
+ lqoc_solver_settings.print();
+
+ std::cout << "===============================================================" << std::endl;
+ }
+
+ //! perform a quick check if the given NLOptCon settings fulfil minimum requirements
+ /*!
+ * \warning This check cannot guarantee that the control problem is well parameterized
+ */
+ bool parametersOk() const
+ {
+ if (dt <= 0)
+ {
+ std::cout << "Invalid parameter dt in NLOptConSettings, needs to be > 0. dt currently is " << dt
+ << std::endl;
+ return false;
+ }
+
+ if (K_sim <= 0)
+ {
+ std::cout << "Invalid parameter K_sim in NLOptConSettings, needs to be >= 1. K_sim currently is " << K_sim
+ << std::endl;
+ return false;
+ }
+
+ if (K_shot <= 0)
+ {
+ std::cout << "Invalid parameter K_shot in NLOptConSettings, needs to be >= 1. K_shot currently is "
+ << K_shot << std::endl;
+ return false;
+ }
+
+ if ((K_shot > 1) && (nlocp_algorithm == ILQR))
+ {
+ std::cout << "Invalid parameter: for iLQR K_shot needs to be 1. K_shot currently is " << K_shot
+ << std::endl;
+ return false;
+ }
+
+ if (nThreads > 100 || nThreadsEigen > 100)
+ {
+ std::cout << "Number of threads should not exceed 100." << std::endl;
+ return false;
+ }
+ return (lineSearchSettings.parametersOk());
+ }
+
+
+ //! load NLOptCon Settings from file
+ /*!
+ *
+ * @param filename path to the settings file
+ * @param verbose print out settings after reading them
+ * @param ns (optional) namspace of parameter file
+ */
+ void load(const std::string& filename, bool verbose = true, const std::string& ns = "alg")
+ {
+ if (verbose)
+ std::cout << "Trying to load NLOptCon config from " << filename << ": " << std::endl;
+
+ boost::property_tree::ptree pt;
+ boost::property_tree::read_info(filename, pt);
+
+ try
+ {
+ epsilon = pt.get<double>(ns + ".epsilon");
+ } catch (...)
+ {
+ }
+ try
+ {
+ timeVaryingDiscretization = pt.get<bool>(ns + ".timeVaryingDiscretization");
+ } catch (...)
+ {
+ }
+ try
+ {
+ min_cost_improvement = pt.get<double>(ns + ".min_cost_improvement");
+ } catch (...)
+ {
+ }
+ try
+ {
+ maxDefectSum = pt.get<double>(ns + ".maxDefectSum");
+ } catch (...)
+ {
+ }
+ try
+ {
+ meritFunctionRho = pt.get<double>(ns + ".meritFunctionRho");
+ } catch (...)
+ {
+ }
+ try
+ {
+ meritFunctionRhoConstraints = pt.get<double>(ns + ".meritFunctionRhoConstraints");
+ } catch (...)
+ {
+ }
+ try
+ {
+ max_iterations = pt.get<int>(ns + ".max_iterations");
+ } catch (...)
+ {
+ }
+ try
+ {
+ nThreads = pt.get<int>(ns + ".nThreads");
+ } catch (...)
+ {
+ }
+ try
+ {
+ loggingPrefix = pt.get<std::string>(ns + ".loggingPrefix");
+ } catch (...)
+ {
+ }
+ try
+ {
+ closedLoopShooting = pt.get<bool>(ns + ".closedLoopShooting");
+ } catch (...)
+ {
+ }
+ try
+ {
+ debugPrint = pt.get<bool>(ns + ".debugPrint");
+ } catch (...)
+ {
+ }
+ try
+ {
+ printSummary = pt.get<bool>(ns + ".printSummary");
+ } catch (...)
+ {
+ }
+ try
+ {
+ useSensitivityIntegrator = pt.get<bool>(ns + ".useSensitivityIntegrator");
+ } catch (...)
+ {
+ }
+ try
+ {
+ dt = pt.get<double>(ns + ".dt");
+ } catch (...)
+ {
+ }
+ try
+ {
+ K_sim = pt.get<int>(ns + ".K_sim");
+ } catch (...)
+ {
+ }
+ try
+ {
+ K_shot = pt.get<int>(ns + ".K_shot");
+ } catch (...)
+ {
+ }
+ try
+ {
+ nThreadsEigen = pt.get<size_t>(ns + ".nThreadsEigen");
+ } catch (...)
+ {
+ }
+ try
+ {
+ recordSmallestEigenvalue = pt.get<bool>(ns + ".recordSmallestEigenvalue");
+ } catch (...)
+ {
+ }
+ try
+ {
+ fixedHessianCorrection = pt.get<bool>(ns + ".fixedHessianCorrection");
+ } catch (...)
+ {
+ }
+
+ try
+ {
+ lineSearchSettings.load(filename, verbose, ns + ".line_search");
+ } catch (...)
+ {
+ }
+ try
+ {
+ lqoc_solver_settings.load(filename, verbose, ns + ".lqoc_solver_settings");
+ } catch (...)
+ {
+ }
+
+
+ try
+ {
+ std::string integratorStr = pt.get<std::string>(ns + ".integrator");
+ if (stringToIntegrator.find(integratorStr) != stringToIntegrator.end())
+ {
+ integrator = stringToIntegrator[integratorStr];
+ }
+ else
+ {
+ std::cout << "Invalid integrator specified in config, should be one of the following:" << std::endl;
+
+ for (auto it = stringToIntegrator.begin(); it != stringToIntegrator.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+ std::string discretizationStr = pt.get<std::string>(ns + ".discretization");
+ if (stringToDiscretization.find(discretizationStr) != stringToDiscretization.end())
+ {
+ discretization = stringToDiscretization[discretizationStr];
+ }
+ else
+ {
+ std::cout << "Invalid discretization specified in config, should be one of the following:" << std::endl;
+
+ for (auto it = stringToDiscretization.begin(); it != stringToDiscretization.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+ std::string nlocp_algorithmStr = pt.get<std::string>(ns + ".nlocp_algorithm");
+ if (stringTonlocp_algorithm.find(nlocp_algorithmStr) != stringTonlocp_algorithm.end())
+ {
+ nlocp_algorithm = stringTonlocp_algorithm[nlocp_algorithmStr];
+ }
+ else
+ {
+ std::cout << "Invalid nlocp_algorithm specified in config, should be one of the following:"
+ << std::endl;
+
+ for (auto it = stringTonlocp_algorithm.begin(); it != stringTonlocp_algorithm.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+
+ std::string locp_solverStr = pt.get<std::string>(ns + ".locp_solver");
+ if (stringTolocp_solver.find(locp_solverStr) != stringTolocp_solver.end())
+ {
+ lqocp_solver = stringTolocp_solver[locp_solverStr];
+ }
+ else
+ {
+ std::cout << "Invalid locp_solver specified in config, should be one of the following:" << std::endl;
+
+ for (auto it = stringTolocp_solver.begin(); it != stringTolocp_solver.end(); it++)
+ {
+ std::cout << it->first << std::endl;
+ }
+
+ exit(-1);
+ }
+
+ } catch (...)
+ {
+ }
+
+ if (verbose)
+ {
+ std::cout << "Loaded NLOptCon config from " << filename << ": " << std::endl;
+ print();
+ }
+ }
+
+
+ //! load settings from config file and return as settings struct
+ /*!
+ * @param filename the path to the settings file
+ * @param verbose print settings
+ * @param ns (optional) settings namespace
+ * @return the newly generated settings struct
+ */
+ static NLOptConSettings fromConfigFile(const std::string& filename,
+ bool verbose = true,
+ const std::string& ns = "alg")
+ {
+ NLOptConSettings settings;
+ settings.load(filename, true, ns);
+ return settings;
+ }
+
+private:
+ //! mappings for integrator types
+ std::map<ct::core::IntegrationType, std::string> integratorToString = {{ct::core::IntegrationType::EULER, "Euler"},
+ {ct::core::IntegrationType::RK4, "Runge-Kutta 4th Order"},
+ {ct::core::IntegrationType::MODIFIED_MIDPOINT, "Modified midpoint"},
+ {ct::core::IntegrationType::ODE45, "ode45"}, {ct::core::IntegrationType::RK5VARIABLE, "RK5 variable step"},
+ {ct::core::IntegrationType::RK78, "RK78"}, {ct::core::IntegrationType::BULIRSCHSTOER, "Bulirsch-Stoer"},
+ {ct::core::IntegrationType::EULERCT, "Euler (CT)"},
+ {ct::core::IntegrationType::RK4CT, "Runge-Kutta 4th Order (CT"},
+ {ct::core::IntegrationType::EULER_SYM, "Symplectic Euler"},
+ {ct::core::IntegrationType::RK_SYM, "Symplectic Runge Kutta"}};
+
+ std::map<std::string, ct::core::IntegrationType> stringToIntegrator = {{"Euler", ct::core::IntegrationType::EULER},
+ {"RK4", ct::core::IntegrationType::RK4}, {"MODIFIED_MIDPOINT", ct::core::IntegrationType::MODIFIED_MIDPOINT},
+ {"ODE45", ct::core::IntegrationType::ODE45}, {"RK5VARIABLE", ct::core::IntegrationType::RK5VARIABLE},
+ {"RK78", ct::core::IntegrationType::RK78}, {"BULIRSCHSTOER", ct::core::IntegrationType::BULIRSCHSTOER},
+ {"EulerCT", ct::core::IntegrationType::EULERCT}, {"RK4CT", ct::core::IntegrationType::RK4CT},
+ {"Euler_Sym", ct::core::IntegrationType::EULER_SYM}, {"Rk_Sym", ct::core::IntegrationType::RK_SYM}};
+
+
+ //! mappings for discretization types
+ std::map<APPROXIMATION, std::string> discretizationToString = {{APPROXIMATION::FORWARD_EULER, "Forward_euler"},
+ {APPROXIMATION::BACKWARD_EULER, "Backward_euler"}, {APPROXIMATION::SYMPLECTIC_EULER, "Symplectic_euler"},
+ {APPROXIMATION::TUSTIN, "Tustin"}, {APPROXIMATION::MATRIX_EXPONENTIAL, "Matrix_exponential"}};
+
+ std::map<std::string, APPROXIMATION> stringToDiscretization = {{"Forward_euler", APPROXIMATION::FORWARD_EULER},
+ {"Backward_euler", APPROXIMATION::BACKWARD_EULER}, {"Symplectic_euler", APPROXIMATION::SYMPLECTIC_EULER},
+ {"Tustin", APPROXIMATION::TUSTIN}, {"Matrix_exponential", APPROXIMATION::MATRIX_EXPONENTIAL}};
+
+
+ //! mappings for algorithm types
+ std::map<NLOCP_ALGORITHM, std::string> nlocp_algorithmToString = {{GNMS, "GNMS"}, {ILQR, "ILQR"}};
+
+ std::map<std::string, NLOCP_ALGORITHM> stringTonlocp_algorithm = {{"GNMS", GNMS}, {"ILQR", ILQR}};
+
+
+ //! mappings for linear-quadratic solver types
+ std::map<LQOCP_SOLVER, std::string> locp_solverToString = {
+ {GNRICCATI_SOLVER, "GNRICCATI_SOLVER"}, {HPIPM_SOLVER, "HPIPM_SOLVER"}};
+
+ std::map<std::string, LQOCP_SOLVER> stringTolocp_solver = {
+ {"GNRICCATI_SOLVER", GNRICCATI_SOLVER}, {"HPIPM_SOLVER", HPIPM_SOLVER}};
+};
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSolver-impl.hpp
new file mode 100644
index 0000000..6c7d9ca
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/NLOptConSolver-impl.hpp
@@ -0,0 +1,334 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOptConSolver(const OptConProblem_t& optConProblem,
+ const Settings_t& settings)
+{
+ initialize(optConProblem, settings);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::NLOptConSolver(const OptConProblem_t& optConProblem,
+ const std::string& settingsFile,
+ bool verbose,
+ const std::string& ns)
+ : NLOptConSolver(optConProblem, Settings_t::fromConfigFile(settingsFile, verbose, ns))
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::~NLOptConSolver()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::initialize(const OptConProblem_t& optConProblem,
+ const Settings_t& settings)
+{
+ if (settings.nThreads > 1)
+ nlocBackend_ = std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new NLOCBackendMP<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(optConProblem, settings));
+ else
+ nlocBackend_ = std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(optConProblem, settings));
+
+ configure(settings);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::configure(const Settings_t& settings)
+{
+ if (nlocBackend_->getSettings().nThreads != settings.nThreads)
+ throw std::runtime_error("cannot switch from ST to MT or vice versa. Please call initialize.");
+
+ nlocBackend_->configure(settings);
+
+ switch (settings.nlocp_algorithm)
+ {
+ case NLOptConSettings::NLOCP_ALGORITHM::GNMS:
+ nlocAlgorithm_ = std::shared_ptr<NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new GNMS<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(nlocBackend_, settings));
+ break;
+ case NLOptConSettings::NLOCP_ALGORITHM::ILQR:
+ nlocAlgorithm_ = std::shared_ptr<NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>(
+ new iLQR<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>(nlocBackend_, settings));
+ break;
+ default:
+ throw std::runtime_error("This algorithm is not implemented in NLOptConSolver.hpp");
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareIteration()
+{
+ nlocAlgorithm_->prepareIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishIteration()
+{
+ return nlocAlgorithm_->finishIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::prepareMPCIteration()
+{
+ nlocAlgorithm_->prepareMPCIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::finishMPCIteration()
+{
+ return nlocAlgorithm_->finishMPCIteration();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::runIteration()
+{
+#ifdef DEBUG_PRINT
+ auto startSolve = std::chrono::steady_clock::now();
+#endif
+ bool success = nlocAlgorithm_->runIteration();
+#ifdef DEBUG_PRINT
+ auto endSolve = std::chrono::steady_clock::now();
+ std::cout << "[NLOC]: runIteration() took "
+ << std::chrono::duration<double, std::milli>(endSolve - startSolve).count() << " ms" << std::endl;
+#endif
+ return success;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::setInitialGuess(const Policy_t& initialGuess)
+{
+ nlocBackend_->setInitialGuess(initialGuess);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+bool NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::solve()
+{
+ bool foundBetter = true;
+ int numIterations = 0;
+
+ while (foundBetter && (numIterations < nlocBackend_->getSettings().max_iterations))
+ {
+ foundBetter = runIteration();
+
+ numIterations++;
+ }
+
+ return (numIterations > 1 || foundBetter || (numIterations == 1 && !foundBetter));
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Policy_t&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSolution()
+{
+ return nlocBackend_->getSolution();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::StateTrajectory<STATE_DIM, SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getStateTrajectory() const
+{
+ return nlocBackend_->getStateTrajectory();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::ControlTrajectory<CONTROL_DIM, SCALAR>
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getControlTrajectory() const
+{
+ return nlocBackend_->getControlTrajectory();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const core::tpl::TimeArray<SCALAR>& NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeArray() const
+{
+ return nlocBackend_->getTimeArray();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getTimeHorizon() const
+{
+ return nlocBackend_->getTimeHorizon();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeTimeHorizon(const SCALAR& tf)
+{
+ nlocBackend_->changeTimeHorizon(tf);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeInitialState(
+ const core::StateVector<STATE_DIM, SCALAR>& x0)
+{
+ nlocBackend_->changeInitialState(x0);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeCostFunction(
+ const typename OptConProblem_t::CostFunctionPtr_t& cf)
+{
+ nlocBackend_->changeCostFunction(cf);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeNonlinearSystem(
+ const typename OptConProblem_t::DynamicsPtr_t& dyn)
+{
+ nlocBackend_->changeNonlinearSystem(dyn);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::changeLinearSystem(
+ const typename OptConProblem_t::LinearPtr_t& lin)
+{
+ nlocBackend_->changeLinearSystem(lin);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+SCALAR NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCost() const
+{
+ return nlocBackend_->getCost();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::Settings_t&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getSettings()
+{
+ return nlocBackend_->getSettings();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBackend()
+{
+ return nlocBackend_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances()
+{
+ return nlocBackend_->getNonlinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::DynamicsPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getNonlinearSystemsInstances() const
+{
+ return nlocBackend_->getNonlinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances()
+{
+ return nlocBackend_->getLinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::LinearPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getLinearSystemsInstances() const
+{
+ return nlocBackend_->getLinearSystemsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances()
+{
+ return nlocBackend_->getCostFunctionInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::CostFunctionPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getCostFunctionInstances() const
+{
+ return nlocBackend_->getCostFunctionInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances()
+{
+ return nlocBackend_->getBoxConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getBoxConstraintsInstances() const
+{
+ return nlocBackend_->getBoxConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+std::vector<typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances()
+{
+ return nlocBackend_->getGeneralConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+const std::vector<
+ typename NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::OptConProblem_t::ConstraintPtr_t>&
+NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::getGeneralConstraintsInstances() const
+{
+ return nlocBackend_->getGeneralConstraintsInstances();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR>
+void NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::logSummaryToMatlab(const std::string& fileName)
+{
+ nlocBackend_->logSummaryToMatlab(fileName);
+}
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/NLOptConSolver.hpp b/ct_optcon/include/ct/optcon/solver/NLOptConSolver.hpp
new file mode 100644
index 0000000..cac1857
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/NLOptConSolver.hpp
@@ -0,0 +1,234 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/problem/OptConProblem.h>
+
+#include <ct/optcon/nloc/NLOCBackendST.hpp>
+#include <ct/optcon/nloc/NLOCBackendMP.hpp>
+
+#include <ct/optcon/nloc/algorithms/ilqr/iLQR.hpp>
+#include <ct/optcon/nloc/algorithms/gnms/GNMS.hpp>
+
+namespace ct {
+namespace optcon {
+
+
+/** \defgroup OptConSolver OptConSolver
+ * Solver interface for finite horizon optimal control problems
+ */
+template <size_t STATE_DIM,
+ size_t CONTROL_DIM,
+ size_t P_DIM = STATE_DIM / 2,
+ size_t V_DIM = STATE_DIM / 2,
+ typename SCALAR = double>
+class NLOptConSolver : public OptConSolver<NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>,
+ core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR>,
+ NLOptConSettings,
+ STATE_DIM,
+ CONTROL_DIM,
+ SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+ static const size_t POS_DIM = P_DIM;
+ static const size_t VEL_DIM = V_DIM;
+
+ typedef NLOptConSolver<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR> Derived;
+ typedef core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;
+ typedef NLOptConSettings Settings_t;
+ typedef SCALAR Scalar_t;
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+
+ //! constructor
+ NLOptConSolver(const OptConProblem_t& optConProblem, const Settings_t& settings);
+
+ //! constructor
+ NLOptConSolver(const OptConProblem_t& optConProblem,
+ const std::string& settingsFile,
+ bool verbose = true,
+ const std::string& ns = "alg");
+
+ //! destructor
+ virtual ~NLOptConSolver();
+
+ /**
+ * configures the solver
+ * */
+ void initialize(const OptConProblem_t& optConProblem, const Settings_t& settings);
+
+ /**
+ * configures the solver
+ * */
+ void configure(const Settings_t& settings) override;
+
+
+ /*!
+ * execute preparation steps for an iteration, e.g. computation of defects
+ */
+ virtual void prepareIteration();
+
+ /*!
+ * execute finishing step for an iteration, e.g. solving Riccati backward pass.
+ * @return
+ */
+ virtual bool finishIteration();
+
+
+ /*!
+ * execute preparation steps for an iteration, e.g. computation of defects
+ */
+ virtual void prepareMPCIteration();
+
+ /*!
+ * execute finishing step for an iteration, e.g. solving Riccati backward pass.
+ * @return
+ */
+ virtual bool finishMPCIteration();
+
+ /**
+ * run a single iteration of the solver
+ * @return true if a better solution was found
+ */
+ virtual bool runIteration();
+
+ /*!
+ * Set the initial guess used by the solver (not all solvers might support initial guesses)
+ */
+ void setInitialGuess(const Policy_t& initialGuess) override;
+
+ /**
+ * solve the optimal control problem
+ * */
+ virtual bool solve() override;
+
+ /**
+ * Get the optimized control policy to the optimal control problem
+ * @return
+ */
+ virtual const Policy_t& getSolution() override;
+
+ /**
+ * Get the optimized trajectory to the optimal control problem
+ * @return
+ */
+ virtual const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const override;
+
+ /**
+ * Get the optimal feedforward control input corresponding to the optimal trajectory
+ * @return
+ */
+ virtual const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const override;
+
+ /**
+ * Get the time indices corresponding to the solution
+ * @return
+ */
+ virtual const core::tpl::TimeArray<SCALAR>& getTimeArray() const override;
+
+ /*!
+ * \brief Get the time horizon the solver currently operates on.
+ *
+ */
+ virtual SCALAR getTimeHorizon() const override;
+
+ /*!
+ * \brief Change the time horizon the solver operates on.
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct time horizon set.
+ */
+ virtual void changeTimeHorizon(const SCALAR& tf) override;
+
+ /*!
+ * \brief Change the initial state for the optimal control problem
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct initial state set
+ */
+ virtual void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0) override;
+
+ /*!
+ * \brief Change the cost function
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct cost function
+ */
+ virtual void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf) override;
+
+ /*!
+ * \brief Change the nonlinear system
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct nonlinear system
+ */
+ virtual void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn) override;
+
+ /*!
+ * \brief Change the linear system
+ *
+ * This function does not need to be called if setOptConProblem() has been called
+ * with an OptConProblem that had the correct linear system
+ */
+ virtual void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin) override;
+
+ virtual SCALAR getCost() const override;
+
+ //! get a reference to the current settings
+ const Settings_t& getSettings();
+
+ //! get a reference to the backend (@todo this is not optimal, allows the user too much access)
+ const std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>>& getBackend();
+
+ //! get reference to the nonlinear system
+ std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() override;
+
+ //! get constant reference to the nonlinear system
+ const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const override;
+
+ //! get reference to the linearized system
+ std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() override;
+
+ //! get constant reference to the linearized system
+ const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const override;
+
+ //! get reference to the cost function
+ std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() override;
+
+ //! get constant reference to the cost function
+ const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const override;
+
+ //! get reference to the box constraints
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() override;
+
+ //! get constant reference to the boxconstraints
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const override;
+
+ //! get reference to the general constraints
+ std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() override;
+
+ //! get constant reference to the general constraints
+ const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const override;
+
+ //! logging a short summary to matlab
+ void logSummaryToMatlab(const std::string& fileName);
+
+protected:
+ //! the backend holding all the math operations
+ std::shared_ptr<NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>> nlocBackend_;
+
+ //! the algorithm for sequencing the math operations in correct manner
+ std::shared_ptr<NLOCAlgorithm<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>> nlocAlgorithm_;
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/OptConSolver.h b/ct_optcon/include/ct/optcon/solver/OptConSolver.h
new file mode 100644
index 0000000..bf022d0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/OptConSolver.h
@@ -0,0 +1,319 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+/**
+ * OptConSolver.hpp
+ *
+ *
+ * Requirements:
+ * - returns an optimal controller. These can be different controller types, feedforward only, feedforward-feedback, feedback only,
+ * therefore it is templated on the controller type
+ */
+
+#pragma once
+
+#include <ct/optcon/problem/OptConProblem.h>
+
+
+namespace ct {
+namespace optcon {
+
+
+/** \defgroup OptConSolver OptConSolver
+ * Solver interface for finite horizon optimal control problems
+ *
+ * * Requirements:
+ * - returns an optimal controller. These can be different controller types, feedforward only, feedforward-feedback, feedback only,
+ * therefore it is templated on the controller type
+ */
+template <typename DERIVED,
+ typename POLICY,
+ typename SETTINGS,
+ size_t STATE_DIM,
+ size_t CONTROL_DIM,
+ typename SCALAR = double>
+class OptConSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const size_t STATE_D = STATE_DIM;
+ static const size_t CONTROL_D = CONTROL_DIM;
+
+ typedef POLICY Policy_t;
+ typedef SETTINGS Settings_t;
+ typedef DERIVED Derived;
+ typedef SCALAR Scalar_t;
+
+ typedef OptConProblem<STATE_DIM, CONTROL_DIM, SCALAR> OptConProblem_t;
+
+
+ OptConSolver() {}
+ virtual ~OptConSolver() {}
+ /*!
+ * \brief Assigns the optimal control problem to the solver.
+ *
+ * Most solvers will require some computational effort to adjust to a new problem.
+ * Therefore, when only adjusting a problem, it is more efficient to adjust individual
+ * properties instead of assigning an entirely new problem. To adjust properties, you can
+ * use
+ * - changeTimeHorizon()
+ * - changeInitialState()
+ * - changeCostFunction()
+ * - changeSystemDynamics()
+ * - changeSystemLinearization()
+ *
+ * @return returns true if the optimal control structure is valid for the given solver
+ */
+ virtual void setProblem(const OptConProblem_t& optConProblem)
+ {
+ optConProblem.verify();
+
+ changeTimeHorizon(optConProblem.getTimeHorizon());
+ changeInitialState(optConProblem.getInitialState());
+ changeCostFunction(optConProblem.getCostFunction());
+ changeNonlinearSystem(optConProblem.getNonlinearSystem());
+ changeLinearSystem(optConProblem.getLinearSystem());
+
+ if (optConProblem.getBoxConstraints())
+ changeBoxConstraints(optConProblem.getBoxConstraints());
+ if (optConProblem.getGeneralConstraints())
+ changeGeneralConstraints(optConProblem.getGeneralConstraints());
+ }
+
+ /**
+ * configures the solver
+ * */
+ virtual void configure(const Settings_t& settings) = 0;
+
+ /**
+ * configures the solver from configFile
+ * */
+ void configureFromFile(const std::string& filename,
+ bool verbose = true,
+ const std::string& ns = DERIVED::SolverName)
+ {
+ Settings_t settings;
+ settings.load(filename, verbose, ns);
+ configure(settings);
+ }
+
+ /**
+ * solve the optimal control problem
+ * @return true if solve succeeded, false otherwise.
+ * */
+ virtual bool solve() = 0;
+
+ /**
+ * run a single iteration of the solver (might not be supported by all solvers)
+ * @return true if a better solution was found
+ */
+ virtual bool runIteration() { throw std::runtime_error("runIteration not supported by solver"); }
+ /**
+ * Get the optimized control policy to the optimal control problem
+ * @return
+ */
+ virtual const Policy_t& getSolution() = 0;
+
+ /**
+ * Get the optimized trajectory to the optimal control problem
+ * @return
+ */
+ virtual const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const = 0;
+
+ /**
+ * Get the optimal feedforward control input corresponding to the optimal trajectory
+ * @return
+ */
+ virtual const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const = 0;
+
+ /**
+ * Get the time indices corresponding to the solution
+ * @return
+ */
+ virtual const core::tpl::TimeArray<SCALAR>& getTimeArray() const = 0;
+
+
+ /*!
+ * Set the initial guess used by the solver (not all solvers might support initial guesses)
+ */
+ virtual void setInitialGuess(const Policy_t& initialGuess) = 0;
+
+
+ /*!
+ * \brief Get the time horizon the solver currently operates on.
+ *
+ */
+ virtual SCALAR getTimeHorizon() const = 0;
+
+
+ /*!
+ * \brief Change the time horizon the solver operates on.
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct time horizon set.
+ */
+ virtual void changeTimeHorizon(const SCALAR& tf) = 0;
+
+ /*!
+ * \brief Change the initial state for the optimal control problem
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct initial state set
+ */
+ virtual void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0) = 0;
+
+ /*!
+ * \brief Change the cost function
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct cost function
+ */
+ virtual void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf) = 0;
+
+ /*!
+ * \brief Change the nonlinear system
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct nonlinear system
+ */
+ virtual void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn) = 0;
+
+ /*!
+ * \brief Change the linear system
+ *
+ * This function does not need to be called if setProblem() has been called
+ * with an OptConProblem that had the correct linear system
+ */
+ virtual void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin) = 0;
+
+ /**
+ * @brief Change the box constraints
+ *
+ * This function does not need to be called if
+ * setProblem() has been called with an OptConProblem that
+ * had the correct linear system
+ *
+ * @param[in] con The new box constraints
+ */
+ virtual void changeBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t con)
+ {
+ throw std::runtime_error("The current solver does not support constraints!");
+ }
+
+ /**
+ * @brief Change the general constraints.
+ *
+ * This function does not need to be called if
+ * setProblem() has been called with an OptConProblem that
+ * had the correct linear system
+ *
+ * @param[in] con The new general constraints
+ */
+ virtual void changeGeneralConstraints(const typename OptConProblem_t::ConstraintPtr_t con)
+ {
+ throw std::runtime_error("The current solver does not support general constraints!");
+ }
+
+ virtual SCALAR getCost() const { throw std::runtime_error("Get cost not implemented"); }
+ /*!
+ * \brief Direct accessor to the system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeNonlinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ virtual std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const = 0;
+
+ /*!
+ * \brief Direct accessor to the linear system instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeLinearSystem() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ virtual std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const = 0;
+
+ /*!
+ * \brief Direct accessor to the cost function instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ * @return
+ */
+ virtual std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const = 0;
+
+ /**
+ * @brief Direct accessor to the box constraint instances
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The state box constraint instances
+ */
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getBoxConstraintsInstances() const = 0;
+
+ /**
+ * @brief Direct accessor to the general constraints
+ *
+ * \warning{Use this only when performance absolutely matters and if you know what you
+ * are doing. Otherwise use e.g. changeCostFunction() to change the system dynamics
+ * in a safe and easy way. You should especially not change the size of the vector or
+ * modify each entry differently.}
+ *
+ * @return The general constraints instances.
+ */
+ virtual std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() = 0;
+
+ virtual const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const = 0;
+
+
+ /**
+ * @brief Generates and compiles AD source code which can be used in
+ * the solver. This method can be called just before solving the
+ * problem, i.e. it can be called from the same executable than
+ * the actual solve
+ *
+ * @param[in] problemCG The optcon problem templated on the AD CG Scalar
+ * @param[in] settings The settings indicating what to generate
+ */
+ virtual void generateAndCompileCode(const OptConProblem<STATE_DIM, CONTROL_DIM, ct::core::ADCGScalar>& problemCG,
+ const ct::core::DerivativesCppadSettings& settings)
+ {
+ throw std::runtime_error("Generate and compile code not implemented for this solver");
+ }
+
+ /**
+ * @brief Generates source AD source code which can be used in the
+ * solver. This method needs to be called ahead of actually
+ * solving the problem (e.g. from a different executable)
+ *
+ * @param[in] settings The settings indicating what to generate
+ */
+ virtual void generateCode(const ct::core::DerivativesCppadSettings& settings)
+ {
+ throw std::runtime_error("Generate Code not implemented for this solver");
+ }
+};
+}
+}
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
new file mode 100644
index 0000000..3c59406
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp
@@ -0,0 +1,354 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::GNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem)
+ : LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>(lqocProblem), N_(-1)
+{
+ Eigen::initParallel();
+ Eigen::setNbThreads(settings_.nThreadsEigen);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::GNRiccatiSolver(int N)
+{
+ changeNumberOfStages(N);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::~GNRiccatiSolver()
+{
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solve()
+{
+ for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= 0; i--)
+ solveSingleStage(i);
+
+ computeStateAndControlUpdates();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::solveSingleStage(int N)
+{
+ if (N == this->lqocProblem_->getNumberOfStages() - 1)
+ initializeCostToGo();
+
+ designController(N);
+
+ if (N > 0)
+ computeCostToGo(N);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::configure(const NLOptConSettings& settings)
+{
+ settings_ = settings;
+ H_corrFix_ = settings_.epsilon * ControlMatrix::Identity();
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ct::core::StateVectorArray<STATE_DIM, SCALAR> GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSolutionState()
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+ ct::core::StateVectorArray<STATE_DIM, SCALAR> x = p.x_;
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages() + 1; k++)
+ {
+ x[k] += this->lx_[k];
+ }
+
+ return x;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSolutionControl()
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> u = p.u_;
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
+ {
+ u[k] += this->lu_[k];
+ }
+ return u;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>
+GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getFeedforwardUpdates()
+{
+ return lv_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getFeedback(
+ ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K)
+{
+ K = L_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeStateAndControlUpdates()
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ this->delta_x_norm_ = 0.0;
+ this->delta_uff_norm_ = 0.0;
+
+ this->lx_[0].setZero();
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
+ {
+ //! control update rule
+ this->lu_[k] = lv_[k];
+ if (k > 0) // lx is zero for k=0
+ this->lu_[k].noalias() += L_[k] * this->lx_[k];
+
+ //! state update rule
+ this->lx_[k + 1] = p.B_[k] * lv_[k] + p.b_[k];
+ if (k > 0)
+ this->lx_[k + 1].noalias() += (p.A_[k] + p.B_[k] * L_[k]) * this->lx_[k];
+
+ //! compute the norms of the updates
+ //! \todo needed?
+ this->delta_x_norm_ += this->lx_[k + 1].norm();
+ this->delta_uff_norm_ += this->lu_[k].norm();
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+SCALAR GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::getSmallestEigenvalue()
+{
+ return smallestEigenvalue_;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem)
+{
+ if (lqocProblem->isConstrained())
+ {
+ throw std::runtime_error(
+ "Selected wrong solver - GNRiccatiSolver cannot handle constrained problems. Use a different solver");
+ }
+
+ const int& N = lqocProblem->getNumberOfStages();
+ changeNumberOfStages(N);
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::changeNumberOfStages(int N)
+{
+ if (N <= 0)
+ return;
+
+ if (N_ == N)
+ return;
+
+ gv_.resize(N);
+ G_.resize(N);
+
+ H_.resize(N);
+ Hi_.resize(N);
+ Hi_inverse_.resize(N);
+
+ lv_.resize(N);
+ L_.resize(N);
+
+ this->lx_.resize(N + 1);
+ this->lu_.resize(N);
+
+ sv_.resize(N + 1);
+ S_.resize(N + 1);
+
+ N_ = N;
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeCostToGo()
+{
+ //! since intializeCostToGo is the first call, we initialize the smallestEigenvalue here.
+ smallestEigenvalue_ = std::numeric_limits<SCALAR>::infinity();
+
+ // initialize quadratic approximation of cost to go
+ const int& N = this->lqocProblem_->getNumberOfStages();
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ S_[N] = p.Q_[N];
+ sv_[N] = p.qv_[N];
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::computeCostToGo(size_t k)
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ S_[k] = p.Q_[k];
+ S_[k].noalias() += p.A_[k].transpose() * S_[k + 1] * p.A_[k];
+ S_[k].noalias() -= L_[k].transpose() * Hi_[k] * L_[k];
+
+ S_[k] = 0.5 * (S_[k] + S_[k].transpose()).eval();
+
+ sv_[k] = p.qv_[k];
+ sv_[k].noalias() += p.A_[k].transpose() * sv_[k + 1];
+ sv_[k].noalias() += p.A_[k].transpose() * S_[k + 1] * p.b_[k];
+ sv_[k].noalias() += L_[k].transpose() * Hi_[k] * lv_[k];
+ sv_[k].noalias() += L_[k].transpose() * gv_[k];
+ sv_[k].noalias() += G_[k].transpose() * lv_[k];
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::designController(size_t k)
+{
+ LQOCProblem_t& p = *this->lqocProblem_;
+
+ gv_[k] = p.rv_[k];
+ gv_[k].noalias() += p.B_[k].transpose() * sv_[k + 1];
+ gv_[k].noalias() += p.B_[k].transpose() * S_[k + 1].template selfadjointView<Eigen::Lower>() * p.b_[k];
+
+ G_[k] = p.P_[k];
+ //G_[k].noalias() += B_[k].transpose() * S_[k+1] * A_[k];
+ G_[k].noalias() += p.B_[k].transpose() * S_[k + 1].template selfadjointView<Eigen::Lower>() * p.A_[k];
+
+ H_[k] = p.R_[k];
+ //H_[k].noalias() += B_[k].transpose() * S_[k+1] * B_[k];
+ H_[k].noalias() += p.B_[k].transpose() * S_[k + 1].template selfadjointView<Eigen::Lower>() * p.B_[k];
+
+ if (settings_.fixedHessianCorrection)
+ {
+ if (settings_.epsilon > 1e-10)
+ Hi_[k] = H_[k] + settings_.epsilon * ControlMatrix::Identity();
+ else
+ Hi_[k] = H_[k];
+
+ if (settings_.recordSmallestEigenvalue)
+ {
+ // compute eigenvalues with eigenvectors enabled
+ eigenvalueSolver_.compute(Hi_[k], Eigen::ComputeEigenvectors);
+ const ControlMatrix& V = eigenvalueSolver_.eigenvectors().real();
+ const ControlVector& lambda = eigenvalueSolver_.eigenvalues();
+
+ smallestEigenvalue_ = std::min(smallestEigenvalue_, lambda.minCoeff());
+
+ // Corrected Eigenvalue Matrix
+ ControlMatrix D = ControlMatrix::Zero();
+ // make D positive semi-definite (as described in IV. B.)
+ D.diagonal() = lambda.cwiseMax(settings_.epsilon);
+
+ // reconstruct H
+ ControlMatrix Hi_regular = V * D * V.transpose();
+
+ // invert D
+ ControlMatrix D_inverse = ControlMatrix::Zero();
+ // eigenvalue-wise inversion
+ D_inverse.diagonal() = -1.0 * D.diagonal().cwiseInverse();
+ ControlMatrix Hi_inverse_regular = V * D_inverse * V.transpose();
+
+ if (!Hi_inverse_[k].isApprox(Hi_inverse_regular, 1e-4))
+ {
+ std::cout << "warning, inverses not identical at " << k << std::endl;
+ std::cout << "Hi_inverse_fixed - Hi_inverse_regular: " << std::endl
+ << Hi_inverse_[k] - Hi_inverse_regular << std::endl
+ << std::endl;
+ }
+ }
+
+ Hi_inverse_[k] = -Hi_[k].template selfadjointView<Eigen::Lower>().llt().solve(ControlMatrix::Identity());
+
+ // calculate FB gain update
+ L_[k].noalias() = Hi_inverse_[k].template selfadjointView<Eigen::Lower>() * G_[k];
+
+ // calculate FF update
+ lv_[k].noalias() = Hi_inverse_[k].template selfadjointView<Eigen::Lower>() * gv_[k];
+ }
+ else
+ {
+ // compute eigenvalues with eigenvectors enabled
+ eigenvalueSolver_.compute(H_[k], Eigen::ComputeEigenvectors);
+ const ControlMatrix& V = eigenvalueSolver_.eigenvectors().real();
+ const ControlVector& lambda = eigenvalueSolver_.eigenvalues();
+
+ if (settings_.recordSmallestEigenvalue)
+ {
+ smallestEigenvalue_ = std::min(smallestEigenvalue_, lambda.minCoeff());
+ }
+
+ // Corrected Eigenvalue Matrix
+ ControlMatrix D = ControlMatrix::Zero();
+ // make D positive semi-definite (as described in IV. B.)
+ D.diagonal() = lambda.cwiseMax(settings_.epsilon);
+
+ // reconstruct H
+ Hi_[k].noalias() = V * D * V.transpose();
+
+ // invert D
+ ControlMatrix D_inverse = ControlMatrix::Zero();
+ // eigenvalue-wise inversion
+ D_inverse.diagonal() = -1.0 * D.diagonal().cwiseInverse();
+ Hi_inverse_[k].noalias() = V * D_inverse * V.transpose();
+
+ // calculate FB gain update
+ L_[k].noalias() = Hi_inverse_[k] * G_[k];
+
+ // calculate FF update
+ lv_[k].noalias() = Hi_inverse_[k] * gv_[k];
+ }
+}
+
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::logToMatlab()
+{
+#ifdef MATLAB_FULL_LOG
+
+ matFile_.open("GNRiccatiSolver.mat");
+
+ matFile_.put("sv", sv_.toImplementation());
+ matFile_.put("S", S_.toImplementation());
+ matFile_.put("L", L_.toImplementation());
+ matFile_.put("H", H_.toImplementation());
+ matFile_.put("Hi_", Hi_.toImplementation());
+ matFile_.put("Hi_inverse", Hi_inverse_.toImplementation());
+ matFile_.put("G", G_.toImplementation());
+ matFile_.put("gv", gv_.toImplementation());
+
+ matFile_.close();
+#endif
+}
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
+void GNRiccatiSolver<STATE_DIM, CONTROL_DIM, SCALAR>::initializeAndAllocate()
+{
+ // do nothing
+}
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
new file mode 100644
index 0000000..66550e6
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp
@@ -0,0 +1,125 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "LQOCSolver.hpp"
+
+#ifdef MATLAB_FULL_LOG
+#include <ct/optcon/matlab.hpp>
+#endif
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * This class implements an general Riccati backward pass for solving an unconstrained
+ * linear-quadratic Optimal Control problem
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class GNRiccatiSolver : public LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const int state_dim = STATE_DIM;
+ static const int control_dim = CONTROL_DIM;
+
+ typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+
+ typedef ct::core::StateMatrix<STATE_DIM, SCALAR> StateMatrix;
+ typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> StateMatrixArray;
+ typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> ControlVector;
+ typedef ct::core::ControlMatrix<CONTROL_DIM, SCALAR> ControlMatrix;
+ typedef ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR> ControlMatrixArray;
+ typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> StateControlMatrixArray;
+ typedef ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> FeedbackArray;
+
+ typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> StateVectorArray;
+ typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
+
+ GNRiccatiSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem = nullptr);
+
+ GNRiccatiSolver(int N);
+
+ virtual ~GNRiccatiSolver();
+
+ virtual void solve() override;
+
+ virtual void initializeAndAllocate() override;
+
+ virtual void solveSingleStage(int N) override;
+
+ virtual void configure(const NLOptConSettings& settings) override;
+
+ virtual ct::core::StateVectorArray<STATE_DIM, SCALAR> getSolutionState() override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getSolutionControl() override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getFeedforwardUpdates() override;
+
+ virtual void getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K) override;
+
+ //! compute the state and control updates.
+ /*!
+ * this method is specific to the GN Riccati solver, since the state updates lx_
+ * need to be completed in an additional forward sweep.
+ *
+ * IMPORTANT: you need to call this method at the right place if you're using solveSingleStage() by yourself.
+ */
+ virtual void computeStateAndControlUpdates() override;
+
+ virtual SCALAR getSmallestEigenvalue() override;
+
+protected:
+ /*!
+ * resize matrices
+ * @param lqocProblem
+ */
+ virtual void setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem) override;
+
+ void changeNumberOfStages(int N);
+
+ void initializeCostToGo();
+
+ void computeCostToGo(size_t k);
+
+ void designController(size_t k);
+
+ void logToMatlab();
+
+ NLOptConSettings settings_;
+
+ ControlVectorArray gv_;
+ FeedbackArray G_;
+
+ ControlMatrixArray H_;
+ ControlMatrixArray Hi_;
+ ControlMatrixArray Hi_inverse_;
+ ControlMatrix H_corrFix_;
+
+ ControlVectorArray lv_;
+ FeedbackArray L_;
+
+ StateVectorArray sv_;
+ StateMatrixArray S_;
+
+ int N_;
+
+ SCALAR smallestEigenvalue_;
+
+ //! Eigenvalue solver, used for inverting the Hessian and for regularization
+ Eigen::SelfAdjointEigenSolver<Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM>> eigenvalueSolver_;
+
+//! if building with MATLAB support, include matfile
+#ifdef MATLAB_FULL_LOG
+ matlab::MatFile matFile_;
+#endif //MATLAB
+};
+
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
new file mode 100644
index 0000000..01255c0
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
@@ -0,0 +1,703 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#ifdef HPIPM
+
+namespace ct {
+namespace optcon {
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+HPIPMInterface<STATE_DIM, CONTROL_DIM>::HPIPMInterface(int N) : N_(-1), x0_(nullptr), settings_(NLOptConSettings())
+{
+ // some zero variables
+ hb0_.setZero();
+ hr0_.setZero();
+
+ // by default, set number of box and general constraints to zero
+ nb_.resize(1, 0);
+ ng_.resize(1, 0);
+
+ configure(settings_);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+HPIPMInterface<STATE_DIM, CONTROL_DIM>::~HPIPMInterface()
+{
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::initializeAndAllocate()
+{
+ int qp_size = ::d_memsize_ocp_qp(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data());
+ qp_mem_.resize(qp_size);
+ ::d_create_ocp_qp(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data(), &qp_, qp_mem_.data());
+
+ int qp_sol_size = ::d_memsize_ocp_qp_sol(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data());
+ qp_sol_mem_.resize(qp_sol_size);
+ ::d_create_ocp_qp_sol(N_, nx_.data(), nu_.data(), nb_.data(), ng_.data(), &qp_sol_, qp_sol_mem_.data());
+
+ int ipm_size = ::d_memsize_ipm_hard_ocp_qp(&qp_, &arg_);
+ ipm_mem_.resize(ipm_size);
+ ::d_create_ipm_hard_ocp_qp(&qp_, &arg_, &workspace_, ipm_mem_.data());
+
+ std::cout << "HPIPM allocating memory for QP"
+ << std::endl; // always print to make sure users take note in case of wrong use
+ if (settings_.lqoc_solver_settings.lqoc_debug_print)
+ {
+ std::cout << "HPIPM qp_size: " << qp_size << std::endl;
+ std::cout << "HPIPM qp_sol_size: " << qp_sol_size << std::endl;
+ std::cout << "HPIPM ipm_size: " << ipm_size << std::endl;
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configure(const NLOptConSettings& settings)
+{
+ settings_ = settings;
+
+ arg_.iter_max = settings_.lqoc_solver_settings.num_lqoc_iterations;
+
+ arg_.alpha_min = 1e-8; // todo review and make setting
+ arg_.mu_max = 1e-12; // todo review and make setting
+ arg_.mu0 = 2.0; // todo review and make setting
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::solve()
+{
+ // optional printout
+ // for (size_t i = 0; i < N_ + 1; i++)
+ // {
+ // std::cout << "HPIPM matrix printout for stage " << i << std::endl;
+ // if (i<N_)
+ // {
+ // printf("\nA\n");
+ // d_print_mat(STATE_DIM, STATE_DIM, hA_[i], STATE_DIM);
+ // printf("\nB\n");
+ // d_print_mat(STATE_DIM, CONTROL_DIM, hB_[i], STATE_DIM);
+ // printf("\nb\n");
+ // d_print_mat(1, STATE_DIM, hb_[i], 1);
+ // }
+ //
+ // printf("\nQ\n");
+ // d_print_mat(STATE_DIM, STATE_DIM, hQ_[i], STATE_DIM);
+ // printf("\nq\n");
+ // d_print_mat(1, STATE_DIM, hq_[i], 1);
+ //
+ //
+ // if (i<N_)
+ // {
+ // printf("\nR\n");
+ // d_print_mat(CONTROL_DIM, CONTROL_DIM, hR_[i], CONTROL_DIM);
+ // printf("\nS\n");
+ // d_print_mat(CONTROL_DIM, STATE_DIM, hS_[i], CONTROL_DIM);
+ // printf("\nr\n");
+ // d_print_mat(1, CONTROL_DIM, hr_[i], 1);
+ // }
+ //
+ // int_print_mat(1, nb_[i], hidxb_[i], 1);
+ // printf("\nhd_lb_\n");
+ // d_print_mat(1, nb_[i], hd_lb_[i], 1);
+ // printf("\nhd_ub_\n");
+ // d_print_mat(1, nb_[i], hd_ub_[i], 1);
+ // } // end optional printout
+
+ // set pointers to optimal control problem
+ ::d_cvt_colmaj_to_ocp_qp(hA_.data(), hB_.data(), hb_.data(), hQ_.data(), hS_.data(), hR_.data(), hq_.data(),
+ hr_.data(), hidxb_.data(), hd_lb_.data(), hd_ub_.data(), hC_.data(), hD_.data(), hd_lg_.data(), hd_ug_.data(),
+ &qp_);
+
+ // solve optimal control problem
+ ::d_solve_ipm2_hard_ocp_qp(&qp_, &qp_sol_, &workspace_);
+
+ // display iteration summary
+ if (settings_.lqoc_solver_settings.lqoc_debug_print)
+ {
+ printf("\nipm iter = %d\n", workspace_.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ d_print_e_tran_mat(5, workspace_.iter, workspace_.stat, 5);
+ }
+
+ // extract state and control updates
+ computeStateAndControlUpdates();
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::computeStateAndControlUpdates()
+{
+ LQOCProblem<STATE_DIM, CONTROL_DIM>& p = *this->lqocProblem_;
+
+ // convert optimal control problem solution to standard column-major representation
+ ::d_cvt_ocp_qp_sol_to_colmaj(&qp_, &qp_sol_, u_.data(), x_.data(), pi_.data(), lam_lb_.data(), lam_ub_.data(),
+ lam_lg_.data(), lam_ug_.data());
+
+ hx_[0] = this->lqocProblem_->x_[0];
+
+ this->delta_x_norm_ = 0.0;
+ this->delta_uff_norm_ = 0.0;
+
+ this->lx_[0].setZero();
+
+ for (int k = 0; k < this->lqocProblem_->getNumberOfStages(); k++)
+ {
+ // reconstruct control update
+ this->lu_[k] = hu_[k] - p.u_[k];
+
+ // reconstruct state update
+ this->lx_[k + 1] = hx_[k + 1] - p.x_[k + 1];
+
+ // compute the norms of the updates
+ // TODO needed?
+ this->delta_x_norm_ += this->lx_[k + 1].norm();
+ this->delta_uff_norm_ += this->lu_[k].norm();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+ct::core::StateVectorArray<STATE_DIM> HPIPMInterface<STATE_DIM, CONTROL_DIM>::getSolutionState()
+{
+ return hx_;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+ct::core::ControlVectorArray<CONTROL_DIM> HPIPMInterface<STATE_DIM, CONTROL_DIM>::getSolutionControl()
+{
+ return hu_;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM>& K)
+{
+ LQOCProblem<STATE_DIM, CONTROL_DIM>& p = *this->lqocProblem_;
+ K.resize(p.getNumberOfStages());
+
+ // for stage 0, HPIPM does not provide feedback, so we have to construct it
+
+ // step 1: reconstruct H[0]
+ Eigen::Matrix<double, control_dim, control_dim> Lr;
+ ::d_cvt_strmat2mat(Lr.rows(), Lr.cols(), &workspace_.L[0], 0, 0, Lr.data(), Lr.rows());
+ Eigen::Matrix<double, control_dim, control_dim> H;
+ H = Lr.template triangularView<Eigen::Lower>() * Lr.transpose(); // Lr is cholesky of H
+
+ // step2: reconstruct S[1]
+ Eigen::Matrix<double, state_dim, state_dim> Lq;
+ ::d_cvt_strmat2mat(Lq.rows(), Lq.cols(), &workspace_.L[1], control_dim, control_dim, Lq.data(), Lq.rows());
+ Eigen::Matrix<double, state_dim, state_dim> S;
+ S = Lq.template triangularView<Eigen::Lower>() * Lq.transpose(); // Lq is cholesky of S
+
+ // step3: compute G[0]
+ Eigen::Matrix<double, control_dim, state_dim> G;
+ G = p.P_[0];
+ G.noalias() += p.B_[0].transpose() * S * p.A_[0];
+
+ // step4: compute K[0]
+ K[0] = (-H.inverse() * G); // \todo use Lr here instead of H!
+
+
+ // for all other steps we can just read Ls
+ Eigen::Matrix<double, state_dim, control_dim> Ls;
+ for (int i = 1; i < this->lqocProblem_->getNumberOfStages(); i++)
+ {
+ ::d_cvt_strmat2mat(Lr.rows(), Lr.cols(), &workspace_.L[i], 0, 0, Lr.data(), Lr.rows());
+ ::d_cvt_strmat2mat(Ls.rows(), Ls.cols(), &workspace_.L[i], Lr.rows(), 0, Ls.data(), Ls.rows());
+ K[i] = (-Ls * Lr.partialPivLu().inverse()).transpose();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+ct::core::ControlVectorArray<CONTROL_DIM> HPIPMInterface<STATE_DIM, CONTROL_DIM>::getFeedforwardUpdates()
+{
+ throw std::runtime_error("HPIPMInterface: getFeedforwardUpdates Not implemented");
+
+ LQOCProblem<STATE_DIM, CONTROL_DIM>& p = *this->lqocProblem_;
+ ct::core::ControlVectorArray<CONTROL_DIM> lv(p.getNumberOfStages());
+
+ for (int i = 1; i < this->lqocProblem_->getNumberOfStages(); i++)
+ {
+ Eigen::Matrix<double, control_dim, control_dim> Lr;
+ ::d_cvt_strmat2mat(Lr.rows(), Lr.cols(), &workspace_.L[i], 0, 0, Lr.data(), Lr.rows());
+
+ Eigen::Matrix<double, 1, control_dim> llTranspose;
+ ::d_cvt_strmat2mat(llTranspose.rows(), llTranspose.cols(), &workspace_.L[i], control_dim + state_dim, 0,
+ llTranspose.data(), llTranspose.rows());
+
+ lv[i] = -Lr.transpose().inverse() * llTranspose.transpose();
+ }
+
+ return lv;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::printSolution()
+{
+ int ii;
+
+ ::d_cvt_ocp_qp_sol_to_colmaj(&qp_, &qp_sol_, u_.data(), x_.data(), pi_.data(), lam_lb_.data(), lam_ub_.data(),
+ lam_lg_.data(), lam_ug_.data());
+
+ printf("\nsolution\n\n");
+ printf("\nu\n");
+ for (ii = 0; ii <= N_; ii++)
+ d_print_mat(1, nu_[ii], u_[ii], 1);
+ printf("\nx\n");
+ for (ii = 0; ii <= N_; ii++)
+ d_print_mat(1, nx_[ii], x_[ii], 1);
+
+ /*
+ #if 1
+ printf("\npi\n");
+ for(ii=0; ii<N_; ii++)
+ d_print_mat(1, nx_[ii+1], pi[ii], 1);
+ printf("\nlam_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], lam_lb[ii], 1);
+ printf("\nlam_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], lam_ub[ii], 1);
+ printf("\nlam_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], lam_lg[ii], 1);
+ printf("\nlam_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], lam_ug[ii], 1);
+
+ printf("\nt_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], (qp_sol_.t_lb+ii)->pa, 1);
+ printf("\nt_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, nb_[ii], (qp_sol_.t_ub+ii)->pa, 1);
+ printf("\nt_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], (qp_sol_.t_lg+ii)->pa, 1);
+ printf("\nt_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_mat(1, ng_[ii], (qp_sol_.t_ug+ii)->pa, 1);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nu_[ii]+nx_[ii], (workspace_.res_g+ii)->pa, 1);
+ printf("\nres_b\n");
+ for(ii=0; ii<N_; ii++)
+ d_print_e_mat(1, nx_[ii+1], (workspace_.res_b+ii)->pa, 1);
+ printf("\nres_m_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_m_lb+ii)->pa, 1);
+ printf("\nres_m_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_m_ub+ii)->pa, 1);
+ printf("\nres_m_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_m_lg+ii)->pa, 1);
+ printf("\nres_m_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_m_ug+ii)->pa, 1);
+ printf("\nres_d_lb\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_d_lb+ii)->pa, 1);
+ printf("\nres_d_ub\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, nb_[ii], (workspace_.res_d_ub+ii)->pa, 1);
+ printf("\nres_d_lg\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_d_lg+ii)->pa, 1);
+ printf("\nres_d_ug\n");
+ for(ii=0; ii<=N_; ii++)
+ d_print_e_mat(1, ng_[ii], (workspace_.res_d_ug+ii)->pa, 1);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", workspace_.res_mu);
+ #endif
+ */
+
+ printf("\nipm iter = %d\n", workspace_.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ ::d_print_e_tran_mat(5, workspace_.iter, workspace_.stat, 5);
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setProblemImpl(
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+{
+ // check if the number of stages N changed and adapt problem dimensions
+ bool nStagesChanged = changeNumberOfStages(lqocProblem->getNumberOfStages());
+
+
+ // WARNING: the allocation should in practice not have to happen during the loop.
+ // If possible, prefer receding horizon MPC problems.
+ // If the number of stages has changed, however, the problem needs to be re-built:
+ if (nStagesChanged)
+ {
+ // update constraint configuration in case the horizon length has changed.
+ if (lqocProblem->isBoxConstrained())
+ configureBoxConstraints(lqocProblem);
+
+ if (lqocProblem->isGeneralConstrained())
+ configureGeneralConstraints(lqocProblem);
+ }
+
+ // we do not need to reset the pointers if
+ bool keepPointers = this->lqocProblem_ && //there was an lqocProblem before
+ N_ == lqocProblem->getNumberOfStages() && // and the number of states did not change
+ this->lqocProblem_ == lqocProblem; // and it was the same pointer
+
+ // setup unconstrained part of problem
+ setupCostAndDynamics(lqocProblem->x_, lqocProblem->u_, lqocProblem->A_, lqocProblem->B_, lqocProblem->b_,
+ lqocProblem->P_, lqocProblem->qv_, lqocProblem->Q_, lqocProblem->rv_, lqocProblem->R_, keepPointers);
+
+
+ if (nStagesChanged)
+ {
+ initializeAndAllocate();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureBoxConstraints(
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+{
+ // stages 1 to N
+ for (size_t i = 0; i < N_ + 1; i++)
+ {
+ nb_[i] = lqocProblem->nb_[i];
+
+ // set pointers to box constraint boundaries and sparsity pattern
+ hd_lb_[i] = lqocProblem->ux_lb_[i].data();
+ hd_ub_[i] = lqocProblem->ux_ub_[i].data();
+ hidxb_[i] = lqocProblem->ux_I_[i].data();
+
+ // first stage requires special treatment as state is not a decision variable
+ if (i == 0)
+ {
+ nb_[i] = 0;
+ for (int j = 0; j < lqocProblem->nb_[i]; j++)
+ {
+ if (lqocProblem->ux_I_[i](j) < CONTROL_DIM)
+ nb_[i]++; // adapt number of constraints such that only controls are listed as decision vars
+ else
+ break;
+ }
+ }
+
+ // TODO clarify with Gianluca if we need to reset the lagrange multiplier
+ // before warmstarting (potentially wrong warmstart for the lambdas)
+
+ // direct pointers of lagrange mult to corresponding containers
+ lam_lb_[i] = cont_lam_lb_[i].data();
+ lam_ub_[i] = cont_lam_ub_[i].data();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureGeneralConstraints(
+ std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+{
+ for (size_t i = 0; i < N_ + 1; i++)
+ {
+ // check dimensions
+ assert(lqocProblem->d_lb_[i].rows() == lqocProblem->d_ub_[i].rows());
+ assert(lqocProblem->d_lb_[i].rows() == lqocProblem->C_[i].rows());
+ assert(lqocProblem->d_lb_[i].rows() == lqocProblem->D_[i].rows());
+ assert(lqocProblem->C_[i].cols() == STATE_DIM);
+ assert(lqocProblem->D_[i].cols() == CONTROL_DIM);
+
+ // get the number of constraints
+ ng_[i] = lqocProblem->d_lb_[i].rows();
+
+ // set pointers to hpipm-style box constraint boundaries and sparsity pattern
+ hd_lg_[i] = lqocProblem->d_lb_[i].data();
+ hd_ug_[i] = lqocProblem->d_ub_[i].data();
+ hC_[i] = lqocProblem->C_[i].data();
+ hD_[i] = lqocProblem->D_[i].data();
+
+ // TODO clarify with Gianluca if we need to reset the lagrange multiplier
+ // before warmstarting (potentially wrong warmstart for the lambdas)
+
+ // direct pointers of lagrange mult to corresponding containers
+ cont_lam_lg_[i].resize(ng_[i]); // todo avoid dynamic allocation (e.g. by defining a max. constraint dim)
+ cont_lam_ug_[i].resize(ng_[i]); // todo avoid dynamic allocation (e.g. by defining a max. constraint dim)
+ lam_lg_[i] = cont_lam_lg_[i].data();
+ lam_ug_[i] = cont_lam_ug_[i].data();
+ }
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::setupCostAndDynamics(StateVectorArray& x,
+ ControlVectorArray& u,
+ StateMatrixArray& A,
+ StateControlMatrixArray& B,
+ StateVectorArray& b,
+ FeedbackArray& P,
+ StateVectorArray& qv,
+ StateMatrixArray& Q,
+ ControlVectorArray& rv,
+ ControlMatrixArray& R,
+ bool keepPointers)
+{
+ if (N_ == -1)
+ throw std::runtime_error("Time horizon not set, please set it first");
+
+ // set the initial state
+ x0_ = x[0].data();
+
+ /*
+ * transcribe the "differential" representation of the OptConProblem to the absolute origin of
+ * the linear system.
+ * Note: constant terms are not even handed over above (not important for solving LQ problem).
+ */
+
+ // STEP 1: transcription of affine system dynamics offset term
+ for (int i = 0; i < N_; i++)
+ {
+ bEigen_[i] = b[i] + x[i + 1] - A[i] * x[i] - B[i] * u[i];
+ }
+ hb0_ = b[0] + x[1] - B[0] * u[0]; // this line needs to be transcribed separately (correction for first stage)
+
+
+ // STEP 2: transcription of intermediate costs
+ for (int i = 0; i < N_; i++)
+ {
+ hqEigen_[i] = qv[i] - Q[i] * x[i] - P[i].transpose() * u[i];
+ hrEigen_[i] = rv[i] - R[i] * u[i] - P[i] * x[i];
+ }
+ hr0_ = hrEigen_[0] + P[0] * x[0]; // this line needs to be transcribed separately (correction for first stage)
+
+
+ // STEP 3: transcription of terminal cost terms
+ hqEigen_[N_] = qv[N_] - Q[N_] * x[N_];
+
+
+ // STEP 4: The following quantities remain unchanged when changing coordinate systems
+ if (!keepPointers)
+ {
+ for (int i = 0; i < N_; i++)
+ {
+ hA_[i] = A[i].data();
+ hB_[i] = B[i].data();
+ }
+
+ // intermediate cost hessians and cross-terms
+ for (int i = 0; i < N_; i++)
+ {
+ hQ_[i] = Q[i].data();
+ hS_[i] = P[i].data();
+ hR_[i] = R[i].data();
+ }
+
+ // final cost hessian state
+ hQ_[N_] = Q[N_].data();
+ }
+
+ // reset lqocProblem pointer, will get set in Base class if needed
+ this->lqocProblem_ = nullptr;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+bool HPIPMInterface<STATE_DIM, CONTROL_DIM>::changeNumberOfStages(int N)
+{
+ if (N_ == N)
+ return false; // return since problem is already correctly sized
+
+ N_ = N;
+
+ this->lx_.resize(N + 1);
+ this->lu_.resize(N);
+
+ nx_.resize(N_ + 1, STATE_DIM); // initialize number of states per stage
+ nu_.resize(N_ + 1, CONTROL_DIM); // initialize number of control inputs per stage
+ nb_.resize(N_ + 1, nb_.back()); // initialize number of box constraints per stage
+ ng_.resize(N_ + 1, ng_.back()); // initialize number of general constraints per stage
+
+ // resize the containers for the affine system dynamics approximation
+ hA_.resize(N_);
+ hB_.resize(N_);
+ bEigen_.resize(N_);
+ hb_.resize(N_);
+
+ // resize the containers for the LQ cost approximation
+ hQ_.resize(N_ + 1);
+ hS_.resize(N_ + 1);
+ hR_.resize(N_ + 1);
+ hqEigen_.resize(N_ + 1);
+ hq_.resize(N_ + 1);
+ hrEigen_.resize(N_ + 1);
+ hr_.resize(N_ + 1);
+
+ hd_lb_.resize(N_ + 1);
+ hd_ub_.resize(N_ + 1);
+ hidxb_.resize(N_ + 1);
+ hd_lg_.resize(N_ + 1);
+ hd_ug_.resize(N_ + 1);
+ hC_.resize(N_ + 1);
+ hD_.resize(N_ + 1);
+
+ u_.resize(N_ + 1);
+ x_.resize(N_ + 1);
+ pi_.resize(N_);
+ hx_.resize(N_ + 1);
+ hpi_.resize(N_);
+ hu_.resize(N_);
+
+ lam_lb_.resize(N_ + 1);
+ lam_ub_.resize(N_ + 1);
+ lam_lg_.resize(N_ + 1);
+ lam_ug_.resize(N_ + 1);
+ cont_lam_lb_.resize(N_ + 1);
+ cont_lam_ub_.resize(N_ + 1);
+ cont_lam_lg_.resize(N_ + 1);
+ cont_lam_ug_.resize(N_ + 1);
+
+ for (int i = 0; i < N_; i++)
+ {
+ // first state and last input are not optimized
+ x_[i + 1] = hx_[i + 1].data();
+ u_[i] = hu_[i].data();
+ }
+ for (int i = 0; i < N_; i++)
+ {
+ pi_[i] = hpi_[i].data();
+ }
+ for (int i = 0; i < N_; i++)
+ {
+ hq_[i] = hqEigen_[i].data();
+ hr_[i] = hrEigen_[i].data();
+ hb_[i] = bEigen_[i].data();
+ }
+
+ hS_[N_] = nullptr;
+ hR_[N_] = nullptr;
+
+ hq_[N_] = hqEigen_[N_].data();
+ hr_[N_] = nullptr;
+
+ hb_[0] = hb0_.data();
+ hr_[0] = hr0_.data();
+
+ ct::core::StateVectorArray<STATE_DIM> hx;
+ ct::core::ControlVectorArray<CONTROL_DIM> hu;
+
+ // initial state is not a decision variable but given
+ nx_[0] = 0;
+
+ // last input is not a decision variable
+ nu_[N] = 0;
+
+ return true;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_zeros(double** pA, int row, int col)
+{
+ *pA = (double*)malloc((row * col) * sizeof(double));
+ double* A = *pA;
+ int i;
+ for (i = 0; i < row * col; i++)
+ A[i] = 0.0;
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_mat(int m, int n, double* A, int lda)
+{
+ int i, j;
+ for (i = 0; i < m; i++)
+ {
+ for (j = 0; j < n; j++)
+ {
+ printf("%9.5f ", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_tran_mat(int row, int col, double* A, int lda)
+{
+ int i, j;
+ for (j = 0; j < col; j++)
+ {
+ for (i = 0; i < row; i++)
+ {
+ printf("%9.5f ", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_e_mat(int m, int n, double* A, int lda)
+{
+ int i, j;
+ for (i = 0; i < m; i++)
+ {
+ for (j = 0; j < n; j++)
+ {
+ printf("%e\t", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::d_print_e_tran_mat(int row, int col, double* A, int lda)
+{
+ int i, j;
+ for (j = 0; j < col; j++)
+ {
+ for (i = 0; i < row; i++)
+ {
+ printf("%e\t", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+
+template <int STATE_DIM, int CONTROL_DIM>
+void HPIPMInterface<STATE_DIM, CONTROL_DIM>::int_print_mat(int row, int col, int* A, int lda)
+{
+ int i, j;
+ for (i = 0; i < row; i++)
+ {
+ for (j = 0; j < col; j++)
+ {
+ printf("%d ", A[i + lda * j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+}
+
+} // namespace optcon
+} // namespace ct
+
+#endif // HPIPM
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
new file mode 100644
index 0000000..ac1a403
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp
@@ -0,0 +1,292 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include "LQOCSolver.hpp"
+
+#ifdef HPIPM
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_d_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.h>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_d_blas.h>
+
+extern "C" {
+#include <hpipm_d_ocp_qp.h>
+#include <hpipm_d_ocp_qp_sol.h>
+#include <hpipm_d_ocp_qp_ipm_hard.h>
+}
+
+#include <unsupported/Eigen/MatrixFunctions>
+
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * This class implements an interface to the HPIPM solver
+ *
+ * \warning in order to allow for an efficient implementation of constrained MPC,
+ * the configuration of the box and general constraints must be done independently
+ * from setProblem()
+ */
+template <int STATE_DIM, int CONTROL_DIM>
+class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const int state_dim = STATE_DIM;
+ static const int control_dim = CONTROL_DIM;
+ static const int max_box_constr_dim = STATE_DIM + CONTROL_DIM;
+
+ using StateMatrix = ct::core::StateMatrix<STATE_DIM>;
+ using StateMatrixArray = ct::core::StateMatrixArray<STATE_DIM>;
+ using ControlMatrix = ct::core::ControlMatrix<CONTROL_DIM>;
+ using ControlMatrixArray = ct::core::ControlMatrixArray<CONTROL_DIM>;
+ using StateControlMatrixArray = ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM>;
+ using FeedbackArray = ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM>;
+
+ using StateVectorArray = ct::core::StateVectorArray<STATE_DIM>;
+ using ControlVectorArray = ct::core::ControlVectorArray<CONTROL_DIM>;
+
+ // definitions for variable-size constraints
+ using constr_vec_t = Eigen::Matrix<double, -1, 1>;
+ using constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t>;
+
+
+ //! typedef a container for a sparsity pattern vector for box constraints
+ using box_constr_sparsity_t = Eigen::Matrix<int, max_box_constr_dim, 1>;
+
+ //! constructor
+ HPIPMInterface(int N = -1);
+
+ //! destructor
+ virtual ~HPIPMInterface();
+
+ virtual void configure(const NLOptConSettings& settings) override;
+
+ void solve() override;
+
+ virtual void computeStateAndControlUpdates() override;
+
+ virtual ct::core::StateVectorArray<STATE_DIM> getSolutionState() override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM> getSolutionControl() override;
+
+ virtual void getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM>& K) override;
+
+ virtual ct::core::ControlVectorArray<CONTROL_DIM> getFeedforwardUpdates() override;
+
+ void printSolution();
+
+ //! brief setup and configure the box constraints
+ virtual void configureBoxConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+
+ //! brief setup and configure the general (in)equality constraints
+ virtual void configureGeneralConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+
+ /*!
+ * @brief allocate memory for HPIPM
+ *
+ * \warning These functions are assumed to be used only outside the control loop.
+ * Their intended use is to call them before the loop to setup the memory needed by
+ * the QP structures and solver (dynamic memory allocation is time consuming).
+ *
+ * Needs to be called when
+ * - the number of stages changes
+ * - the box constraint configuration changes
+ * - the general constraint configuration changes
+ */
+ virtual void initializeAndAllocate() override;
+
+private:
+ /*!
+ * @brief set problem implementation for hpipm
+ * \warning This method is called in the loop. As little memory as possible
+ * should be allocated in this function. Ideally this method only sets pointers.
+ *
+ * \warning If you wish to
+ */
+ void setProblemImpl(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
+
+ /*!
+ * @brief transcribe the problem from original local formulation to HPIPM's global coordinates
+ *
+ * See also the description of the LQOC Problem in class LQOCProblem.h
+ *
+ * @param x current state reference \f$ \hat \mathbf x_n \f$
+ * @param u current control reference \f$ \hat \mathbf u_n \f$
+ * @param A affine system matrices \f$ \mathbf A_n \f$
+ * @param B affine system matrices \f$ \mathbf B_n \f$
+ * @param b affine system matrix \f$ \mathbf b_n \f$
+ * @param P mixed cost term \f$ \mathbf P_n \f$
+ * @param qv pure state-cost term \f$ \mathbf q_n \f$ (first order derivative)
+ * @param Q pure state-cost term \f$ \mathbf Q_n \f$ (second order derivative)
+ * @param rv pure input-cost term \f$ \mathbf r_n \f$ (first order derivative)
+ * @param R pure input-cost term \f$ \mathbf R_n \f$ (second order derivative)
+ * @param keepPointers keep pointers
+ *
+ *
+ * This method needs change coordinate systems, in the sense that
+ * \f[
+ * \mathbf x_{n+1} = \mathbf A_n \mathbf x_n + \mathbf B_n \mathbf u_n +\mathbf b_n
+ * + \hat \mathbf x_{n+1} - \mathbf A_n \hat \mathbf x_n - \mathbf B_n \hat \mathbf u_n
+ * \f]
+ */
+ void setupCostAndDynamics(StateVectorArray& x,
+ ControlVectorArray& u,
+ StateMatrixArray& A,
+ StateControlMatrixArray& B,
+ StateVectorArray& b,
+ FeedbackArray& P,
+ StateVectorArray& qv,
+ StateMatrixArray& Q,
+ ControlVectorArray& rv,
+ ControlMatrixArray& R,
+ bool keepPointers = false);
+
+ /*!
+ * @brief change number of states of the optimal control problem
+ * @return true if number of stages changed, false if number of stages is unchanged.
+ */
+ bool changeNumberOfStages(int N);
+
+ //! creates a zero matrix
+ void d_zeros(double** pA, int row, int col);
+
+ //! prints a matrix in column-major format
+ void d_print_mat(int m, int n, double* A, int lda);
+
+ //! prints a matrix in column-major format (exponential notation)
+ void d_print_e_mat(int m, int n, double* A, int lda);
+
+ //! prints the transposed of a matrix in column-major format (exponential notation)
+ void d_print_e_tran_mat(int row, int col, double* A, int lda);
+
+ //! prints the transposed of a matrix in column-major format
+ void d_print_tran_mat(int row, int col, double* A, int lda);
+
+ //! prints a matrix in column-major format
+ void int_print_mat(int row, int col, int* A, int lda);
+
+ //! horizon length
+ int N_;
+
+ //! number of states per stage
+ std::vector<int> nx_;
+ //! number of inputs per stage
+ std::vector<int> nu_;
+
+ //! number of box constraints per stage
+ std::vector<int> nb_;
+ //! number of general constraints per stage
+ std::vector<int> ng_;
+
+ //! initial state
+ double* x0_;
+
+ //! system state sensitivities
+ std::vector<double*> hA_;
+ //! system input sensitivities
+ std::vector<double*> hB_;
+ //! system offset term
+ std::vector<double*> hb_;
+ //! intermediate container for intuitive transcription of system representation from local to global coordinates
+ StateVectorArray bEigen_;
+ //! intermediate container for intuitive transcription of first stage from local to global coordinates
+ Eigen::Matrix<double, state_dim, 1> hb0_;
+
+
+ //! pure state penalty hessian
+ std::vector<double*> hQ_;
+ //! state-control cross-terms
+ std::vector<double*> hS_;
+ //! pure control penalty hessian
+ std::vector<double*> hR_;
+ //! pure state penalty jacobian
+ std::vector<double*> hq_;
+ //! pure control penalty jacobian
+ std::vector<double*> hr_;
+ //! intermediate container for intuitive transcription of first stage from local to global coordinates
+ Eigen::Matrix<double, control_dim, 1> hr0_;
+ //! interm. container for intuitive transcription of 1st order state penalty from local to global coordinates
+ StateVectorArray hqEigen_;
+ //! interm. container for intuitive transcription of 1st order control penalty from local to global coordinates
+ ControlVectorArray hrEigen_;
+
+
+ //! pointer to lower box constraint boundary
+ std::vector<double*> hd_lb_;
+ //! pointer to upper box constraint boundary
+ std::vector<double*> hd_ub_;
+ //! pointer to sparsity pattern for box constraints
+ std::vector<int*> hidxb_;
+
+ //! lower general constraint boundary
+ std::vector<double*> hd_lg_;
+ //! upper general constraint boundary
+ std::vector<double*> hd_ug_;
+ //! general constraint jacobians w.r.t. states
+ std::vector<double*> hC_;
+ //! general constraint jacobians w.r.t. controls (presumably)
+ std::vector<double*> hD_;
+
+
+ /*
+ * SOLUTION variables
+ */
+ //! optimal control trajectory
+ std::vector<double*> u_;
+ //! optimal state trajectory
+ std::vector<double*> x_;
+ //! @todo what is this ?
+ std::vector<double*> pi_;
+ //! ptr to lagrange multiplier box-constraint lower
+ std::vector<double*> lam_lb_;
+ //! ptr to lagrange multiplier box-constraint upper
+ std::vector<double*> lam_ub_;
+ //! ptr to lagrange multiplier general-constraint lower
+ std::vector<double*> lam_lg_;
+ //! ptr to lagrange multiplier general-constraint upper
+ std::vector<double*> lam_ug_;
+
+ //! container lagr. mult. box-constr. lower
+ ct::core::DiscreteArray<Eigen::Matrix<double, max_box_constr_dim, 1>> cont_lam_lb_;
+ //! container lagr. mult. box-constr. upper
+ ct::core::DiscreteArray<Eigen::Matrix<double, max_box_constr_dim, 1>> cont_lam_ub_;
+ //! container for lagr. mult. general-constraint lower
+ ct::core::DiscreteArray<Eigen::Matrix<double, -1, 1>> cont_lam_lg_;
+ //! container for lagr. mult. general-constraint upper
+ ct::core::DiscreteArray<Eigen::Matrix<double, -1, 1>> cont_lam_ug_;
+
+ ct::core::StateVectorArray<STATE_DIM> hx_;
+ ct::core::StateVectorArray<STATE_DIM> hpi_;
+ ct::core::ControlVectorArray<CONTROL_DIM> hu_;
+
+ //! settings from NLOptConSolver
+ NLOptConSettings settings_;
+
+ std::vector<char> qp_mem_;
+ struct d_ocp_qp qp_;
+
+ std::vector<char> qp_sol_mem_;
+ struct d_ocp_qp_sol qp_sol_;
+
+ struct d_ipm_hard_ocp_qp_arg arg_; //! IPM settings
+ std::vector<char> ipm_mem_;
+ struct d_ipm_hard_ocp_qp_workspace workspace_;
+};
+
+
+} // namespace optcon
+} // namespace ct
+
+#endif // HPIPM
diff --git a/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp b/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp
new file mode 100644
index 0000000..7143b29
--- /dev/null
+++ b/ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp
@@ -0,0 +1,104 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#include <ct/optcon/solver/NLOptConSettings.hpp>
+
+#include <ct/optcon/problem/LQOCProblem.hpp>
+
+namespace ct {
+namespace optcon {
+
+/*!
+ * Base class for solvers to solve an LQOCProblem
+ * (both constrained / unconstrained, etc.)
+ *
+ * \todo uncouple from NLOptConSettings
+ */
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class LQOCSolver
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
+
+ /*!
+ * Constructor. Initialize by handing over an LQOCProblem, or otherwise by calling setProblem()
+ * @param lqocProblem shared_ptr to the LQOCProblem to be solved.
+ */
+ LQOCSolver(const std::shared_ptr<LQOCProblem_t>& lqocProblem = nullptr) : lqocProblem_(lqocProblem) {}
+ //! destructor
+ virtual ~LQOCSolver() {}
+ /*!
+ * set a new problem
+ * update the shared_ptr to the LQOCProblem instance and call initialize instance deriving from this class.
+ * @param lqocProblem
+ */
+ void setProblem(std::shared_ptr<LQOCProblem_t> lqocProblem)
+ {
+ setProblemImpl(lqocProblem);
+ lqocProblem_ = lqocProblem;
+ }
+
+
+ virtual void configure(const NLOptConSettings& settings) = 0;
+
+ //! setup and configure the box constraints
+ virtual void configureBoxConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+ {
+ throw std::runtime_error("box constraints are not available for this solver.");
+ }
+
+ //! setup and configure the general (in)equality constraints
+ virtual void configureGeneralConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
+ {
+ throw std::runtime_error("general constraints are not available for this solver.");
+ }
+
+ //! a method reserved for memory allocation (e.g. required for HPIPM)
+ virtual void initializeAndAllocate() = 0;
+
+ //! solve the LQOC problem
+ virtual void solve() = 0;
+
+ virtual void solveSingleStage(int N)
+ {
+ throw std::runtime_error("solveSingleStage not available for this solver.");
+ }
+
+ virtual SCALAR getSmallestEigenvalue()
+ {
+ throw std::runtime_error("getSmallestEigenvalue not available for this solver.");
+ }
+
+ virtual void computeStateAndControlUpdates() = 0;
+
+ virtual ct::core::StateVectorArray<STATE_DIM, SCALAR> getSolutionState() = 0;
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getSolutionControl() = 0;
+
+ virtual void getFeedback(ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>& K) = 0;
+
+ const SCALAR& getControlUpdateNorm() { return delta_uff_norm_; }
+ const SCALAR& getStateUpdateNorm() { return delta_x_norm_; }
+ const core::StateVectorArray<STATE_DIM, SCALAR>& getStateUpdates() { return lx_; }
+ const core::ControlVectorArray<CONTROL_DIM, SCALAR>& getControlUpdates() { return lu_; }
+ virtual ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> getFeedforwardUpdates() = 0;
+
+protected:
+ virtual void setProblemImpl(std::shared_ptr<LQOCProblem_t> lqocProblem) = 0;
+
+ std::shared_ptr<LQOCProblem_t> lqocProblem_;
+
+ core::StateVectorArray<STATE_DIM, SCALAR> lx_; // differential update on the state
+ core::ControlVectorArray<CONTROL_DIM, SCALAR> lu_; // differential update on the control
+
+ SCALAR delta_uff_norm_; //! l2-norm of the control update
+ SCALAR delta_x_norm_; //! l2-norm of the state update
+};
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/package.xml b/ct_optcon/package.xml
new file mode 100644
index 0000000..9c8ac4a
--- /dev/null
+++ b/ct_optcon/package.xml
@@ -0,0 +1,23 @@
+<package>
+ <name>ct_optcon</name>
+ <version>0.2.1</version>
+ <description>Optimal Control package of the Control Toolbox</description>
+ <maintainer email="mgiftthaler@ethz.ch">Markus Giftthaler</maintainer>
+ <maintainer email="neunertm@gmail.com">Michael Neunert</maintainer>
+ <license>Apache v2</license>
+
+ <url>http://ros.org/wiki/foo_core</url>
+ <author>Markus Giftthaler</author>
+ <author>Michael Neunert</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>roscpp</build_depend>
+ <build_depend>ct_core</build_depend>
+ <build_depend>cmake_modules</build_depend>
+ <build_depend>lapack</build_depend>
+
+ <run_depend>ct_core</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>lapack</run_depend>
+</package>
diff --git a/ct_optcon/prespec/constraint/ConstraintContainerAD.cpp.in b/ct_optcon/prespec/constraint/ConstraintContainerAD.cpp.in
new file mode 100644
index 0000000..0c7ff3b
--- /dev/null
+++ b/ct_optcon/prespec/constraint/ConstraintContainerAD.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/ConstraintContainerAD-impl.h>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::ConstraintContainerAD<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/constraint/ConstraintContainerAnalytical.cpp.in b/ct_optcon/prespec/constraint/ConstraintContainerAnalytical.cpp.in
new file mode 100644
index 0000000..1c7a0a0
--- /dev/null
+++ b/ct_optcon/prespec/constraint/ConstraintContainerAnalytical.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/ConstraintContainerAnalytical-impl.h>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::ConstraintContainerAnalytical<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/constraint/ConstraintContainerBase.cpp.in b/ct_optcon/prespec/constraint/ConstraintContainerBase.cpp.in
new file mode 100644
index 0000000..736ca3b
--- /dev/null
+++ b/ct_optcon/prespec/constraint/ConstraintContainerBase.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/ConstraintContainerBase-impl.h>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::ConstraintContainerBase<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/constraint/LinearConstraintContainer.cpp.in b/ct_optcon/prespec/constraint/LinearConstraintContainer.cpp.in
new file mode 100644
index 0000000..cba7b45
--- /dev/null
+++ b/ct_optcon/prespec/constraint/LinearConstraintContainer.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/LinearConstraintContainer-impl.h>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::LinearConstraintContainer<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/constraint/term/ConstraintBase.cpp.in b/ct_optcon/prespec/constraint/term/ConstraintBase.cpp.in
new file mode 100644
index 0000000..c864db5
--- /dev/null
+++ b/ct_optcon/prespec/constraint/term/ConstraintBase.cpp.in
@@ -0,0 +1,4 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/term/ConstraintBase-impl.h>
+
+template class ct::optcon::ConstraintBase<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
diff --git a/ct_optcon/prespec/constraint/term/ControlInputConstraint.cpp.in b/ct_optcon/prespec/constraint/term/ControlInputConstraint.cpp.in
new file mode 100644
index 0000000..c3fd567
--- /dev/null
+++ b/ct_optcon/prespec/constraint/term/ControlInputConstraint.cpp.in
@@ -0,0 +1,4 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/term/ControlInputConstraint-impl.h>
+
+template class ct::optcon::ControlInputConstraint<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
diff --git a/ct_optcon/prespec/constraint/term/ObstacleConstraint.cpp.in b/ct_optcon/prespec/constraint/term/ObstacleConstraint.cpp.in
new file mode 100644
index 0000000..9a80159
--- /dev/null
+++ b/ct_optcon/prespec/constraint/term/ObstacleConstraint.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/term/ObstacleConstraint-impl.h>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::ObstacleConstraint<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/constraint/term/StateConstraint.cpp.in b/ct_optcon/prespec/constraint/term/StateConstraint.cpp.in
new file mode 100644
index 0000000..ee36548
--- /dev/null
+++ b/ct_optcon/prespec/constraint/term/StateConstraint.cpp.in
@@ -0,0 +1,4 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/term/StateConstraint-impl.h>
+
+template class ct::optcon::StateConstraint<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
diff --git a/ct_optcon/prespec/constraint/term/TerminalConstraint.cpp.in b/ct_optcon/prespec/constraint/term/TerminalConstraint.cpp.in
new file mode 100644
index 0000000..68bf926
--- /dev/null
+++ b/ct_optcon/prespec/constraint/term/TerminalConstraint.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/constraint/term/TerminalConstraint-impl.h>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::TerminalConstraint<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/lqr/FHDTLQR.cpp.in b/ct_optcon/prespec/lqr/FHDTLQR.cpp.in
new file mode 100644
index 0000000..4efe869
--- /dev/null
+++ b/ct_optcon/prespec/lqr/FHDTLQR.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/lqr/FHDTLQR-impl.hpp>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::FHDTLQR<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/lqr/LQR.cpp.in b/ct_optcon/prespec/lqr/LQR.cpp.in
new file mode 100644
index 0000000..fbc35ec
--- /dev/null
+++ b/ct_optcon/prespec/lqr/LQR.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/lqr/LQR-impl.hpp>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::LQR<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/lqr/riccati/CARE.cpp.in b/ct_optcon/prespec/lqr/riccati/CARE.cpp.in
new file mode 100644
index 0000000..ed8c62f
--- /dev/null
+++ b/ct_optcon/prespec/lqr/riccati/CARE.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/lqr/riccati/CARE-impl.hpp>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::CARE<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/lqr/riccati/DARE.cpp.in b/ct_optcon/prespec/lqr/riccati/DARE.cpp.in
new file mode 100644
index 0000000..335485d
--- /dev/null
+++ b/ct_optcon/prespec/lqr/riccati/DARE.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/lqr/riccati/DARE-impl.hpp>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::DARE<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/mpc/MPC.cpp.in b/ct_optcon/prespec/mpc/MPC.cpp.in
new file mode 100644
index 0000000..4309114
--- /dev/null
+++ b/ct_optcon/prespec/mpc/MPC.cpp.in
@@ -0,0 +1,9 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/mpc/MPC-impl.h>
+
+
+// default definition of MPC solver template
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@ && @DOUBLE_OR_FLOAT@
+ #define MPC_SOLVER_PRESPEC ct::optcon::NLOptConSolver<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>
+ template class ct::optcon::MPC<MPC_SOLVER_PRESPEC>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/mpc/policyhandler/PolicyHandler.cpp.in b/ct_optcon/prespec/mpc/policyhandler/PolicyHandler.cpp.in
new file mode 100644
index 0000000..540fb5d
--- /dev/null
+++ b/ct_optcon/prespec/mpc/policyhandler/PolicyHandler.cpp.in
@@ -0,0 +1,12 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/mpc/policyhandler/PolicyHandler-impl.h>
+
+
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@ && @DOUBLE_OR_FLOAT@
+
+#define MPC_SOLVER_PRESPEC ct::optcon::NLOptConSolver<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>
+#define MPC_POLICY_PRESPEC MPC_SOLVER_PRESPEC::Policy_t
+
+template class ct::optcon::PolicyHandler<MPC_POLICY_PRESPEC, @STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/mpc/policyhandler/default/StateFeedbackPolicyHandler.cpp.in b/ct_optcon/prespec/mpc/policyhandler/default/StateFeedbackPolicyHandler.cpp.in
new file mode 100644
index 0000000..c820668
--- /dev/null
+++ b/ct_optcon/prespec/mpc/policyhandler/default/StateFeedbackPolicyHandler.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h>
+
+#if @DOUBLE_OR_FLOAT@
+template class ct::optcon::StateFeedbackPolicyHandler<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/mpc/timehorizon/MpcTimeHorizon.cpp.in b/ct_optcon/prespec/mpc/timehorizon/MpcTimeHorizon.cpp.in
new file mode 100644
index 0000000..0db1faa
--- /dev/null
+++ b/ct_optcon/prespec/mpc/timehorizon/MpcTimeHorizon.cpp.in
@@ -0,0 +1,4 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h>
+
+template class ct::optcon::tpl::MpcTimeHorizon<@SCALAR_PRESPEC@>;
\ No newline at end of file
diff --git a/ct_optcon/prespec/nloc/NLOCBackendBase.cpp.in b/ct_optcon/prespec/nloc/NLOCBackendBase.cpp.in
new file mode 100644
index 0000000..ffd9634
--- /dev/null
+++ b/ct_optcon/prespec/nloc/NLOCBackendBase.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/nloc/NLOCBackendBase-impl.hpp>
+
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@ && @DOUBLE_OR_FLOAT@
+template class ct::optcon::NLOCBackendBase<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/nloc/NLOCBackendMP.cpp.in b/ct_optcon/prespec/nloc/NLOCBackendMP.cpp.in
new file mode 100644
index 0000000..51b9d8f
--- /dev/null
+++ b/ct_optcon/prespec/nloc/NLOCBackendMP.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/nloc/NLOCBackendMP-impl.hpp>
+
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@ && @DOUBLE_OR_FLOAT@
+template class ct::optcon::NLOCBackendMP<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/nloc/NLOCBackendST.cpp.in b/ct_optcon/prespec/nloc/NLOCBackendST.cpp.in
new file mode 100644
index 0000000..c2e38e9
--- /dev/null
+++ b/ct_optcon/prespec/nloc/NLOCBackendST.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/nloc/NLOCBackendST-impl.hpp>
+
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@ && @DOUBLE_OR_FLOAT@
+template class ct::optcon::NLOCBackendST<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/nloc/algorithms/gnms/GNMS.cpp.in b/ct_optcon/prespec/nloc/algorithms/gnms/GNMS.cpp.in
new file mode 100644
index 0000000..4728613
--- /dev/null
+++ b/ct_optcon/prespec/nloc/algorithms/gnms/GNMS.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp>
+
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@ && @DOUBLE_OR_FLOAT@
+template class ct::optcon::GNMS<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/nloc/algorithms/ilqr/iLQR.cpp.in b/ct_optcon/prespec/nloc/algorithms/ilqr/iLQR.cpp.in
new file mode 100644
index 0000000..009b9e3
--- /dev/null
+++ b/ct_optcon/prespec/nloc/algorithms/ilqr/iLQR.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp>
+
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@ && @DOUBLE_OR_FLOAT@
+template class ct::optcon::iLQR<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/problem/LQOCProblem.cpp.in b/ct_optcon/prespec/problem/LQOCProblem.cpp.in
new file mode 100644
index 0000000..669fb63
--- /dev/null
+++ b/ct_optcon/prespec/problem/LQOCProblem.cpp.in
@@ -0,0 +1,4 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/problem/LQOCProblem-impl.hpp>
+
+template class ct::optcon::LQOCProblem<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
\ No newline at end of file
diff --git a/ct_optcon/prespec/problem/OptConProblem.cpp.in b/ct_optcon/prespec/problem/OptConProblem.cpp.in
new file mode 100644
index 0000000..243c617
--- /dev/null
+++ b/ct_optcon/prespec/problem/OptConProblem.cpp.in
@@ -0,0 +1,4 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/problem/OptConProblem-impl.h>
+
+template class ct::optcon::OptConProblem<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
\ No newline at end of file
diff --git a/ct_optcon/prespec/solver/NLOptConSolver.cpp.in b/ct_optcon/prespec/solver/NLOptConSolver.cpp.in
new file mode 100644
index 0000000..aa74eac
--- /dev/null
+++ b/ct_optcon/prespec/solver/NLOptConSolver.cpp.in
@@ -0,0 +1,6 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/solver/NLOptConSolver-impl.hpp>
+
+#if @POS_DIM_PRESPEC@ && @VEL_DIM_PRESPEC@
+template class ct::optcon::NLOptConSolver<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @POS_DIM_PRESPEC@, @VEL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
+#endif
\ No newline at end of file
diff --git a/ct_optcon/prespec/solver/lqp/GNRiccatiSolver.cpp.in b/ct_optcon/prespec/solver/lqp/GNRiccatiSolver.cpp.in
new file mode 100644
index 0000000..9902729
--- /dev/null
+++ b/ct_optcon/prespec/solver/lqp/GNRiccatiSolver.cpp.in
@@ -0,0 +1,4 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp>
+
+template class ct::optcon::GNRiccatiSolver<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@, @SCALAR_PRESPEC@>;
\ No newline at end of file
diff --git a/ct_optcon/prespec/solver/lqp/HPIPMInterface.cpp.in b/ct_optcon/prespec/solver/lqp/HPIPMInterface.cpp.in
new file mode 100644
index 0000000..9a9ce8f
--- /dev/null
+++ b/ct_optcon/prespec/solver/lqp/HPIPMInterface.cpp.in
@@ -0,0 +1,10 @@
+#include <ct/optcon/optcon-prespec.h>
+#include <ct/optcon/solver/lqp/HPIPMInterface-impl.hpp>
+
+#if @DOUBLE_OR_FLOAT@
+#ifdef HPIPM
+
+template class ct::optcon::HPIPMInterface<@STATE_DIM_PRESPEC@, @CONTROL_DIM_PRESPEC@>;
+
+#endif // HPIPM
+#endif // DOUBLE_OR_FLOAT
\ No newline at end of file
diff --git a/ct_optcon/src/nlp/solver/SnoptSolver.cpp b/ct_optcon/src/nlp/solver/SnoptSolver.cpp
new file mode 100644
index 0000000..ded43c1
--- /dev/null
+++ b/ct_optcon/src/nlp/solver/SnoptSolver.cpp
@@ -0,0 +1,361 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+
+namespace ct {
+namespace optcon {
+
+
+SnoptMemory::SnoptMemory(const SnoptSolver &self) : self(self)
+{
+ // Put in memory pool
+ auto mem_it = std::find(mempool.begin(), mempool.end(), nullptr);
+ if (mem_it == mempool.end())
+ {
+ // Append to end
+ memind = mempool.size();
+ mempool.push_back(this);
+ }
+ else
+ {
+ // Reuse freed element
+ memind = mem_it - mempool.begin();
+ *mem_it = this;
+ }
+}
+
+
+SnoptMemory::~SnoptMemory()
+{
+ // Remove from memory pool
+ auto mem_it = std::find(mempool.begin(), mempool.end(), this);
+ if (mem_it == mempool.end())
+ {
+ // Should probably shut down program
+ std::cout << "Error while destroying SNOPT memory" << std::endl;
+ }
+ else
+ {
+ delete[] iAfun_;
+ delete[] jAvar_;
+ delete[] A_;
+
+ delete[] x_;
+ delete[] xlow_;
+ delete[] xupp_;
+ delete[] xmul_;
+ delete[] xstate_;
+
+ delete[] F_;
+ delete[] Flow_;
+ delete[] Fupp_;
+ delete[] Fmul_;
+ delete[] Fstate_;
+
+ delete[] iGfun_;
+ delete[] jGvar_;
+
+ *mem_it = nullptr;
+ }
+}
+
+
+SnoptSolver::SnoptSolver(std::shared_ptr<Nlp> nlp, const NlpSolverSettings &settings)
+ : BASE(nlp, settings), settings_(BASE::settings_.snoptSettings_), memoryPtr_(nullptr)
+{
+ memoryPtr_ = alloc_memory();
+ currStartOption_ = Cold_;
+}
+
+SnoptSolver::~SnoptSolver()
+{
+ free_memory(memoryPtr_);
+}
+
+
+void SnoptSolver::init_memory(SnoptMemory *mem) const
+{
+ mem->x_ = new Number[n_];
+ mem->xlow_ = new Number[n_];
+ mem->xupp_ = new Number[n_];
+ mem->xmul_ = new Number[n_];
+ mem->xstate_ = new int[n_];
+
+ mem->F_ = new Number[neF_];
+ mem->Flow_ = new Number[neF_];
+ mem->Fupp_ = new Number[neF_];
+ mem->Fmul_ = new Number[neF_];
+ mem->Fstate_ = new int[neF_];
+
+ mem->iAfun_ = new int[lenA_];
+ mem->jAvar_ = new int[lenA_];
+ mem->A_ = new Number[lenA_];
+
+ mem->iGfun_ = new int[lenG_];
+ mem->jGvar_ = new int[lenG_];
+}
+
+
+void SnoptSolver::fill_memory(SnoptMemory *mem) const
+{
+ MapVecXd xVec(mem->x_, n_);
+ MapVecXd x_lVec(mem->xlow_, n_);
+ MapVecXd x_uVec(mem->xupp_, n_);
+ MapVecXd xMulVec(mem->xmul_, n_);
+ MapVecXi xStateVec(mem->xstate_, n_);
+
+ MapVecXd FVec(mem->F_, neF_);
+ MapVecXd f_lVec(&mem->Flow_[1], nlp_->getConstraintsCount()); // map from the second element
+ MapVecXd f_uVec(&mem->Fupp_[1], nlp_->getConstraintsCount()); // map from the second element
+ MapVecXd fMulVec(mem->Fmul_, neF_);
+ MapVecXi FStateVec(mem->Fstate_, neF_);
+
+ MapVecXi iAVec(mem->iAfun_, lenA_);
+ MapVecXi jAVec(mem->jAvar_, lenA_);
+ MapVecXd AVec(mem->A_, lenA_);
+
+ MapVecXi iGVecFull(mem->iGfun_, lenG_);
+ MapVecXi jGVecFull(mem->jGvar_, lenG_);
+ MapVecXi iGVecJac(&mem->iGfun_[n_], nlp_->getNonZeroJacobianCount());
+ MapVecXi jGVecGrad(mem->jGvar_, n_);
+ MapVecXi jGVecJac(&mem->jGvar_[n_], nlp_->getNonZeroJacobianCount());
+
+ nlp_->getInitialGuess(n_, xVec);
+ nlp_->getVariableBounds(x_lVec, x_uVec, n_);
+ nlp_->getOptimizationMultState(n_, xMulVec, xStateVec);
+
+ FVec.setZero();
+ mem->Flow_[0] = std::numeric_limits<double>::lowest();
+ mem->Fupp_[0] = std::numeric_limits<double>::max();
+ nlp_->getConstraintBounds(f_lVec, f_uVec, nlp_->getConstraintsCount());
+ nlp_->getConstraintsMultState(neF_, fMulVec, FStateVec);
+
+ iAVec.setZero();
+ jAVec.setZero();
+ AVec.setZero();
+
+ iGVecFull.setZero();
+ jGVecFull.setZero();
+
+ jGVecGrad = Eigen::VectorXi::LinSpaced(n_, 0, n_ - 1);
+ nlp_->getSparsityPatternJacobian(nlp_->getNonZeroJacobianCount(), iGVecJac, jGVecJac);
+ iGVecJac += Eigen::VectorXi::Ones(nlp_->getNonZeroJacobianCount());
+}
+
+
+void SnoptSolver::configureDerived(const NlpSolverSettings &settings)
+{
+ std::cout << "calling SNOPT configure derived" << std::endl;
+ settings_ = settings.snoptSettings_;
+
+ n_ = nlp_->getVarCount();
+ neF_ = nlp_->getConstraintsCount() + 1;
+
+ lenA_ = 0;
+ neA_ = 0;
+
+ lenG_ = n_ + nlp_->getNonZeroJacobianCount();
+ neG_ = n_ + nlp_->getNonZeroJacobianCount();
+
+ init_memory(memoryPtr_);
+ setSolverOptions();
+ BASE::isInitialized_ = true;
+}
+
+
+void SnoptSolver::setupSnoptObjects()
+{
+ snoptApp_.setProblemSize(n_, neF_);
+ snoptApp_.setObjective(ObjRow_, ObjAdd_);
+ snoptApp_.setX(memoryPtr_->x_, memoryPtr_->xlow_, memoryPtr_->xupp_, memoryPtr_->xmul_, memoryPtr_->xstate_);
+ snoptApp_.setF(memoryPtr_->F_, memoryPtr_->Flow_, memoryPtr_->Fupp_, memoryPtr_->Fmul_, memoryPtr_->Fstate_);
+ snoptApp_.setA(lenA_, neA_, memoryPtr_->iAfun_, memoryPtr_->jAvar_, memoryPtr_->A_);
+ snoptApp_.setG(lenG_, neG_, memoryPtr_->iGfun_, memoryPtr_->jGvar_);
+ snoptApp_.setUserFun(NLP_Function);
+}
+
+
+void SnoptSolver::setSolverOptions()
+{
+ snoptApp_.setIntParameter("Derivative option", settings_.derivative_option_param_);
+ snoptApp_.setIntParameter("Verify level", settings_.verify_level_param_);
+ snoptApp_.setIntParameter("Major iterations limit", settings_.major_iteration_limit_param_);
+ snoptApp_.setIntParameter("Minor iterations limit", settings_.minor_iteration_limit_param_);
+ snoptApp_.setIntParameter("Iterations limit", settings_.iteration_limit_param_);
+ snoptApp_.setRealParameter("Major optimality tolerance", settings_.major_optimality_tolerance_param_);
+ snoptApp_.setRealParameter("Major feasibility tolerance", settings_.major_feasibility_tolerance_param_);
+ snoptApp_.setRealParameter("Minor feasibility tolerance", settings_.minor_feasibility_tolerance_param_);
+ snoptApp_.setIntParameter("Print file", settings_.print_file_param_);
+ snoptApp_.setIntParameter("Minor print level", settings_.minor_print_level_param_);
+ snoptApp_.setIntParameter("Major print level", settings_.major_print_level_param_);
+
+ snoptApp_.setIntParameter("New basis file", settings_.new_basis_file_param_);
+ snoptApp_.setIntParameter("Old basis file", settings_.old_basis_file_param_);
+ snoptApp_.setIntParameter("Backup basis file", settings_.backup_basis_file_param_);
+ snoptApp_.setRealParameter("Linesearch tolerance", settings_.line_search_tolerance_param_);
+ snoptApp_.setIntParameter("Crash option", settings_.crash_option_);
+ snoptApp_.setIntParameter("Hessian updates", settings_.hessian_updates_);
+}
+
+
+bool SnoptSolver::solve()
+{
+ if (!this->isInitialized_)
+ throw std::runtime_error("SNOPT was not initialized before. Call NLPSolver->configure()");
+
+ snoptApp_.setUserI(&(memoryPtr_->memind), 1);
+ std::cout << "Ready to solve... " << std::endl;
+ fill_memory(memoryPtr_);
+ setupSnoptObjects();
+ int status = snoptApp_.solve(currStartOption_);
+ if (status == 1 || status == 31 || status == 32) //Optimal Solution Found or Iteration limit reached
+ {
+ // Store solution to optvector
+ MapVecXd xVec(memoryPtr_->x_, n_);
+ MapVecXd xMulVec(memoryPtr_->xmul_, n_);
+ MapVecXi xStateVec(memoryPtr_->xstate_, n_);
+ MapVecXd fMulVec(memoryPtr_->Fmul_, neF_);
+ MapVecXi fStateVec(memoryPtr_->Fstate_, neF_);
+ nlp_->extractSnoptSolution(xVec, xMulVec, xStateVec, fMulVec, fStateVec);
+ }
+
+ return true;
+}
+
+
+void SnoptSolver::prepareWarmStart(size_t maxIterations)
+{
+ currStartOption_ = Warm_;
+ Eigen::Map<Eigen::VectorXi> xState_local(memoryPtr_->xstate_, n_);
+ Eigen::Map<Eigen::VectorXi> Fstate_local(memoryPtr_->Fstate_, neF_);
+ Fstate_local.setConstant(3);
+ xState_local.setConstant(3);
+ snoptApp_.setIntParameter("Major iterations limit", (int)maxIterations);
+ snoptApp_.setIntParameter("Minor iterations limit", 50);
+ snoptApp_.setIntParameter("Iterations limit", 500);
+ snoptApp_.setIntParameter("Summary file", 0);
+}
+
+
+void SnoptSolver::validateSNOPTStatus(const int &status) const
+{
+ std::string message;
+ switch (status)
+ {
+ case 2:
+ {
+ message = "Optimal Solution Found";
+ break;
+ }
+ case 3:
+ message = "Problem is Infeasible";
+ break;
+ case 4:
+ message = "Unbounded";
+ break;
+ case 5:
+ message = "Iteration Limit";
+ break;
+ case 6:
+ message = "Numerical Difficulties";
+ break;
+ case 7:
+ message = "Error in user supplied functions";
+ break;
+ case 8:
+ message = "Undefined user supplied functions";
+ break;
+ case 9:
+ message = "User requested termination";
+ break;
+ case 10:
+ message = "Insufficient storage allocated";
+ break;
+ case 11:
+ message = "Input arguments out of range";
+ break;
+ case 12:
+ message = "System error";
+ break;
+ default:
+ message = "Unknown error";
+ break;
+ }
+ std::cout << message << std::endl;
+}
+
+
+void SnoptSolver::NLP_Function(int *Status,
+ int *n,
+ double x[],
+ int *needF,
+ int *neF,
+ double F[],
+ int *needG,
+ int *neG,
+ double G[],
+ char *cu,
+ int *lencu,
+ int iu[],
+ int *leniu,
+ double ru[],
+ int *lenru)
+{
+ auto m = SnoptMemory::mempool.at(iu[0]);
+ m->self.NLP_Function(m, Status, n, x, needF, neF, F, needG, neG, G, cu, lencu, iu, leniu, ru, lenru);
+}
+
+
+void SnoptSolver::NLP_Function(SnoptMemory *m,
+ int *Status,
+ int *n,
+ double x[],
+ int *needF,
+ int *neF,
+ double F[],
+ int *needG,
+ int *neG,
+ double G[],
+ char *cu,
+ int *lencu,
+ int iu[],
+ int *leniu,
+ double ru[],
+ int *lenru) const
+{
+ if (*Status > 1)
+ {
+ m->self.validateSNOPTStatus(*Status);
+ }
+
+ if (*needF > 0 || needG > 0)
+ {
+ Eigen::Map<const Eigen::VectorXd> xVec(&x[0], *n);
+ m->self.nlp_->extractOptimizationVars(xVec, true);
+ }
+
+ if (*needF > 0)
+ {
+ F[0] = m->self.nlp_->evaluateCostFun();
+ MapVecXd fVec(&F[1], *neF - 1);
+ m->self.nlp_->evaluateConstraints(fVec);
+ }
+
+ if (*needG > 0)
+ {
+ size_t grad_len = m->self.nlp_->getVarCount();
+ MapVecXd gradVec(&G[0], grad_len);
+ m->self.nlp_->evaluateCostGradient(grad_len, gradVec);
+ MapVecXd jacVec(&G[grad_len], *neG - grad_len);
+ m->self.nlp_->evaluateConstraintJacobian(*neG - grad_len, jacVec);
+ }
+}
+
+std::vector<SnoptMemory *> SnoptMemory::mempool;
+
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/constraint/ConstraintComparison.cpp b/ct_optcon/test/constraint/ConstraintComparison.cpp
new file mode 100644
index 0000000..9dc8b22
--- /dev/null
+++ b/ct_optcon/test/constraint/ConstraintComparison.cpp
@@ -0,0 +1,23 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#define EIGEN_INITIALIZE_MATRICES_BY_NAN
+
+#include <ct/optcon/optcon.h>
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <random>
+
+#include "ConstraintComparison.h"
+
+using namespace ct::optcon::example;
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/constraint/ConstraintComparison.h b/ct_optcon/test/constraint/ConstraintComparison.h
new file mode 100644
index 0000000..44bd245
--- /dev/null
+++ b/ct_optcon/test/constraint/ConstraintComparison.h
@@ -0,0 +1,223 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This file implements constraint unit tests.
+ * For more intuitive examples, visit the tutorial.
+ * \example ConstraintComparison.h
+ */
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+const bool verbose = true;
+
+const size_t state_dim = 3;
+const size_t control_dim = 3;
+
+//! A simple example with an 1d constraint
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintTerm1D : public ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ const static size_t term_dim = 1;
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef typename ct::core::tpl::TraitSelector<ct::core::ADCGScalar>::Trait TraitCG;
+ typedef ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ ConstraintTerm1D()
+ {
+ Base::lb_.resize(term_dim);
+ Base::ub_.resize(term_dim);
+ Base::lb_.setZero();
+ Base::ub_.setZero();
+ }
+
+ virtual ~ConstraintTerm1D() {}
+ virtual ConstraintTerm1D<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
+ {
+ return new ConstraintTerm1D<STATE_DIM, CONTROL_DIM, SCALAR>();
+ }
+
+ virtual size_t getConstraintSize() const override { return term_dim; }
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ Eigen::Matrix<SCALAR, term_dim, 1> constr_violation;
+ constr_violation.template segment<1>(0) << (u(1) * Trait::cos(x(2)) - u(0) * Trait::sin(x(2)) - u(2));
+ return constr_violation;
+ }
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ Eigen::Matrix<ct::core::ADCGScalar, term_dim, 1> constr_violation;
+ constr_violation.template segment<1>(0) << (u(1) * TraitCG::cos(x(2)) - u(0) * TraitCG::sin(x(2)) - u(2));
+ return constr_violation;
+ }
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ Eigen::Matrix<SCALAR, term_dim, STATE_DIM> jac;
+ jac.setZero();
+ jac << SCALAR(0.0), SCALAR(0.0), -u(1) * sin(x(2)) - u(0) * cos(x(2));
+ return jac;
+ }
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ Eigen::Matrix<SCALAR, term_dim, CONTROL_DIM> jac;
+ jac.setZero();
+ jac << -sin(x(2)), cos(x(2)), SCALAR(-1.0);
+ return jac;
+ }
+};
+
+
+//! A simple example with a 2d constraint
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class ConstraintTerm2D : public ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ const static size_t term_dim = 2;
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef typename ct::core::tpl::TraitSelector<ct::core::ADCGScalar>::Trait TraitCG;
+ typedef ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ ConstraintTerm2D()
+ {
+ Base::lb_.resize(term_dim);
+ Base::ub_.resize(term_dim);
+ Base::lb_.setZero();
+ Base::ub_.setZero();
+ }
+
+ virtual ~ConstraintTerm2D() {}
+ ConstraintTerm2D<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
+ {
+ return new ConstraintTerm2D<STATE_DIM, CONTROL_DIM, SCALAR>();
+ }
+
+ virtual size_t getConstraintSize() const override { return term_dim; }
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ Eigen::Matrix<SCALAR, term_dim, 1> constr_violation;
+ constr_violation(0) = (u(1) * Trait::cos(x(2)) - u(0) * Trait::sin(x(2)) - u(2));
+ constr_violation(1) = (u(2) * Trait::cos(x(1)) - u(2) * Trait::sin(x(1)) - u(1));
+ return constr_violation;
+ }
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ Eigen::Matrix<ct::core::ADCGScalar, term_dim, 1> constr_violation;
+ constr_violation(0) = (u(1) * TraitCG::cos(x(2)) - u(0) * TraitCG::sin(x(2)) - u(2));
+ constr_violation(1) = (u(2) * TraitCG::cos(x(1)) - u(2) * TraitCG::sin(x(1)) - u(1));
+ return constr_violation;
+ }
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ Eigen::Matrix<SCALAR, term_dim, STATE_DIM> jac;
+ jac.setZero();
+ jac.row(0) << 0.0, 0.0, -u(1) * sin(x(2)) - u(0) * cos(x(2));
+ jac.row(1) << 0.0, -(u(2)) * sin(x(1)) - u(2) * cos(x(1)), 0.0;
+
+ return jac;
+ }
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ Eigen::Matrix<SCALAR, term_dim, CONTROL_DIM> jac;
+ jac.setZero();
+ jac.row(0) << -sin(x(2)), cos(x(2)), -1.0;
+ jac.row(1) << 0.0, -1.0, cos(x(1)) - sin(x(1));
+ return jac;
+ }
+};
+
+
+TEST(ConstraintComparison, comparisonAnalyticAD)
+{
+ std::shared_ptr<ct::optcon::ConstraintContainerAD<state_dim, control_dim>> constraintAD(
+ new ct::optcon::ConstraintContainerAD<state_dim, control_dim>());
+
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> constraintAN(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ std::shared_ptr<ConstraintTerm1D<state_dim, control_dim>> term1_ad(new ConstraintTerm1D<state_dim, control_dim>());
+ term1_ad->setName("term1_ad");
+ std::shared_ptr<ConstraintTerm2D<state_dim, control_dim>> term2_ad(new ConstraintTerm2D<state_dim, control_dim>());
+ term2_ad->setName("term2_ad");
+
+ std::shared_ptr<ConstraintTerm1D<state_dim, control_dim, double>> term1_an(
+ new ConstraintTerm1D<state_dim, control_dim, double>());
+ term1_an->setName("term1_an");
+ std::shared_ptr<ConstraintTerm2D<state_dim, control_dim, double>> term2_an(
+ new ConstraintTerm2D<state_dim, control_dim, double>());
+ term2_an->setName("term2_an");
+
+
+ std::cout << "Adding terms to constraint_analytic" << std::endl;
+ constraintAD->addIntermediateConstraint(term1_ad, verbose);
+ constraintAD->addIntermediateConstraint(term2_ad, verbose);
+ constraintAN->addIntermediateConstraint(term1_an, verbose);
+ constraintAN->addIntermediateConstraint(term2_an, verbose);
+
+ constraintAD->initialize();
+ constraintAN->initialize();
+
+ /* evaluate constraint */
+ Eigen::VectorXd g1_ad, g1_an;
+
+ Eigen::Matrix<double, state_dim, 1> state = Eigen::Matrix<double, state_dim, 1>::Random();
+ Eigen::Matrix<double, control_dim, 1> control = Eigen::Matrix<double, control_dim, 1>::Random();
+ double time = 0.5;
+
+ constraintAN->setCurrentStateAndControl(state, control, time);
+ constraintAD->setCurrentStateAndControl(state, control, time);
+
+ g1_an = constraintAN->evaluateIntermediate();
+ g1_ad = constraintAD->evaluateIntermediate();
+
+ std::cout << "g1_an: " << g1_an.transpose() << std::endl;
+ std::cout << "g1_ad: " << g1_ad.transpose() << std::endl;
+
+ // test if constraint violations are the same
+ ASSERT_TRUE(g1_an.isApprox(g1_ad));
+
+ Eigen::MatrixXd C_an, C_ad, D_an, D_ad;
+ C_an = constraintAN->jacobianStateIntermediate();
+ D_an = constraintAN->jacobianInputIntermediate();
+ C_ad = constraintAD->jacobianStateIntermediate();
+ D_ad = constraintAD->jacobianInputIntermediate();
+
+ ASSERT_TRUE(C_an.isApprox(C_ad));
+
+ ASSERT_TRUE(D_an.isApprox(D_ad));
+
+ ASSERT_TRUE(1.0);
+}
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/constraint/ConstraintTest.cpp b/ct_optcon/test/constraint/ConstraintTest.cpp
new file mode 100644
index 0000000..bda361e
--- /dev/null
+++ b/ct_optcon/test/constraint/ConstraintTest.cpp
@@ -0,0 +1,23 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#define EIGEN_INITIALIZE_MATRICES_BY_NAN
+
+#include <gtest/gtest.h>
+#include <ct/optcon/optcon.h>
+
+#include <cmath>
+#include <random>
+
+#include "ConstraintTest.h"
+
+using namespace ct::optcon::example;
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/constraint/ConstraintTest.h b/ct_optcon/test/constraint/ConstraintTest.h
new file mode 100644
index 0000000..e3c0546
--- /dev/null
+++ b/ct_optcon/test/constraint/ConstraintTest.h
@@ -0,0 +1,310 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This file implements constraint unit tests.
+ * For more intuitive examples, visit the tutorial.
+ * \example ConstraintTest.h
+ */
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+const bool verbose = true;
+
+const size_t state_dim = 12;
+const size_t control_dim = 4;
+
+
+//! A pure state constraint term
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class PureStateConstraint_Example : public ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef ct::optcon::ConstraintBase<state_dim, control_dim, SCALAR> Base;
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ PureStateConstraint_Example()
+ {
+ Base::lb_.resize(STATE_DIM);
+ Base::ub_.resize(STATE_DIM);
+ Base::lb_.setZero();
+ Base::ub_.setZero();
+ }
+
+ PureStateConstraint_Example(const PureStateConstraint_Example& arg) : Base(arg), A_(arg.A_) {}
+ virtual ~PureStateConstraint_Example() {}
+ virtual PureStateConstraint_Example<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
+ {
+ return new PureStateConstraint_Example(*this);
+ }
+
+ virtual size_t getConstraintSize() const override { return STATE_DIM; }
+ virtual VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ return A_ * x;
+ }
+
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ return A_.template cast<ct::core::ADCGScalar>() * x;
+ }
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ return A_;
+ }
+
+ void setA(const Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM>& A) { A_ = A; }
+private:
+ Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> A_;
+};
+
+//! A state input constraint term
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
+class StateInputConstraint_Example : public ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef typename ct::core::tpl::TraitSelector<SCALAR>::Trait Trait;
+ typedef ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR> Base;
+ typedef core::StateVector<STATE_DIM, SCALAR> state_vector_t;
+ typedef core::ControlVector<CONTROL_DIM, SCALAR> control_vector_t;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, 1> VectorXs;
+ typedef Eigen::Matrix<SCALAR, Eigen::Dynamic, Eigen::Dynamic> MatrixXs;
+
+ StateInputConstraint_Example()
+ {
+ Base::lb_.resize(CONTROL_DIM);
+ Base::ub_.resize(CONTROL_DIM);
+ Base::lb_.setZero();
+ Base::ub_.setZero();
+ }
+
+ StateInputConstraint_Example(const StateInputConstraint_Example& arg)
+ : ct::optcon::ConstraintBase<STATE_DIM, CONTROL_DIM, SCALAR>(arg), A_(arg.A_), B_(arg.B_)
+ {
+ Base::lb_.resize(CONTROL_DIM);
+ Base::ub_.resize(CONTROL_DIM);
+ Base::lb_.setZero();
+ Base::ub_.setZero();
+ }
+
+ virtual StateInputConstraint_Example<STATE_DIM, CONTROL_DIM, SCALAR>* clone() const override
+ {
+ return new StateInputConstraint_Example(*this);
+ }
+
+ virtual ~StateInputConstraint_Example() {}
+ virtual size_t getConstraintSize() const override { return CONTROL_DIM; }
+ VectorXs evaluate(const state_vector_t& x, const control_vector_t& u, const SCALAR t) { return A_ * x + B_ * u; }
+ virtual Eigen::Matrix<ct::core::ADCGScalar, Eigen::Dynamic, 1> evaluateCppadCg(
+ const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ return (A_.template cast<ct::core::ADCGScalar>() * x + B_.template cast<ct::core::ADCGScalar>() * u);
+ }
+
+ virtual MatrixXs jacobianState(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ return A_;
+ }
+
+ virtual MatrixXs jacobianInput(const state_vector_t& x, const control_vector_t& u, const SCALAR t) override
+ {
+ return B_;
+ }
+
+ void setAB(const Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM>& A,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> B)
+ {
+ A_ = A;
+ B_ = B;
+ }
+
+private:
+ Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> A_;
+ Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> B_;
+};
+
+
+TEST(pureStateConstraintTest, pureStateConstraintTest)
+{
+ std::shared_ptr<ct::optcon::ConstraintContainerAD<state_dim, control_dim>> constraintAD(
+ new ct::optcon::ConstraintContainerAD<state_dim, control_dim>());
+
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> constraintAN(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ Eigen::Matrix<double, state_dim, state_dim> A;
+ A.setRandom();
+
+ std::shared_ptr<PureStateConstraint_Example<state_dim, control_dim>> term1_ad(
+ new PureStateConstraint_Example<state_dim, control_dim>());
+ term1_ad->setName("term1_ad");
+ term1_ad->setA(A);
+
+ std::shared_ptr<PureStateConstraint_Example<state_dim, control_dim, double>> term1_an(
+ new PureStateConstraint_Example<state_dim, control_dim, double>());
+ term1_an->setName("term1_an");
+ term1_an->setA(A);
+
+ constraintAD->addIntermediateConstraint(term1_ad, verbose);
+ constraintAD->initialize();
+ constraintAN->addIntermediateConstraint(term1_an, verbose);
+ constraintAN->initialize();
+
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> constraintAN_cloned(
+ constraintAN->clone());
+ std::shared_ptr<ct::optcon::ConstraintContainerAD<state_dim, control_dim>> constraintAD_cloned(
+ constraintAD->clone());
+
+ size_t nRuns = 100;
+
+ for (size_t i = 0; i < nRuns; i++)
+ {
+ /* evaluate constraint */
+ Eigen::VectorXd g1_ad, g1_an, g1_ad_cl, g1_an_cl;
+
+ Eigen::Matrix<double, state_dim, 1> state;
+ state.setRandom();
+ Eigen::Matrix<double, control_dim, 1> input;
+ input.setRandom();
+
+ std::random_device rd;
+ std::mt19937 gen(rd());
+ std::uniform_real_distribution<> time_distr(0, 100);
+ double time = time_distr(gen);
+
+ constraintAN->setCurrentStateAndControl(state, input, time);
+ constraintAD->setCurrentStateAndControl(state, input, time);
+ constraintAD_cloned->setCurrentStateAndControl(state, input, time);
+ constraintAN_cloned->setCurrentStateAndControl(state, input, time);
+
+ g1_an = constraintAN->evaluateIntermediate();
+ g1_ad = constraintAD->evaluateIntermediate();
+ g1_an_cl = constraintAN_cloned->evaluateIntermediate();
+ g1_ad_cl = constraintAD_cloned->evaluateIntermediate();
+
+ // test if constraint violations are the same
+ ASSERT_TRUE(g1_an.isApprox(g1_ad));
+ ASSERT_TRUE(g1_an.isApprox(g1_ad_cl));
+ ASSERT_TRUE(g1_an.isApprox(g1_an_cl));
+
+ Eigen::MatrixXd F_an, F_ad, F_cloned, F_cloned_an;
+ F_an.setZero();
+ F_ad.setZero();
+ F_cloned.setZero();
+ F_cloned_an.setZero();
+
+ F_an = constraintAN->jacobianStateIntermediate();
+ F_ad = constraintAD->jacobianStateIntermediate();
+ F_cloned_an = constraintAN_cloned->jacobianStateIntermediate();
+ F_cloned = constraintAD_cloned->jacobianStateIntermediate();
+
+ // compare jacobians
+
+ ASSERT_TRUE(F_an.isApprox(F_ad));
+ ASSERT_TRUE(F_an.isApprox(F_cloned));
+ ASSERT_TRUE(F_an.isApprox(F_cloned_an));
+ }
+}
+
+
+TEST(stateInputConstraintTest, stateInputConstraintTest)
+{
+ std::shared_ptr<ct::optcon::ConstraintContainerAD<state_dim, control_dim>> constraintAD(
+ new ct::optcon::ConstraintContainerAD<state_dim, control_dim>());
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> constraintAN(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ Eigen::Matrix<double, control_dim, state_dim> A;
+ A.setRandom();
+ Eigen::Matrix<double, control_dim, control_dim> B;
+ B.setRandom();
+
+ std::shared_ptr<StateInputConstraint_Example<state_dim, control_dim>> term1_ad(
+ new StateInputConstraint_Example<state_dim, control_dim>());
+ term1_ad->setName("term1_ad");
+ term1_ad->setAB(A, B);
+
+ std::shared_ptr<StateInputConstraint_Example<state_dim, control_dim, double>> term1_an(
+ new StateInputConstraint_Example<state_dim, control_dim, double>());
+ term1_an->setName("term1_an");
+ term1_an->setAB(A, B);
+
+
+ // std::cout << "Adding terms to constraintAD" << std::endl;
+ constraintAD->addIntermediateConstraint(term1_ad, verbose);
+ constraintAD->initialize();
+ constraintAN->addIntermediateConstraint(term1_an, verbose);
+ constraintAN->initialize();
+
+ /* evaluate constraint */
+ Eigen::VectorXd g1_ad, g1_an;
+
+ Eigen::Matrix<double, state_dim, 1> state;
+ state.setRandom();
+ Eigen::Matrix<double, control_dim, 1> input;
+ input.setRandom();
+ double time = 1.0;
+
+
+ constraintAN->setCurrentStateAndControl(state, input, time);
+ constraintAD->setCurrentStateAndControl(state, input, time);
+
+
+ g1_an = constraintAN->evaluateIntermediate();
+ g1_ad = constraintAD->evaluateIntermediate();
+
+ // test if constraint violations are the same
+ ASSERT_TRUE(g1_an.isApprox(g1_ad));
+
+ Eigen::MatrixXd C_an, C_ad, C_cloned, C_cloned_an;
+ Eigen::MatrixXd D_an, D_ad, D_cloned, D_cloned_an;
+
+ C_an = constraintAN->jacobianStateIntermediate();
+ C_ad = constraintAD->jacobianStateIntermediate();
+ D_an = constraintAN->jacobianInputIntermediate();
+ D_ad = constraintAD->jacobianInputIntermediate();
+
+
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>> constraintAN_cloned(
+ constraintAN->clone());
+
+ std::shared_ptr<ct::optcon::ConstraintContainerAD<state_dim, control_dim>> constraintAD_cloned(
+ constraintAD->clone());
+
+ C_cloned_an = constraintAN_cloned->jacobianStateIntermediate();
+ C_cloned = constraintAD_cloned->jacobianStateIntermediate();
+ D_cloned_an = constraintAN_cloned->jacobianInputIntermediate();
+ D_cloned = constraintAD_cloned->jacobianInputIntermediate();
+
+ // compare jacobians
+ ASSERT_TRUE(C_an.isApprox(C_ad));
+ ASSERT_TRUE(C_an.isApprox(C_cloned));
+ ASSERT_TRUE(C_an.isApprox(C_cloned_an));
+
+ ASSERT_TRUE(D_an.isApprox(D_ad));
+ ASSERT_TRUE(D_an.isApprox(D_cloned));
+ ASSERT_TRUE(D_an.isApprox(D_cloned_an));
+}
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/constraint/ConstraintTestPrespec.cpp b/ct_optcon/test/constraint/ConstraintTestPrespec.cpp
new file mode 100644
index 0000000..3959bdd
--- /dev/null
+++ b/ct_optcon/test/constraint/ConstraintTestPrespec.cpp
@@ -0,0 +1,23 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#define EIGEN_INITIALIZE_MATRICES_BY_NAN
+
+#include <ct/optcon/optcon-prespec.h>
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <random>
+
+#include "ConstraintTest.h"
+
+using namespace ct::optcon::example;
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/constraint/SparseBoxConstraintTest.cpp b/ct_optcon/test/constraint/SparseBoxConstraintTest.cpp
new file mode 100644
index 0000000..49d123d
--- /dev/null
+++ b/ct_optcon/test/constraint/SparseBoxConstraintTest.cpp
@@ -0,0 +1,120 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This unit test for the box constraint terms compares automatically generated sparsity patterns against hard-coded ones.
+ */
+
+#include <ct/optcon/optcon.h>
+#include <gtest/gtest.h>
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+// state and control dimensions
+const size_t state_dim = 10;
+const size_t control_dim = 5;
+
+bool verbose = false;
+
+TEST(SparseBoxConstraintTest, HardcodedPatternTest)
+{
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // box constraint boundaries with sparsities
+ Eigen::VectorXi sp_control(control_dim);
+ sp_control << 0, 1, 0, 0, 1;
+ Eigen::VectorXd u_lb(2);
+ Eigen::VectorXd u_ub(2);
+ u_lb.setConstant(-1.11);
+ u_ub = -u_lb;
+
+ Eigen::VectorXi sp_state(state_dim);
+ sp_state << 0, 1, 0, 0, 1, 0, 1, 1, 0, 0;
+ Eigen::VectorXd x_lb(4);
+ Eigen::VectorXd x_ub(4);
+ x_lb.setConstant(-3.33);
+ x_ub = -x_lb;
+
+ // constraint terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub, sp_control));
+ controlConstraint->setName("ControlInputConstraint");
+ std::shared_ptr<StateConstraint<state_dim, control_dim>> stateConstraint(
+ new StateConstraint<state_dim, control_dim>(x_lb, x_ub, sp_state));
+ stateConstraint->setName("StateConstraint");
+
+ // add and initialize constraint terms
+ constraints->addIntermediateConstraint(controlConstraint, true);
+ constraints->addIntermediateConstraint(stateConstraint, true);
+ constraints->initialize();
+
+ if (verbose)
+ {
+ std::cout << "=============================================" << std::endl;
+ std::cout << "Printing example for sparse box constraint:" << std::endl;
+ std::cout << "=============================================" << std::endl;
+ constraints->printout();
+ }
+
+
+ /*!
+ * compare results against hard-coded references.
+ */
+
+ ASSERT_EQ(constraints->getJacobianStateNonZeroCountIntermediate(), 4);
+ ASSERT_EQ(constraints->getJacobianInputNonZeroCountIntermediate(), 2);
+
+ Eigen::Matrix<double, 4 + 2, state_dim> jac_x_ref = Eigen::Matrix<double, 4 + 2, state_dim>::Zero();
+ jac_x_ref(2, 1) = 1;
+ jac_x_ref(3, 4) = 1;
+ jac_x_ref(4, 6) = 1;
+ jac_x_ref(5, 7) = 1;
+ Eigen::MatrixXd jac_x = constraints->jacobianStateIntermediate();
+ for (size_t i = 0; i < 4 + 2; i++)
+ for (size_t j = 0; j < state_dim; j++)
+ ASSERT_EQ(jac_x(i, j), jac_x_ref(i, j));
+
+
+ Eigen::Matrix<double, 4 + 2, control_dim> jac_u_ref = Eigen::Matrix<double, 4 + 2, control_dim>::Zero();
+ jac_u_ref(0, 1) = 1;
+ jac_u_ref(1, 4) = 1;
+ Eigen::MatrixXd jac_u = constraints->jacobianInputIntermediate();
+ for (size_t i = 0; i < 4 + 2; i++)
+ for (size_t j = 0; j < control_dim; j++)
+ ASSERT_EQ(jac_u(i, j), jac_u_ref(i, j));
+
+
+ ASSERT_EQ(constraints->jacobianInputSparseIntermediate().rows(), 2);
+ ASSERT_EQ(constraints->jacobianStateSparseIntermediate().rows(), 4);
+
+ Eigen::VectorXi iRows, jCols;
+
+ constraints->sparsityPatternStateIntermediate(iRows, jCols);
+ ASSERT_EQ(iRows(0), 2);
+ ASSERT_EQ(iRows(1), 3);
+ ASSERT_EQ(iRows(2), 4);
+ ASSERT_EQ(iRows(3), 5);
+ ASSERT_EQ(jCols(0), 1);
+ ASSERT_EQ(jCols(1), 4);
+ ASSERT_EQ(jCols(2), 6);
+ ASSERT_EQ(jCols(3), 7);
+
+ constraints->sparsityPatternInputIntermediate(iRows, jCols);
+ ASSERT_EQ(iRows(0), 0);
+ ASSERT_EQ(iRows(1), 1);
+ ASSERT_EQ(jCols(0), 1);
+ ASSERT_EQ(jCols(1), 4);
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/costfunction/ADTest_timeDependent.h b/ct_optcon/test/costfunction/ADTest_timeDependent.h
new file mode 100644
index 0000000..76d9613
--- /dev/null
+++ b/ct_optcon/test/costfunction/ADTest_timeDependent.h
@@ -0,0 +1,215 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#define EIGEN_INITIALIZE_MATRICES_BY_NAN
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+/*
+ * Define a cost function term of form cost = c*t^2 (x'Qx + u'Ru)
+ * analytical derivatives:
+ * state Derivative: 2*c*t^2 Qx
+ * control Derivative: 2*c*t^2 Rx
+ * state second Derivative: 2*c*t^2 Q
+ * control second Derivative: 2*c*t^2 R
+ * */
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double, typename SCALAR = SCALAR_EVAL>
+class TestTerm : public TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM> control_state_matrix_double_t;
+
+
+ TestTerm();
+
+ TestTerm(const state_matrix_t& Q, const control_matrix_t& R, const SCALAR_EVAL& c) : Q_(Q), R_(R), c_(c)
+ {
+ x_ref_.setZero(); // default values
+ u_ref_.setZero();
+ }
+
+ TestTerm(const TestTerm& arg)
+ : TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>(arg),
+ Q_(arg.Q_),
+ R_(arg.R_),
+ c_(arg.c_),
+ x_ref_(arg.x_ref_),
+ u_ref_(arg.u_ref_)
+ {
+ }
+
+ virtual ~TestTerm() {}
+ TestTerm<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>* clone() const override { return new TestTerm(*this); }
+ void setWeights(const state_matrix_t& Q, const control_matrix_t& R, const double& c)
+ {
+ Q_ = Q;
+ R_ = R;
+ c_ = c;
+ }
+
+ void setStateAndControlReference(const core::StateVector<STATE_DIM, SCALAR_EVAL>& x_ref,
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL>& u_ref)
+ {
+ x_ref_ = x_ref;
+ u_ref_ = u_ref;
+ }
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1>& x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1>& u,
+ const SCALAR& t) override
+ {
+ return evalLocal<SCALAR>(x, u, t);
+ }
+
+ virtual ct::core::ADCGScalar evaluateCppadCg(const core::StateVector<STATE_DIM, ct::core::ADCGScalar>& x,
+ const core::ControlVector<CONTROL_DIM, ct::core::ADCGScalar>& u,
+ ct::core::ADCGScalar t) override
+ {
+ return evalLocal<ct::core::ADCGScalar>(x, u, t);
+ }
+
+
+ // todo: this is quite ugly
+ core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivativeAnalytical(const core::StateVector<STATE_DIM>& x,
+ const core::ControlVector<CONTROL_DIM>& u,
+ const double t)
+ {
+ return (2 * c_ * (SCALAR_EVAL)t * (SCALAR_EVAL)t * Q_ * x.template cast<SCALAR_EVAL>());
+ }
+
+ // todo: this is very ugly
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivativeAnalytical(const core::StateVector<STATE_DIM>& x,
+ const core::ControlVector<CONTROL_DIM>& u,
+ double t)
+ {
+ return (2 * c_ * (SCALAR_EVAL)t * (SCALAR_EVAL)t * R_ * u.template cast<SCALAR_EVAL>());
+ }
+
+ // todo: this is quite ugly
+ state_matrix_t stateSecondDerivativeAnalytical(const core::StateVector<STATE_DIM>& x,
+ const core::ControlVector<CONTROL_DIM>& u,
+ const double t)
+ {
+ return (2 * c_ * (SCALAR_EVAL)t * (SCALAR_EVAL)t * Q_);
+ }
+
+ // todo: this is very ugly
+ control_matrix_t controlSecondDerivativeAnalytical(const core::StateVector<STATE_DIM>& x,
+ const core::ControlVector<CONTROL_DIM>& u,
+ double t)
+ {
+ return (2 * c_ * (SCALAR_EVAL)t * (SCALAR_EVAL)t * R_);
+ }
+
+
+protected:
+ template <typename SC>
+ SC evalLocal(const Eigen::Matrix<SC, STATE_DIM, 1>& x, const Eigen::Matrix<SC, CONTROL_DIM, 1>& u, const SC& t)
+ {
+ Eigen::Matrix<SC, STATE_DIM, 1> xDiff = (x - x_ref_.template cast<SC>());
+ Eigen::Matrix<SC, CONTROL_DIM, 1> uDiff = (u - u_ref_.template cast<SC>());
+
+ return ((xDiff.transpose() * Q_.template cast<SC>() * xDiff) * SC(c_) * t * t +
+ (uDiff.transpose() * R_.template cast<SC>() * uDiff) * SC(c_) * t * t)(0, 0);
+ }
+
+ state_matrix_t Q_;
+ control_matrix_t R_;
+ SCALAR_EVAL c_;
+
+ core::StateVector<STATE_DIM, SCALAR_EVAL> x_ref_;
+ core::ControlVector<CONTROL_DIM, SCALAR_EVAL> u_ref_;
+};
+
+
+TEST(AD_TEST_TIME_VAR, AD_TEST_TIME_VAR)
+{
+ using namespace ct;
+
+ Eigen::Matrix<double, 3, 3> Q;
+ Q.setIdentity();
+ Eigen::Matrix<double, 3, 3> R;
+ R.setIdentity();
+
+ Eigen::Matrix<double, 3, 3> Q_f = 10 * Q;
+ Eigen::Matrix<double, 3, 3> R_f = 10 * R;
+
+ double c = 1.0;
+ double c_f = 2.0;
+
+ std::shared_ptr<TestTerm<3, 3, double, ct::core::ADCGScalar>> term_intermediate(
+ new TestTerm<3, 3, double, ct::core::ADCGScalar>(Q, R, c));
+ std::shared_ptr<TestTerm<3, 3, double, ct::core::ADCGScalar>> term_final(
+ new TestTerm<3, 3, double, ct::core::ADCGScalar>(Q_f, R_f, c_f));
+
+ // autodiff costfunction
+ std::shared_ptr<ct::optcon::CostFunctionAD<3, 3>> ADcf(new ct::optcon::CostFunctionAD<3, 3>());
+ ADcf->addIntermediateADTerm(term_intermediate);
+ ADcf->addFinalADTerm(term_final);
+
+
+ Eigen::Vector3d x;
+ x.setRandom();
+ Eigen::Vector3d u;
+ u.setRandom();
+
+ double t_final = 4.0;
+
+ ADcf->initialize();
+
+ for (double t = 0.0; t <= t_final; t = t + 1)
+ {
+ ADcf->setCurrentStateAndControl(x, u, t);
+
+ Eigen::Matrix<double, 3, 1> diff1 = (ADcf->stateDerivativeTerminal()).template cast<double>() -
+ (term_final->stateDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff1.maxCoeff() - diff1.minCoeff() < 1e-9);
+
+ Eigen::Matrix<double, 3, 1> diff2 = (ADcf->stateDerivativeIntermediate()).template cast<double>() -
+ (term_intermediate->stateDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff2.maxCoeff() - diff2.minCoeff() < 1e-9);
+
+ Eigen::Matrix<double, 3, 1> diff3 = (ADcf->controlDerivativeIntermediate()).template cast<double>() -
+ (term_intermediate->controlDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff3.maxCoeff() - diff3.minCoeff() < 1e-9);
+
+ Eigen::Matrix<double, 3, 1> diff4 = (ADcf->controlDerivativeTerminal()).template cast<double>() -
+ (term_final->controlDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff4.maxCoeff() - diff4.minCoeff() < 1e-9);
+
+ Eigen::Matrix<double, 3, 3> diff5 = (ADcf->controlSecondDerivativeIntermediate()).template cast<double>() -
+ (term_intermediate->controlSecondDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff5.maxCoeff() - diff5.minCoeff() < 1e-9);
+
+ Eigen::Matrix<double, 3, 3> diff6 = (ADcf->controlSecondDerivativeTerminal()).template cast<double>() -
+ (term_final->controlSecondDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff6.maxCoeff() - diff6.minCoeff() < 1e-9);
+
+ Eigen::Matrix<double, 3, 3> diff7 = (ADcf->stateSecondDerivativeIntermediate()).template cast<double>() -
+ (term_intermediate->stateSecondDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff7.maxCoeff() - diff7.minCoeff() < 1e-9);
+
+ Eigen::Matrix<double, 3, 3> diff8 = (ADcf->stateSecondDerivativeTerminal()).template cast<double>() -
+ (term_final->stateSecondDerivativeAnalytical(x, u, t));
+ ASSERT_TRUE(diff8.maxCoeff() - diff8.minCoeff() < 1e-9);
+ }
+}
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/costfunction/CostFunctionTest.h b/ct_optcon/test/costfunction/CostFunctionTest.h
new file mode 100644
index 0000000..844ce3d
--- /dev/null
+++ b/ct_optcon/test/costfunction/CostFunctionTest.h
@@ -0,0 +1,348 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+#define EIGEN_INITIALIZE_MATRICES_BY_NAN
+#define DEBUG
+
+#include "compareCostFunctions.h"
+
+
+const size_t state_dim = 12;
+const size_t control_dim = 4;
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+
+template <size_t state_dim, size_t control_dim>
+void printCostFunctionOutput(CostFunctionQuadratic<state_dim, control_dim>& costFunction,
+ CostFunctionQuadratic<state_dim, control_dim>& costFunction2)
+{
+ std::cout << "eval intermediate " << std::endl;
+ std::cout << costFunction.evaluateIntermediate() << std::endl << std::endl;
+ std::cout << costFunction2.evaluateIntermediate() << std::endl;
+
+ std::cout << "eval terminal " << std::endl;
+ std::cout << costFunction.evaluateTerminal() << std::endl << std::endl;
+ std::cout << costFunction2.evaluateTerminal() << std::endl;
+
+ std::cout << "eval stateDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.stateDerivativeIntermediate() << std::endl << std::endl;
+ std::cout << costFunction2.stateDerivativeIntermediate() << std::endl;
+
+ std::cout << "eval stateDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.stateDerivativeTerminal() << std::endl << std::endl;
+ std::cout << costFunction2.stateDerivativeTerminal() << std::endl;
+
+ std::cout << "eval stateSecondDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.stateSecondDerivativeIntermediate() << std::endl << std::endl;
+ std::cout << costFunction2.stateSecondDerivativeIntermediate() << std::endl;
+
+ std::cout << "eval stateSecondDerivativeTerminal " << std::endl;
+ std::cout << costFunction.stateSecondDerivativeTerminal() << std::endl << std::endl;
+ std::cout << costFunction2.stateSecondDerivativeTerminal() << std::endl;
+
+ std::cout << "eval controlDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.controlDerivativeIntermediate() << std::endl << std::endl;
+ std::cout << costFunction2.controlDerivativeIntermediate() << std::endl;
+
+ std::cout << "eval controlDerivativeTerminal " << std::endl;
+ std::cout << costFunction.controlDerivativeTerminal() << std::endl << std::endl;
+ std::cout << costFunction2.controlDerivativeTerminal() << std::endl;
+
+ std::cout << "eval controlSecondDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.controlSecondDerivativeIntermediate() << std::endl << std::endl;
+ std::cout << costFunction2.controlSecondDerivativeIntermediate() << std::endl;
+
+ std::cout << "eval controlSecondDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.controlSecondDerivativeTerminal() << std::endl << std::endl;
+ std::cout << costFunction2.controlSecondDerivativeTerminal() << std::endl;
+
+ std::cout << "eval stateControlDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.stateControlDerivativeIntermediate() << std::endl << std::endl;
+ std::cout << costFunction2.stateControlDerivativeIntermediate() << std::endl;
+
+ std::cout << "eval stateControlDerivativeTerminal " << std::endl;
+ std::cout << costFunction.stateControlDerivativeTerminal() << std::endl << std::endl;
+ std::cout << costFunction2.stateControlDerivativeTerminal() << std::endl;
+
+ std::cout << "eval stateSecondDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.stateSecondDerivativeIntermediate() << std::endl << std::endl;
+ std::cout << costFunction.stateSecondDerivativeIntermediate().transpose() << std::endl;
+
+ std::cout << "eval controlSecondDerivativeIntermediate " << std::endl;
+ std::cout << costFunction.controlSecondDerivativeIntermediate() << std::endl << std::endl;
+ std::cout << costFunction.controlSecondDerivativeIntermediate().transpose() << std::endl;
+}
+
+
+TEST(CostFunctionTest, ADQuadraticTest)
+{
+ const size_t nWeights = 2;
+ const size_t nTests = 10;
+
+ CostFunctionAnalytical<state_dim, control_dim> costFunction;
+ CostFunctionAD<state_dim, control_dim> costFunctionAD;
+
+ // intermediate cost terms
+ std::shared_ptr<TermQuadratic<state_dim, control_dim, double>> termQuadratic_interm(
+ new TermQuadratic<state_dim, control_dim>);
+ std::shared_ptr<TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>> termQuadraticAD_interm(
+ new TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>);
+
+ // final cost terms
+ std::shared_ptr<TermQuadratic<state_dim, control_dim, double>> termQuadratic_final(
+ new TermQuadratic<state_dim, control_dim>);
+ std::shared_ptr<TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>> termQuadraticAD_final(
+ new TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>);
+
+ costFunction.addIntermediateTerm(termQuadratic_interm, true);
+ costFunctionAD.addIntermediateADTerm(termQuadraticAD_interm, true);
+ costFunction.addFinalTerm(termQuadratic_final, true);
+ costFunctionAD.addFinalADTerm(termQuadraticAD_final, true);
+
+ Eigen::Matrix<double, state_dim, state_dim> Q_interm;
+ Eigen::Matrix<double, state_dim, state_dim> Q_final;
+ Eigen::Matrix<double, control_dim, control_dim> R;
+
+ core::StateVector<state_dim> x_ref;
+ core::ControlVector<control_dim> u_ref;
+
+ for (size_t i = 0; i < nWeights; i++)
+ {
+ try
+ {
+ Q_interm.setRandom();
+ Q_final.setRandom();
+ R.setRandom();
+ x_ref.setRandom();
+ u_ref.setRandom();
+
+ if (i == 0)
+ {
+ Q_interm.setZero();
+ Q_final.setZero();
+ R.setZero();
+ x_ref.setZero();
+ u_ref.setZero();
+ }
+
+ Q_interm += Q_interm.transpose().eval(); // make symmetric
+ R += R.transpose().eval(); // make symmetric
+ Q_final += Q_final.transpose().eval(); // make symmetric
+
+ termQuadratic_interm->setWeights(Q_interm, R);
+ termQuadraticAD_interm->setWeights(Q_interm, R);
+ termQuadratic_interm->setStateAndControlReference(x_ref, u_ref);
+ termQuadraticAD_interm->setStateAndControlReference(x_ref, u_ref);
+
+ termQuadratic_final->setWeights(Q_final, R);
+ termQuadraticAD_final->setWeights(Q_final, R);
+ termQuadratic_final->setStateAndControlReference(x_ref, u_ref);
+ termQuadraticAD_final->setStateAndControlReference(x_ref, u_ref);
+
+ costFunctionAD.initialize();
+
+ // create cloned cost function
+ std::shared_ptr<CostFunctionAD<state_dim, control_dim>> costFunctionAD_clone(costFunctionAD.clone());
+ costFunctionAD_clone->initialize();
+
+ for (size_t j = 0; j < nTests; j++)
+ {
+ core::StateVector<state_dim> x;
+ core::ControlVector<control_dim> u;
+ x.setRandom();
+ u.setRandom();
+
+ if (j == 0)
+ {
+ x.setZero();
+ u.setZero();
+ }
+
+ costFunction.setCurrentStateAndControl(x, u, 1.0);
+ costFunctionAD.setCurrentStateAndControl(x, u, 1.0);
+ costFunctionAD_clone->setCurrentStateAndControl(x, u, 1.0);
+
+ // printCostFunctionOutput(costFunction, costFunctionAD);
+ compareCostFunctionOutput(costFunction, costFunctionAD);
+ compareCostFunctionOutput(costFunction, *costFunctionAD_clone);
+
+ // now some manual assertions
+ ASSERT_TRUE(costFunction.stateDerivativeIntermediate().isApprox(2 * Q_interm * (x - x_ref)));
+ ASSERT_TRUE(costFunction.stateDerivativeTerminal().isApprox(2 * Q_final * (x - x_ref)));
+
+ ASSERT_TRUE(costFunction.stateSecondDerivativeIntermediate().isApprox(2 * Q_interm));
+ ASSERT_TRUE(costFunction.stateSecondDerivativeTerminal().isApprox(2 * Q_final));
+
+ ASSERT_TRUE(costFunction.controlDerivativeIntermediate().isApprox(2 * R * (u - u_ref)));
+
+ ASSERT_TRUE(costFunction.controlSecondDerivativeIntermediate().isApprox(2 * R));
+ }
+ } catch (std::exception& e)
+ {
+ FAIL();
+ }
+ }
+}
+
+
+TEST(CostFunctionTest, ADQuadMultTest)
+{
+ const size_t nWeights = 3;
+ const size_t nTests = 10;
+
+ CostFunctionAnalytical<state_dim, control_dim> costFunction;
+ CostFunctionAD<state_dim, control_dim> costFunctionAD;
+
+ std::shared_ptr<TermQuadMult<state_dim, control_dim, double>> termQuadMult(
+ new TermQuadMult<state_dim, control_dim>);
+ std::shared_ptr<TermQuadMult<state_dim, control_dim, double, ct::core::ADCGScalar>> termQuadMultAD(
+ new TermQuadMult<state_dim, control_dim, double, ct::core::ADCGScalar>);
+
+ std::shared_ptr<TermMixed<state_dim, control_dim, double>> termMixed(new TermMixed<state_dim, control_dim, double>);
+ std::shared_ptr<TermMixed<state_dim, control_dim, double, ct::core::ADCGScalar>> termMixedAD(
+ new TermMixed<state_dim, control_dim, double, ct::core::ADCGScalar>);
+
+
+ costFunction.addIntermediateTerm(termQuadMult);
+ costFunctionAD.addIntermediateADTerm(termQuadMultAD);
+
+ Eigen::Matrix<double, state_dim, state_dim> Q;
+ Eigen::Matrix<double, control_dim, control_dim> R;
+ Eigen::Matrix<double, control_dim, state_dim> P;
+
+ core::StateVector<state_dim> x_ref;
+ core::ControlVector<control_dim> u_ref;
+
+ for (size_t i = 0; i < nWeights; i++)
+ {
+ try
+ {
+ Q.setRandom();
+ R.setRandom();
+ P.setRandom();
+ x_ref.setRandom();
+ u_ref.setRandom();
+
+ if (i == 0)
+ {
+ Q.setZero();
+ R.setZero();
+ P.setZero();
+ x_ref.setZero();
+ u_ref.setZero();
+ }
+
+ if (i == 1)
+ {
+ Q.setIdentity();
+ R.setIdentity();
+ P.setZero();
+ x_ref.setConstant(10.0);
+ u_ref.setConstant(10.0);
+ }
+
+ Q += Q.transpose().eval(); // make symmetric
+ R += R.transpose().eval(); // make symmetric
+
+ termQuadMult->setWeights(Q, R);
+ termQuadMultAD->setWeights(Q, R);
+ termQuadMult->setStateAndControlReference(x_ref, u_ref);
+ termQuadMultAD->setStateAndControlReference(x_ref, u_ref);
+
+ costFunctionAD.initialize();
+
+ // create cloned cost function
+ std::shared_ptr<CostFunctionAD<state_dim, control_dim>> costFunctionAD_clone(costFunctionAD.clone());
+ costFunctionAD_clone->initialize();
+
+
+ for (size_t j = 0; j < nTests; j++)
+ {
+ core::StateVector<state_dim> x;
+ core::ControlVector<control_dim> u;
+ x.setRandom();
+ u.setRandom();
+ double t = 3.14;
+
+ if (j == 0)
+ {
+ x.setZero();
+ u.setZero();
+ }
+
+ costFunction.setCurrentStateAndControl(x, u, t);
+ costFunctionAD.setCurrentStateAndControl(x, u, t);
+ costFunctionAD_clone->setCurrentStateAndControl(x, u, t);
+
+ compareCostFunctionOutput(costFunction, costFunctionAD);
+ compareCostFunctionOutput(*costFunctionAD_clone, costFunctionAD);
+ }
+ } catch (std::exception& e)
+ {
+ FAIL();
+ }
+ }
+}
+
+
+/*!
+ * This is simply a little integration test that shows that the tracking cost function term builds and that
+ * its analytic derivatives match NumDiff derivatives.
+ * This unit test tests the tracking cost function and the tracking cost function term.
+ * \example TrackingTest.cpp
+ */
+TEST(CostFunctionTest, TrackingTermTest)
+{
+ const size_t state_dim = 12;
+ const size_t control_dim = 4;
+
+ // analytical costfunction
+ std::shared_ptr<CostFunctionAnalytical<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>());
+
+ Eigen::Matrix<double, state_dim, state_dim> Q;
+ Eigen::Matrix<double, control_dim, control_dim> R;
+ Q.setIdentity();
+ R.setIdentity();
+
+ // create a reference trajectory and fill it with random values
+ core::StateTrajectory<state_dim> stateTraj;
+ core::ControlTrajectory<control_dim> controlTraj;
+ size_t trajSize = 50;
+ bool timeIsAbsolute = true;
+ for (size_t i = 0; i < trajSize; ++i)
+ {
+ stateTraj.push_back(core::StateVector<state_dim>::Random(), double(i), timeIsAbsolute);
+ controlTraj.push_back(core::ControlVector<control_dim>::Random(), double(i), timeIsAbsolute);
+ }
+
+ std::shared_ptr<TermQuadTracking<state_dim, control_dim>> trackingTerm(new TermQuadTracking<state_dim, control_dim>(
+ Q, R, core::InterpolationType::LIN, core::InterpolationType::ZOH, true));
+
+ trackingTerm->setStateAndControlReference(stateTraj, controlTraj);
+ costFunction->addIntermediateTerm(trackingTerm);
+
+ ct::core::StateVector<state_dim> x;
+ ct::core::ControlVector<control_dim> u;
+ x.setRandom();
+ u.setRandom();
+ double t = 0.0;
+
+ costFunction->setCurrentStateAndControl(x, u, t);
+
+ ASSERT_TRUE(costFunction->stateDerivativeIntermediateTest());
+ ASSERT_TRUE(costFunction->controlDerivativeIntermediateTest());
+}
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/costfunction/CostFunctionTests.cpp b/ct_optcon/test/costfunction/CostFunctionTests.cpp
new file mode 100644
index 0000000..840cffb
--- /dev/null
+++ b/ct_optcon/test/costfunction/CostFunctionTests.cpp
@@ -0,0 +1,19 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <gtest/gtest.h>
+#include <ct/optcon/optcon.h>
+
+#include "ADTest_timeDependent.h"
+#include "CostFunctionTest.h"
+
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/costfunction/LoadFromFileTest.cpp b/ct_optcon/test/costfunction/LoadFromFileTest.cpp
new file mode 100644
index 0000000..d2447e9
--- /dev/null
+++ b/ct_optcon/test/costfunction/LoadFromFileTest.cpp
@@ -0,0 +1,188 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <gtest/gtest.h>
+
+#include <ct/optcon/optcon.h>
+
+#include "compareCostFunctions.h"
+#include "costfunction_test_dir.h"
+
+std::string costFunctionFile = std::string(COSTFUNCTION_TEST_DIR) + "/example_cost_file.info";
+const bool verbose = false;
+
+const size_t state_dim = 12;
+const size_t control_dim = 6;
+
+// test state and control
+ct::core::StateVector<state_dim> x = ct::core::StateVector<state_dim>::Random();
+ct::core::ControlVector<control_dim> u = ct::core::ControlVector<control_dim>::Random();
+double t = 0.0;
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+
+//! Test case: trying to load analytical cost function directly from file
+TEST(LoadCostFromFileTest, LoadAnalyticalDirect)
+{
+ try
+ {
+ std::shared_ptr<ct::optcon::CostFunctionAnalytical<state_dim, control_dim>> costFun(
+ new ct::optcon::CostFunctionAnalytical<state_dim, control_dim>(costFunctionFile, verbose));
+
+ // check cloning
+ std::shared_ptr<ct::optcon::CostFunctionAnalytical<state_dim, control_dim>> costFun_cloned(costFun->clone());
+
+ // set states and compare costs and gradients
+ costFun->setCurrentStateAndControl(x, u, t);
+ costFun_cloned->setCurrentStateAndControl(x, u, t);
+ compareCostFunctionOutput<state_dim, control_dim>(*costFun, *costFun_cloned);
+
+ } catch (std::runtime_error& e)
+ {
+ std::cout << "Exception caught: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! Test case: trying to load analytical cost terms separately and then add them cost function
+TEST(LoadCostFromFileTest, LoadAnalyticalViaTerms)
+{
+ try
+ {
+ std::shared_ptr<ct::optcon::CostFunctionAnalytical<state_dim, control_dim>> costFun(
+ new ct::optcon::CostFunctionAnalytical<state_dim, control_dim>());
+
+ // here, we take the detour via loading the terms separately.
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> termQuadratic_intermediate(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>);
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>> termQuadratic_final(
+ new ct::optcon::TermQuadratic<state_dim, control_dim>);
+
+ termQuadratic_intermediate->loadConfigFile(costFunctionFile, "term0", verbose);
+ termQuadratic_final->loadConfigFile(costFunctionFile, "term1", verbose);
+
+ costFun->addIntermediateTerm(termQuadratic_intermediate);
+ costFun->addFinalTerm(termQuadratic_final);
+
+ std::shared_ptr<ct::optcon::CostFunctionAnalytical<state_dim, control_dim>> costFun_cloned(costFun->clone());
+
+
+ // set states and compare costs and gradients
+ costFun->setCurrentStateAndControl(x, u, t);
+ costFun_cloned->setCurrentStateAndControl(x, u, t);
+ compareCostFunctionOutput<state_dim, control_dim>(*costFun, *costFun_cloned);
+
+ } catch (std::runtime_error& e)
+ {
+ std::cout << "Exception caught: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! Test case: trying to load AD cost terms separately and then add them to the cost function
+TEST(LoadCostFromFileTest, LoadADViaTerms)
+{
+ try
+ {
+ std::shared_ptr<ct::optcon::CostFunctionAD<state_dim, control_dim>> costFun(
+ new ct::optcon::CostFunctionAD<state_dim, control_dim>());
+
+ // here, we take the detour via loading the terms separately.
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>>
+ termQuadraticAD_intermediate(
+ new ct::optcon::TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>);
+ std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>>
+ termQuadraticAD_final(new ct::optcon::TermQuadratic<state_dim, control_dim, double, ct::core::ADCGScalar>);
+
+ termQuadraticAD_intermediate->loadConfigFile(costFunctionFile, "term0", verbose);
+ termQuadraticAD_final->loadConfigFile(costFunctionFile, "term1", verbose);
+
+ costFun->addIntermediateADTerm(termQuadraticAD_intermediate);
+ costFun->addFinalADTerm(termQuadraticAD_final);
+ costFun->initialize(); // todo we need to get rid of this initialize call()
+
+ std::shared_ptr<ct::optcon::CostFunctionAD<state_dim, control_dim>> costFun_cloned(costFun->clone());
+
+ // set states and compare costs and gradients
+ costFun->setCurrentStateAndControl(x, u, t);
+ costFun_cloned->setCurrentStateAndControl(x, u, t);
+ compareCostFunctionOutput<state_dim, control_dim>(*costFun, *costFun_cloned);
+
+ } catch (std::runtime_error& e)
+ {
+ std::cout << "Exception caught: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+//! Test case: trying to load AD cost directly, but calling standard constructor first.
+TEST(LoadCostFromFileTest, LoadADDirect1)
+{
+ try
+ {
+ // first create object
+ std::shared_ptr<ct::optcon::CostFunctionAD<state_dim, control_dim>> costFun(
+ new ct::optcon::CostFunctionAD<state_dim, control_dim>());
+ // then load and initialize
+ costFun->loadFromConfigFile(costFunctionFile, verbose);
+ costFun->initialize(); // todo need to get rid of this init call
+
+ std::shared_ptr<ct::optcon::CostFunctionAD<state_dim, control_dim>> costFun_cloned(costFun->clone());
+
+ // set states and compare costs and gradients
+ costFun->setCurrentStateAndControl(x, u, t);
+ costFun_cloned->setCurrentStateAndControl(x, u, t);
+ compareCostFunctionOutput<state_dim, control_dim>(*costFun, *costFun_cloned);
+
+ } catch (std::runtime_error& e)
+ {
+ std::cout << "Exception caught: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+//! Test case: trying to load AD cost directly
+TEST(LoadCostFromFileTest, LoadADDirect2)
+{
+ try
+ {
+ // test constructor taking the cost function file directly
+ std::shared_ptr<ct::optcon::CostFunctionAD<state_dim, control_dim>> costFun(
+ new ct::optcon::CostFunctionAD<state_dim, control_dim>(costFunctionFile, verbose));
+ costFun->initialize(); // todo need to get rid of this init call
+
+ std::shared_ptr<ct::optcon::CostFunctionAD<state_dim, control_dim>> costFun_cloned(costFun->clone());
+
+ // set states and compare costs and gradients
+ costFun->setCurrentStateAndControl(x, u, t);
+ costFun_cloned->setCurrentStateAndControl(x, u, t);
+ compareCostFunctionOutput<state_dim, control_dim>(*costFun, *costFun_cloned);
+
+ } catch (std::runtime_error& e)
+ {
+ std::cout << "Exception caught: " << e.what() << std::endl;
+ ASSERT_TRUE(false);
+ }
+}
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
+
+
+int main(int argc, char** argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/costfunction/compareCostFunctions.h b/ct_optcon/test/costfunction/compareCostFunctions.h
new file mode 100644
index 0000000..9c1185d
--- /dev/null
+++ b/ct_optcon/test/costfunction/compareCostFunctions.h
@@ -0,0 +1,53 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+/*!
+ * @brief This method is called from different unit tests in order to compare the cost, first and second order gradients of two cost functions
+ * @param costFunction the first cost function to be compared
+ * @param costFunction2 the second cost function to be compared
+ */
+template <size_t state_dim, size_t control_dim>
+void compareCostFunctionOutput(CostFunctionQuadratic<state_dim, control_dim>& costFunction,
+ CostFunctionQuadratic<state_dim, control_dim>& costFunction2)
+{
+ ASSERT_NEAR(costFunction.evaluateIntermediate(), costFunction2.evaluateIntermediate(), 1e-9);
+ ASSERT_NEAR(costFunction.evaluateTerminal(), costFunction2.evaluateTerminal(), 1e-9);
+
+ ASSERT_TRUE(costFunction.stateDerivativeIntermediate().isApprox(costFunction2.stateDerivativeIntermediate()));
+ ASSERT_TRUE(costFunction.stateDerivativeTerminal().isApprox(costFunction2.stateDerivativeTerminal()));
+
+ ASSERT_TRUE(
+ costFunction.stateSecondDerivativeIntermediate().isApprox(costFunction2.stateSecondDerivativeIntermediate()));
+ ASSERT_TRUE(costFunction.stateSecondDerivativeTerminal().isApprox(costFunction2.stateSecondDerivativeTerminal()));
+
+ ASSERT_TRUE(costFunction.controlDerivativeIntermediate().isApprox(costFunction2.controlDerivativeIntermediate()));
+ ASSERT_TRUE(costFunction.controlDerivativeTerminal().isApprox(costFunction2.controlDerivativeTerminal()));
+
+ ASSERT_TRUE(costFunction.controlSecondDerivativeIntermediate().isApprox(
+ costFunction2.controlSecondDerivativeIntermediate()));
+ ASSERT_TRUE(
+ costFunction.controlSecondDerivativeTerminal().isApprox(costFunction2.controlSecondDerivativeTerminal()));
+
+ ASSERT_TRUE(
+ costFunction.stateControlDerivativeIntermediate().isApprox(costFunction2.stateControlDerivativeIntermediate()));
+ ASSERT_TRUE(costFunction.stateControlDerivativeTerminal().isApprox(costFunction2.stateControlDerivativeTerminal()));
+
+ // second derivatives have to be symmetric
+ ASSERT_TRUE(costFunction.stateSecondDerivativeIntermediate().isApprox(
+ costFunction.stateSecondDerivativeIntermediate().transpose()));
+ ASSERT_TRUE(costFunction.controlSecondDerivativeIntermediate().isApprox(
+ costFunction.controlSecondDerivativeIntermediate().transpose()));
+}
+
+} // namespace example
+} // namesapce optcon
+} // namespace ct
diff --git a/ct_optcon/test/costfunction/costfunction_test_dir.h.in b/ct_optcon/test/costfunction/costfunction_test_dir.h.in
new file mode 100644
index 0000000..433cd3a
--- /dev/null
+++ b/ct_optcon/test/costfunction/costfunction_test_dir.h.in
@@ -0,0 +1 @@
+#define COSTFUNCTION_TEST_DIR "@COSTFUNCTION_TEST_DIR@"
diff --git a/ct_optcon/test/costfunction/example_cost_file.info b/ct_optcon/test/costfunction/example_cost_file.info
new file mode 100644
index 0000000..0b3ac01
--- /dev/null
+++ b/ct_optcon/test/costfunction/example_cost_file.info
@@ -0,0 +1,115 @@
+term0
+{
+ name "intermediate cost"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ scaling 5
+
+ ; joint position
+ (0,0) 2.0
+ (1,1) 2.0
+ (2,2) 2.0
+ (3,3) 2.0
+ (4,4) 2.0
+ (5,5) 2.0
+
+ ; joint velocity
+ (6,6) 10.0
+ (7,7) 10.0
+ (8,8) 10.0
+ (9,9) 10.0
+ (10,10) 10.0
+ (11,11) 10.0
+ }
+ R
+ {
+
+ scaling 0.5
+ (0,0) 1.0
+ (1,1) 1.0
+ (2,2) 2.0
+ (3,3) 3.0
+ (4,4) 4.0
+ (5,5) 5.0
+ }
+
+
+ x_des
+ {
+ ; joint position
+ (0,0) 0.0
+ (1,0) 0.0
+ (2,0) 0.0
+ (3,0) 0.0
+ (4,0) 0.0
+ (5,0) 0.0
+
+ ; joint velocity
+ (6,0) 0.0
+ (7,0) 0.0
+ (8,0) 0.0
+ (9,0) 0.0
+ (10,0) 0.0
+ (11,0) 0.0
+ }
+ }
+}
+
+
+term1
+{
+ name "final cost"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ scaling 100.0
+
+ ; joint position
+ (0,0) 100.0
+ (1,1) 100.0
+ (2,2) 100.0
+ (3,3) 100.0
+ (4,4) 100.0
+ (5,5) 100.0
+
+
+ ; joint velocity
+ (6,6) 10.0
+ (7,7) 10.0
+ (8,8) 10.0
+ (9,9) 10.0
+ (10,10) 10.0
+ (11,11) 10.0
+ }
+
+ x_des
+ {
+ ; joint position
+ (0,0) 1.5
+ (1,0) 1.5
+ (2,0) 1.0
+ (3,0) 1.0
+ (4,0) 1.0
+ (5,0) 1.0
+
+ ; joint velocity
+ (6,0) 0.0
+ (7,0) 0.0
+ (8,0) 0.0
+ (9,0) 0.0
+ (10,0) 0.0
+ (11,0) 0.0
+ }
+ }
+}
\ No newline at end of file
diff --git a/ct_optcon/test/dms/oscillator/matfiles/matFilesGenerator.cpp b/ct_optcon/test/dms/oscillator/matfiles/matFilesGenerator.cpp
new file mode 100644
index 0000000..5da9925
--- /dev/null
+++ b/ct_optcon/test/dms/oscillator/matfiles/matFilesGenerator.cpp
@@ -0,0 +1,235 @@
+/**********************************************************************************************************************
+Copyright (c) 2016, Agile & Dexterous Robotics Lab, ETH ZURICH. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name of ETH ZURICH nor the names of its contributors may be used
+ to endorse or promote products derived from this software without specific
+ prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+
+#include "oscDMSTest_settings.h"
+
+#ifdef MATLAB
+#include <ct/optcon/matlab.hpp>
+#endif
+
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+using namespace ct;
+using namespace optcon;
+
+
+class MatFilesGenerator
+{
+public:
+ typedef DmsDimensions<2, 1> OscDimensions;
+
+ MatFilesGenerator() : w_n_(0.5), zeta_(0.01)
+ {
+ matlabPathIPOPT_ = std::string(DATA_DIR) + "/solutionIpopt.mat";
+ matlabPathSNOPT_ = std::string(DATA_DIR) + "/solutionSnopt.mat";
+ settings_.N_ = 25;
+ settings_.T_ = 5.0;
+ settings_.nThreads_ = 2;
+ settings_.splineType_ = DmsSettings::PIECEWISE_LINEAR;
+ settings_.costEvaluationType_ = DmsSettings::FULL;
+ settings_.objectiveType_ = DmsSettings::KEEP_TIME_AND_GRID;
+ settings_.h_min_ = 0.1; // minimum admissible distance between two nodes in [sec]
+ settings_.integrationType_ = DmsSettings::RK4;
+ settings_.dt_sim_ = 0.01;
+ settings_.absErrTol_ = 1e-8;
+ settings_.relErrTol_ = 1e-8;
+ }
+
+ void initialize()
+ {
+ oscillator_ = std::shared_ptr<ct::core::SecondOrderSystem>(new ct::core::SecondOrderSystem(w_n_, zeta_));
+ x_0_ << 0.0, 0.0;
+ x_final_ << 2.0, -1.0;
+ Q_ << 0.0, 0.0, 0.0, 10.0;
+
+ Q_final_ << 0.0, 0.0, 0.0, 0.0;
+
+ R_ << 0.001;
+ u_des_ << 0.0;
+
+ costFunction_ = std::shared_ptr<ct::optcon::CostFunctionQuadratic<2, 1>>(
+ new ct::optcon::CostFunctionQuadraticSimple<2, 1>(Q_, R_, x_final_, u_des_, x_final_, Q_final_));
+ }
+
+ void generateMatFilesIPOPT()
+ {
+ settings_.solverSettings_.solverType_ = NlpSolverSettings::IPOPT;
+
+ generalConstraints_ = std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>>(
+ new ct::optcon::ConstraintContainerAnalytical<2, 1>());
+
+ std::shared_ptr<TerminalConstraint<2, 1>> termConstraint(new TerminalConstraint<2, 1>(x_final_));
+
+ termConstraint->setName("crazyTerminalConstraint");
+ generalConstraints_->addTerminalConstraint(termConstraint, true);
+
+ OptConProblem<2, 1> optProblem(oscillator_, costFunction_);
+ optProblem.setInitialState(x_0_);
+
+ optProblem.setTimeHorizon(settings_.T_);
+ optProblem.setGeneralConstraints(generalConstraints_);
+
+ dmsPlanner_ = std::shared_ptr<DmsSolver<2, 1>>(new DmsSolver<2, 1>(optProblem, settings_));
+
+ calcInitGuess();
+ dmsPlanner_->setInitialGuess(initialPolicy_);
+ dmsPlanner_->solve();
+ solutionPolicy_ = dmsPlanner_->getSolution();
+
+ //Solution Containers
+ OscDimensions::state_vector_array_t stateSolutionIpopt;
+ OscDimensions::control_vector_array_t inputSolutionIpopt;
+ OscDimensions::time_array_t timeSolutionIpopt;
+ stateSolutionIpopt = solutionPolicy_.xSolution_;
+ inputSolutionIpopt = solutionPolicy_.uSolution_;
+ timeSolutionIpopt = solutionPolicy_.tSolution_;
+
+
+#ifdef MATLAB
+ matlab::MatFile matFile;
+ matFile.open(matlabPathIPOPT_);
+ matFile.put("stateDmsIpopt", stateSolutionIpopt.toImplementation());
+ matFile.put("inputDmsIpopt", inputSolutionIpopt.toImplementation());
+ matFile.put("timeDmsIpopt", timeSolutionIpopt.toEigenTrajectory());
+ matFile.close();
+#endif //MATLAB
+ }
+
+ void generateMatFilesSNOPT()
+ {
+ settings_.solverSettings_.solverType_ = NlpSolverSettings::SNOPT;
+
+ generalConstraints_ = std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>>(
+ new ct::optcon::ConstraintContainerAnalytical<2, 1>());
+
+ std::shared_ptr<TerminalConstraint<2, 1>> termConstraint(new TerminalConstraint<2, 1>(x_final_));
+
+ termConstraint->setName("crazyTerminalConstraint");
+ generalConstraints_->addTerminalConstraint(termConstraint, true);
+
+ OptConProblem<2, 1> optProblem(oscillator_, costFunction_);
+ optProblem.setInitialState(x_0_);
+
+ optProblem.setTimeHorizon(settings_.T_);
+ optProblem.setGeneralConstraints(generalConstraints_);
+ generalConstraints_->initialize();
+
+ dmsPlanner_ = std::shared_ptr<DmsSolver<2, 1>>(new DmsSolver<2, 1>(optProblem, settings_));
+
+ calcInitGuess();
+ dmsPlanner_->setInitialGuess(initialPolicy_);
+ dmsPlanner_->solve();
+ solutionPolicy_ = dmsPlanner_->getSolution();
+
+ //Solution Containers
+ OscDimensions::state_vector_array_t stateSolutionSnopt;
+ OscDimensions::control_vector_array_t inputSolutionSnopt;
+ OscDimensions::time_array_t timeSolutionSnopt;
+ stateSolutionSnopt = solutionPolicy_.xSolution_;
+ inputSolutionSnopt = solutionPolicy_.uSolution_;
+ timeSolutionSnopt = solutionPolicy_.tSolution_;
+
+#ifdef MATLAB
+ matlab::MatFile matFile;
+ matFile.open(matlabPathSNOPT_);
+ matFile.put("stateDmsSnopt", stateSolutionSnopt.toImplementation());
+ matFile.put("inputDmsSnopt", inputSolutionSnopt.toImplementation());
+ matFile.put("timeDmsSnopt", timeSolutionSnopt.toEigenTrajectory());
+ matFile.close();
+#endif //MATLAB
+ }
+
+
+private:
+ void calcInitGuess()
+ {
+ x_initguess_.resize(settings_.N_ + 1, OscDimensions::state_vector_t::Zero());
+ u_initguess_.resize(settings_.N_ + 1, OscDimensions::control_vector_t::Zero());
+ for (size_t i = 0; i < settings_.N_ + 1; ++i)
+ {
+ x_initguess_[i] = x_0_ + (x_final_ - x_0_) * (i / settings_.N_);
+ }
+
+ initialPolicy_.xSolution_ = x_initguess_;
+ initialPolicy_.uSolution_ = u_initguess_;
+ }
+
+ double w_n_;
+ double zeta_;
+ std::shared_ptr<ct::core::SecondOrderSystem> oscillator_;
+
+ std::string matlabPathIPOPT_;
+ std::string matlabPathSNOPT_;
+
+ DmsSettings settings_;
+ std::shared_ptr<DmsSolver<2, 1>> dmsPlanner_;
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<2, 1>> costFunction_;
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>> generalConstraints_;
+
+
+ DmsPolicy<2, 1> initialPolicy_;
+ DmsPolicy<2, 1> solutionPolicy_;
+ OscDimensions::state_vector_array_t x_initguess_;
+ OscDimensions::control_vector_array_t u_initguess_;
+
+ OscDimensions::state_vector_t x_0_;
+ OscDimensions::state_vector_t x_final_;
+ OscDimensions::state_matrix_t Q_;
+ OscDimensions::state_matrix_t Q_final_;
+ OscDimensions::control_matrix_t R_;
+ OscDimensions::control_vector_t u_des_;
+};
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
+
+
+/*!
+ * This executable is used to generate the reference matfiles for the unit tests.
+ */
+int main(int argc, char* argv[])
+{
+ using namespace ct::optcon::example;
+
+ MatFilesGenerator oscDms;
+ oscDms.initialize();
+#ifdef BUILD_WITH_SNOPT_SUPPORT
+ std::cout << "Generating Mat Files using SNOPT:" << std::endl;
+ oscDms.generateMatFilesSNOPT();
+#endif // BUILD_WITH_IPOPT_SUPPORT
+
+#ifdef BUILD_WITH_IPOPT_SUPPORT
+ std::cout << "Generating Mat Files using IPOPT:" << std::endl;
+ oscDms.generateMatFilesIPOPT();
+#endif // BUILD_WITH_IPOPT_SUPPORT
+
+ return 0;
+}
diff --git a/ct_optcon/test/dms/oscillator/matfiles/solutionIpopt.mat b/ct_optcon/test/dms/oscillator/matfiles/solutionIpopt.mat
new file mode 100644
index 0000000..67ca9ab
--- /dev/null
+++ b/ct_optcon/test/dms/oscillator/matfiles/solutionIpopt.mat
Binary files differ
diff --git a/ct_optcon/test/dms/oscillator/matfiles/solutionSnopt.mat b/ct_optcon/test/dms/oscillator/matfiles/solutionSnopt.mat
new file mode 100644
index 0000000..d19d421
--- /dev/null
+++ b/ct_optcon/test/dms/oscillator/matfiles/solutionSnopt.mat
Binary files differ
diff --git a/ct_optcon/test/dms/oscillator/matfiles/sparsityPattern.mat b/ct_optcon/test/dms/oscillator/matfiles/sparsityPattern.mat
new file mode 100644
index 0000000..d49ed3d
--- /dev/null
+++ b/ct_optcon/test/dms/oscillator/matfiles/sparsityPattern.mat
Binary files differ
diff --git a/ct_optcon/test/dms/oscillator/oscDMSTest.cpp b/ct_optcon/test/dms/oscillator/oscDMSTest.cpp
new file mode 100644
index 0000000..bf9381a
--- /dev/null
+++ b/ct_optcon/test/dms/oscillator/oscDMSTest.cpp
@@ -0,0 +1,277 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This file implements dms unit tests.
+ * For more intuitive examples, visit the tutorial.
+ * \example oscDMSTest.cpp
+ */
+
+#include <ct/optcon/optcon.h>
+#include <gtest/gtest.h>
+
+#include "oscDMSTest_settings.h"
+
+
+#ifdef MATLAB
+#include <ct/optcon/matlab.hpp>
+#endif
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+class OscDms
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef DmsDimensions<2, 1> OscDimensions;
+
+ OscDms() : w_n_(0.5), zeta_(0.01)
+ {
+ matlabPathIPOPT_ = std::string(DATA_DIR) + "/solutionIpopt.mat";
+ matlabPathSNOPT_ = std::string(DATA_DIR) + "/solutionSnopt.mat";
+
+ settings_.N_ = 25;
+ settings_.T_ = 5.0;
+ settings_.nThreads_ = 4;
+ settings_.splineType_ = DmsSettings::PIECEWISE_LINEAR;
+ settings_.costEvaluationType_ = DmsSettings::FULL;
+ settings_.objectiveType_ = DmsSettings::KEEP_TIME_AND_GRID;
+ settings_.h_min_ = 0.1; // minimum admissible distance between two nodes in [sec]
+ settings_.integrationType_ = DmsSettings::RK4;
+ settings_.dt_sim_ = 0.01;
+ settings_.absErrTol_ = 1e-8;
+ settings_.relErrTol_ = 1e-8;
+
+ settings_.print();
+ }
+
+ ~OscDms() { std::cout << "Oscillator dms destructor called" << std::endl; }
+ void getIpoptMatlabTrajectories()
+ {
+#ifdef MATLAB
+ matFileIpopt_.open(matlabPathIPOPT_, matlab::MatFile::READ);
+ assert(matFileIpopt_.isOpen());
+ matFileIpopt_.get("stateDmsIpopt", stateMatIpopt_.toImplementation());
+ matFileIpopt_.get("inputDmsIpopt", inputMatIpopt_.toImplementation());
+ std::vector<Eigen::Matrix<double, 1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 1, 1>>> timeEigen;
+ matFileIpopt_.get("timeDmsIpopt", timeEigen);
+ timeMatIpopt_.fromEigenTrajectory(timeEigen);
+ matFileIpopt_.close();
+#endif
+ }
+
+ void getSnoptMatlabTrajectories()
+ {
+#ifdef MATLAB
+ matFileSnopt_.open(matlabPathSNOPT_, matlab::MatFile::READ);
+ assert(matFileSnopt_.isOpen());
+ matFileSnopt_.get("stateDmsSnopt", stateMatSnopt_.toImplementation());
+ matFileSnopt_.get("inputDmsSnopt", inputMatSnopt_.toImplementation());
+ std::vector<Eigen::Matrix<double, 1, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 1, 1>>> timeEigen;
+ matFileSnopt_.get("timeDmsSnopt", timeEigen);
+ timeMatSnopt_.fromEigenTrajectory(timeEigen);
+ matFileSnopt_.close();
+#endif
+ }
+
+ void initialize()
+ {
+ oscillator_ = std::shared_ptr<ct::core::SecondOrderSystem>(new ct::core::SecondOrderSystem(w_n_, zeta_));
+ x_0_ << 0.0, 0.0;
+ x_final_ << 2.0, -1.0;
+ Q_ << 0.0, 0.0, 0.0, 10.0;
+
+ Q_final_ << 0.0, 0.0, 0.0, 0.0;
+
+ R_ << 0.001;
+ u_des_ << 0.0;
+
+ costFunction_ = std::shared_ptr<ct::optcon::CostFunctionQuadratic<2, 1>>(
+ new ct::optcon::CostFunctionQuadraticSimple<2, 1>(Q_, R_, x_final_, u_des_, x_final_, Q_final_));
+ }
+
+ void getIpoptSolution()
+ {
+ settings_.solverSettings_.solverType_ = NlpSolverSettings::IPOPT;
+
+ generalConstraints_ = std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>>(
+ new ct::optcon::ConstraintContainerAnalytical<2, 1>());
+
+ std::shared_ptr<TerminalConstraint<2, 1>> termConstraint(new TerminalConstraint<2, 1>(x_final_));
+
+ termConstraint->setName("TerminalConstraint");
+ generalConstraints_->addTerminalConstraint(termConstraint, true);
+ generalConstraints_->initialize();
+
+ OptConProblem<2, 1> optProblem(oscillator_, costFunction_);
+ optProblem.setInitialState(x_0_);
+
+ optProblem.setTimeHorizon(settings_.T_);
+ optProblem.setGeneralConstraints(generalConstraints_);
+ dmsPlanner_ = std::shared_ptr<DmsSolver<2, 1>>(new DmsSolver<2, 1>(optProblem, settings_));
+
+ calcInitGuess();
+ dmsPlanner_->setInitialGuess(initialPolicy_);
+ dmsPlanner_->solve();
+ solutionPolicy_ = dmsPlanner_->getSolution();
+
+
+ stateSolutionIpopt_ = solutionPolicy_.xSolution_;
+ inputSolutionIpopt_ = solutionPolicy_.uSolution_;
+ timeSolutionIpopt_ = solutionPolicy_.tSolution_;
+ }
+
+ void getSnoptSolution()
+ {
+ settings_.solverSettings_.solverType_ = NlpSolverSettings::SNOPT;
+
+ generalConstraints_ = std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>>(
+ new ct::optcon::ConstraintContainerAnalytical<2, 1>());
+
+ std::shared_ptr<TerminalConstraint<2, 1>> termConstraint(new TerminalConstraint<2, 1>(x_final_));
+
+ termConstraint->setName("TerminalConstraint");
+ generalConstraints_->addTerminalConstraint(termConstraint, true);
+ generalConstraints_->initialize();
+
+ OptConProblem<2, 1> optProblem(oscillator_, costFunction_);
+ optProblem.setInitialState(x_0_);
+
+ optProblem.setTimeHorizon(settings_.T_);
+ optProblem.setGeneralConstraints(generalConstraints_);
+ dmsPlanner_ = std::shared_ptr<DmsSolver<2, 1>>(new DmsSolver<2, 1>(optProblem, settings_));
+
+ calcInitGuess();
+ dmsPlanner_->setInitialGuess(initialPolicy_);
+ dmsPlanner_->solve();
+ solutionPolicy_ = dmsPlanner_->getSolution();
+
+ stateSolutionSnopt_ = solutionPolicy_.xSolution_;
+ inputSolutionSnopt_ = solutionPolicy_.uSolution_;
+ timeSolutionSnopt_ = solutionPolicy_.tSolution_;
+ }
+
+ void compareIpoptSolutions()
+ {
+#ifdef MATLAB
+ ASSERT_TRUE(stateSolutionIpopt_.size() == stateMatIpopt_.size());
+ for (size_t i = 0; i < stateSolutionIpopt_.size(); ++i)
+ {
+ ASSERT_TRUE(stateSolutionIpopt_[i].isApprox(stateMatIpopt_[i]));
+ ASSERT_TRUE(inputSolutionIpopt_[i].isApprox(inputMatIpopt_[i]));
+ ASSERT_TRUE(timeSolutionIpopt_[i] == timeMatIpopt_[i]);
+ }
+#endif
+ }
+
+ void compareSnoptSolutions()
+ {
+#ifdef MATLAB
+ ASSERT_TRUE(stateSolutionSnopt_.size() == stateMatSnopt_.size());
+ for (size_t i = 0; i < stateSolutionSnopt_.size(); ++i)
+ {
+ ASSERT_TRUE(stateSolutionSnopt_[i].isApprox(stateMatSnopt_[i]));
+ ASSERT_TRUE(inputSolutionSnopt_[i].isApprox(inputMatSnopt_[i]));
+ ASSERT_TRUE(timeSolutionSnopt_[i] == timeMatSnopt_[i]);
+ }
+#endif
+ }
+
+
+private:
+ void calcInitGuess()
+ {
+ x_initguess_.resize(settings_.N_ + 1, OscDimensions::state_vector_t::Zero());
+ u_initguess_.resize(settings_.N_ + 1, OscDimensions::control_vector_t::Zero());
+ for (size_t i = 0; i < settings_.N_ + 1; ++i)
+ {
+ x_initguess_[i] = x_0_ + (x_final_ - x_0_) * (i / settings_.N_);
+ }
+
+ initialPolicy_.xSolution_ = x_initguess_;
+ initialPolicy_.uSolution_ = u_initguess_;
+ }
+
+ double w_n_;
+ double zeta_;
+ std::shared_ptr<ct::core::SecondOrderSystem> oscillator_;
+
+ std::string matlabPathIPOPT_;
+ std::string matlabPathSNOPT_;
+
+ DmsSettings settings_;
+ std::shared_ptr<DmsSolver<2, 1>> dmsPlanner_;
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<2, 1>> costFunction_;
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>> generalConstraints_;
+
+ OscDimensions::state_vector_t x_0_;
+ OscDimensions::state_vector_t x_final_;
+ OscDimensions::state_matrix_t Q_;
+ OscDimensions::state_matrix_t Q_final_;
+ OscDimensions::control_matrix_t R_;
+ OscDimensions::control_vector_t u_des_;
+
+ DmsPolicy<2, 1> initialPolicy_;
+ DmsPolicy<2, 1> solutionPolicy_;
+ OscDimensions::state_vector_array_t x_initguess_;
+ OscDimensions::control_vector_array_t u_initguess_;
+
+ //Solutions loaded from Mat Files
+ OscDimensions::state_vector_array_t stateMatIpopt_;
+ OscDimensions::control_vector_array_t inputMatIpopt_;
+ OscDimensions::time_array_t timeMatIpopt_;
+ OscDimensions::state_vector_array_t stateMatSnopt_;
+ OscDimensions::control_vector_array_t inputMatSnopt_;
+ OscDimensions::time_array_t timeMatSnopt_;
+
+ //Solution obtained by IPOPT, SNOPT
+ OscDimensions::state_vector_array_t stateSolutionIpopt_;
+ OscDimensions::control_vector_array_t inputSolutionIpopt_;
+ OscDimensions::time_array_t timeSolutionIpopt_;
+ OscDimensions::state_vector_array_t stateSolutionSnopt_;
+ OscDimensions::control_vector_array_t inputSolutionSnopt_;
+ OscDimensions::time_array_t timeSolutionSnopt_;
+
+#ifdef MATLAB
+ matlab::MatFile matFileIpopt_;
+ matlab::MatFile matFileSnopt_;
+#endif //MATLAB
+};
+
+TEST(DmsTest, OscDmsTest)
+{
+ OscDms oscDms;
+ oscDms.initialize();
+#ifdef BUILD_WITH_SNOPT_SUPPORT
+ oscDms.getSnoptMatlabTrajectories();
+ oscDms.getSnoptSolution();
+ oscDms.compareSnoptSolutions();
+#endif // BUILD_WITH_SNOPT_SUPPORT
+
+#ifdef BUILD_WITH_IPOPT_SUPPORT
+ oscDms.getIpoptMatlabTrajectories();
+ oscDms.getIpoptSolution();
+ oscDms.compareIpoptSolutions();
+#endif // BUILD_WITH_IPOPT_SUPPORT
+}
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
+
+/*!
+ * This unit test applies Direct Multiple Shooting to an oscillator system, uses different solvers and compares the outputs.
+ * \example oscDMSTest.cpp
+ */
+int main(int argc, char **argv)
+{
+ // using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/dms/oscillator/oscDMSTestAllVariants.cpp b/ct_optcon/test/dms/oscillator/oscDMSTestAllVariants.cpp
new file mode 100644
index 0000000..fdf4790
--- /dev/null
+++ b/ct_optcon/test/dms/oscillator/oscDMSTestAllVariants.cpp
@@ -0,0 +1,161 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This unit test applies Direct Multiple Shooting to an oscillator system. It employs all possible combinations of settings and solvers.
+ * \example oscDMSTestAllVariants.cpp
+ */
+
+#include <ct/optcon/optcon.h>
+#include <gtest/gtest.h>
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+class OscillatorDms
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef DmsDimensions<2, 1> OscDimensions;
+
+ OscillatorDms() : w_n_(0.5), zeta_(0.01) {}
+ ~OscillatorDms() {}
+ void initialize(const DmsSettings settings)
+ {
+ settings_ = settings;
+ settings_.print();
+
+ oscillator_ = std::shared_ptr<ct::core::SecondOrderSystem>(new ct::core::SecondOrderSystem(w_n_, zeta_));
+ x_0_ << 0.0, 0.0;
+ x_final_ << 2.0, -1.0;
+ Q_ << 0.0, 0.0, 0.0, 10.0;
+
+ Q_final_ << 0.0, 0.0, 0.0, 0.0;
+
+ R_ << 0.001;
+ u_des_ << 0.0;
+
+ costFunction_ = std::shared_ptr<ct::optcon::CostFunctionQuadratic<2, 1>>(
+ new ct::optcon::CostFunctionQuadraticSimple<2, 1>(Q_, R_, x_final_, u_des_, x_final_, Q_final_));
+
+
+ generalConstraints_ = std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>>(
+ new ct::optcon::ConstraintContainerAnalytical<2, 1>());
+
+ std::shared_ptr<TerminalConstraint<2, 1>> termConstraint(new TerminalConstraint<2, 1>(x_final_));
+
+ termConstraint->setName("crazyTerminalConstraint");
+
+ generalConstraints_->addTerminalConstraint(termConstraint, true);
+
+ generalConstraints_->initialize();
+
+ OptConProblem<2, 1> optProblem(oscillator_, costFunction_);
+ optProblem.setInitialState(x_0_);
+ optProblem.setTimeHorizon(settings_.T_);
+ optProblem.setGeneralConstraints(generalConstraints_);
+
+ calcInitGuess();
+ dmsPlanner_ = std::shared_ptr<DmsSolver<2, 1>>(new DmsSolver<2, 1>(optProblem, settings_));
+ dmsPlanner_->setInitialGuess(initialPolicy_);
+ }
+
+ void getSolution()
+ {
+ dmsPlanner_->solve();
+ // dmsPlanner_->printSolution();
+ }
+
+
+private:
+ void calcInitGuess()
+ {
+ x_initguess_.resize(settings_.N_ + 1, OscDimensions::state_vector_t::Zero());
+ u_initguess_.resize(settings_.N_ + 1, OscDimensions::control_vector_t::Zero());
+ for (size_t i = 0; i < settings_.N_ + 1; ++i)
+ {
+ x_initguess_[i] = x_0_ + (x_final_ - x_0_) * ((double)i / (double)settings_.N_);
+ }
+
+ initialPolicy_.xSolution_ = x_initguess_;
+ initialPolicy_.uSolution_ = u_initguess_;
+ }
+
+ double w_n_;
+ double zeta_;
+ std::shared_ptr<ct::core::SecondOrderSystem> oscillator_;
+
+ DmsSettings settings_;
+ std::shared_ptr<DmsSolver<2, 1>> dmsPlanner_;
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<2, 1>> costFunction_;
+ std::shared_ptr<ct::optcon::ConstraintContainerAnalytical<2, 1>> generalConstraints_;
+
+ OscDimensions::state_vector_t x_0_;
+ OscDimensions::state_vector_t x_final_;
+ OscDimensions::state_matrix_t Q_;
+ OscDimensions::state_matrix_t Q_final_;
+ OscDimensions::control_matrix_t R_;
+ OscDimensions::control_vector_t u_des_;
+
+ DmsPolicy<2, 1> initialPolicy_;
+ OscDimensions::state_vector_array_t x_initguess_;
+ OscDimensions::control_vector_array_t u_initguess_;
+};
+
+TEST(DmsTest, OscillatorDmsTestAllVariants)
+{
+ for (int splineType = 0; splineType < DmsSettings::SplineType::num_types_splining; splineType++)
+ {
+ for (int costEvalT = 0; costEvalT < DmsSettings::CostEvaluationType::num_types_costevaluation; costEvalT++)
+ {
+ DmsSettings settings;
+ settings.N_ = 25;
+ settings.T_ = 5.0;
+ settings.nThreads_ = 1;
+ settings.splineType_ = static_cast<DmsSettings::SplineType>(splineType); // ZOH, PWL
+ settings.costEvaluationType_ = static_cast<DmsSettings::CostEvaluationType>(costEvalT); // SIMPLE, FULL
+ settings.objectiveType_ = static_cast<DmsSettings::ObjectiveType>(0); // keep grid, opt. grid
+ settings.h_min_ = 0.1;
+ settings.integrationType_ = DmsSettings::RK4;
+ settings.dt_sim_ = 0.01;
+ settings.absErrTol_ = 1e-6;
+ settings.relErrTol_ = 1e-6;
+
+#ifdef BUILD_WITH_SNOPT_SUPPORT
+ NlpSolverSettings nlpsettings;
+ nlpsettings.solverType_ = static_cast<NlpSolverSettings::SolverType>(0); // IPOPT, SNOPT
+ settings.solverSettings_ = nlpsettings;
+ OscillatorDms oscDms;
+ oscDms.initialize(settings);
+ oscDms.getSolution();
+#endif
+
+#ifdef BUILD_WITH_IPOPT_SUPPORT
+ NlpSolverSettings nlpsettings_ipopt;
+ nlpsettings_ipopt.solverType_ = static_cast<NlpSolverSettings::SolverType>(1); // IPOPT, SNOPT
+ settings.solverSettings_ = nlpsettings_ipopt;
+ OscillatorDms oscDms_ipopt;
+ oscDms_ipopt.initialize(settings);
+ oscDms_ipopt.getSolution();
+#endif
+ }
+ }
+}
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
+
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/dms/oscillator/oscDMSTest_settings.h.in b/ct_optcon/test/dms/oscillator/oscDMSTest_settings.h.in
new file mode 100644
index 0000000..ed4de72
--- /dev/null
+++ b/ct_optcon/test/dms/oscillator/oscDMSTest_settings.h.in
@@ -0,0 +1 @@
+#define DATA_DIR "@DMS_OSC_TEST_MAT_DIR@"
diff --git a/ct_optcon/test/lqr/LqrTest.cpp b/ct_optcon/test/lqr/LqrTest.cpp
new file mode 100644
index 0000000..49f1a92
--- /dev/null
+++ b/ct_optcon/test/lqr/LqrTest.cpp
@@ -0,0 +1,15 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+#include "LqrTest.h"
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/lqr/LqrTest.h b/ct_optcon/test/lqr/LqrTest.h
new file mode 100644
index 0000000..a9a8996
--- /dev/null
+++ b/ct_optcon/test/lqr/LqrTest.h
@@ -0,0 +1,201 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This file implements a LQR unit tests and a comparison against MATLAB
+ * For more intuitive examples, visit the tutorial.
+ * \example LqrTest.h
+ */
+
+#pragma once
+
+#include <chrono>
+
+#ifdef MATLAB
+#include <matlabCppInterface/Engine.hpp>
+#endif
+
+// Bring in gtest
+#include <gtest/gtest.h>
+
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+TEST(LQRTest, DARETest)
+{
+ const size_t stateDim = 2;
+ const size_t controlDim = 1;
+
+ Eigen::Matrix<double, stateDim, stateDim> A;
+ Eigen::Matrix<double, stateDim, controlDim> B;
+ Eigen::Matrix<double, stateDim, stateDim> Q;
+ Eigen::Matrix<double, controlDim, controlDim> R;
+ Eigen::Matrix<double, controlDim, stateDim> K;
+
+ A << 1, 1, 1, 0;
+ B << 0, 1;
+ Q << 1, 0, 0, 1;
+ R << 1;
+
+ ct::optcon::DARE<stateDim, controlDim> dare;
+ Eigen::Matrix<double, stateDim, stateDim> P = dare.computeSteadyStateRiccatiMatrix(Q, R, A, B, K, true);
+}
+
+
+TEST(LQRTest, quadTest)
+{
+ // std::cout << "QUADROTOR TEST"<<std::endl;
+ // std::cout << "==================================="<<std::endl;
+ // std::cout << "==================================="<<std::endl << std::endl << std::endl;
+
+ const size_t stateDim = 12;
+ const size_t controlDim = 4;
+
+ Eigen::Matrix<double, stateDim, stateDim> A;
+ Eigen::Matrix<double, stateDim, controlDim> B;
+ Eigen::Matrix<double, stateDim, stateDim> Q;
+ Eigen::Matrix<double, controlDim, controlDim> R;
+ Eigen::Matrix<double, controlDim, stateDim> K;
+ Eigen::Matrix<double, controlDim, stateDim> Kiterative;
+
+ ct::optcon::LQR<stateDim, controlDim> lqr;
+
+ Q << 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2000, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0.02;
+
+ R << 100, 0, 0, 0, 0, 1000, 0, 0, 0, 0, 1000, 0, 0, 0, 0, 100;
+
+
+ A << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+ 0, 0, 9.81, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -9.81, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0, -0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, -0, 0, 0, 0, 0, -0, -0, 0, 0, 0, 0, -0, -0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0, 0,
+ 0;
+
+
+ B << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -0, 0, 0, 0, 1.39665, 0, 0,
+ 0, 0, 142.857, -0, 0, 0, 0, 142.857, 0, 0, -0, 0, 83.3333;
+
+ Kiterative.setZero();
+ K.setZero();
+
+
+ bool foundSolutionIterative = lqr.compute(Q, R, A, B, Kiterative, false, true);
+ ASSERT_TRUE(foundSolutionIterative);
+
+#ifdef CT_USE_LAPACK
+ bool foundSolutionDirect = lqr.compute(Q, R, A, B, K, false);
+ ASSERT_TRUE(foundSolutionDirect);
+ ASSERT_LT((K - Kiterative).array().abs().maxCoeff(), 1e-4);
+#endif
+
+ int nTests = 1000;
+#ifdef CT_USE_LAPACK
+ auto start1 = std::chrono::system_clock::now();
+ for (int i = 0; i < nTests; i++)
+ {
+ lqr.compute(Q, R, A, B, K, false);
+ }
+ auto end1 = std::chrono::system_clock::now();
+ auto elapsed1 = std::chrono::duration_cast<std::chrono::milliseconds>(end1 - start1);
+ std::cout << "solved " << nTests << " lqr problems with state dimension " << stateDim << " in " << elapsed1.count()
+ << " ms (average: " << elapsed1.count() / static_cast<double>(nTests) << " ms / lqr)" << std::endl;
+#endif
+
+ auto start2 = std::chrono::system_clock::now();
+ for (int i = 0; i < nTests; i++)
+ {
+ lqr.compute(Q, R, A, B, Kiterative, false, true);
+ }
+ auto end2 = std::chrono::system_clock::now();
+ auto elapsed2 = std::chrono::duration_cast<std::chrono::milliseconds>(end2 - start2);
+ std::cout << "solved " << nTests << " lqr problems iteratively with state dimension " << stateDim << " in "
+ << elapsed2.count() << " ms (average: " << elapsed2.count() / static_cast<double>(nTests) << " ms / lqr)"
+ << std::endl;
+}
+
+#ifdef MATLAB
+TEST(LQRTest, matlabTest)
+{
+ matlab::Engine engine(true);
+ ASSERT_TRUE(engine.good());
+
+ const size_t stateDim = 5;
+ std::string stateDimString = std::to_string(stateDim);
+
+ Eigen::MatrixXd Ad;
+ Eigen::MatrixXd Bd;
+ Eigen::MatrixXd Qd;
+ Eigen::MatrixXd Rd;
+ Eigen::MatrixXd K_Matlab;
+
+ Eigen::Matrix<double, stateDim, stateDim> A;
+ Eigen::Matrix<double, stateDim, stateDim> B;
+ Eigen::Matrix<double, stateDim, stateDim> Q;
+ Eigen::Matrix<double, stateDim, stateDim> R;
+ Eigen::Matrix<double, stateDim, stateDim> K_Cpp;
+ Eigen::Matrix<double, stateDim, stateDim> K_Cpp_iteratively;
+
+ ct::optcon::LQR<stateDim, stateDim> lqr;
+
+ // std::cout << "ARTIFICIAL TEST "<<std::endl;
+ // std::cout << "==================================="<<std::endl;
+ // std::cout << "==================================="<<std::endl << std::endl << std::endl;
+
+ for (int i = 0; i < 10; i++)
+ {
+ std::cout << "Test " << std::to_string(i) << std::endl;
+ std::cout << "===================================" << std::endl;
+
+ std::cout << "1. Generating problem in Matlab" << std::endl;
+ engine.executeCommand("A = magic(" + stateDimString + ");");
+ engine.executeCommand("B = magic(" + stateDimString + ");");
+ engine.executeCommand("Q = diag(100*rand(" + stateDimString + ",1));");
+ engine.executeCommand("R = diag(100*rand(" + stateDimString + ",1));");
+ engine.executeCommand("N = zeros(" + stateDimString + ");");
+
+ std::cout << "2. Computing LQR in Matlab" << std::endl;
+ std::cout << engine.executeCommand("[K,S,E] = lqr(A,B,Q,R,N);");
+
+ std::cout << "3. Obtaining problem from Matlab" << std::endl;
+ engine.get("A", Ad);
+ A = Ad;
+ engine.get("B", Bd);
+ B = Bd;
+ engine.get("Q", Qd);
+ Q = Qd;
+ engine.get("R", Rd);
+ R = Rd;
+
+ std::cout << "4. Obtaining LQR solution from Matlab" << std::endl;
+ engine.get("K", K_Matlab);
+
+ std::cout << "5. Computing LQR solution in C++" << std::endl;
+
+ bool foundSolutionDirect = lqr.compute(Q, R, A, B, K_Cpp, false);
+ ASSERT_TRUE(foundSolutionDirect);
+
+ bool foundSolutionIterative = lqr.compute(Q, R, A, B, K_Cpp_iteratively, false, true);
+ ASSERT_TRUE(foundSolutionIterative);
+
+
+ std::cout << "7. Comparing both solutions" << std::endl;
+ ASSERT_LT((K_Matlab - K_Cpp).array().abs().maxCoeff(), 1e-4);
+ ASSERT_LT((K_Matlab - K_Cpp_iteratively).array().abs().maxCoeff(), 1e-4);
+
+ std::cout << std::endl << std::endl << std::endl;
+ }
+}
+#endif //MATLAB
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/lqr/LqrTestPrespec.cpp b/ct_optcon/test/lqr/LqrTestPrespec.cpp
new file mode 100644
index 0000000..1490ef1
--- /dev/null
+++ b/ct_optcon/test/lqr/LqrTestPrespec.cpp
@@ -0,0 +1,16 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon-prespec.h>
+#include "LqrTest.h"
+
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/mpc/NLOC_MPCTest.cpp b/ct_optcon/test/mpc/NLOC_MPCTest.cpp
new file mode 100644
index 0000000..91dd303
--- /dev/null
+++ b/ct_optcon/test/mpc/NLOC_MPCTest.cpp
@@ -0,0 +1,17 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+
+#include <ct/optcon/optcon.h>
+#include "NLOC_MPCTest.h"
+
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/mpc/NLOC_MPCTest.h b/ct_optcon/test/mpc/NLOC_MPCTest.h
new file mode 100644
index 0000000..2bbccbb
--- /dev/null
+++ b/ct_optcon/test/mpc/NLOC_MPCTest.h
@@ -0,0 +1,357 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+//#define DEBUG_PRINT_MPC
+//#define MATLAB_LOG_MPC
+
+#pragma once
+
+#include <chrono>
+#include <gtest/gtest.h>
+
+#include "mpcTestSettings.h"
+#include "../testSystems/LinearOscillator.h"
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+using std::shared_ptr;
+
+
+/**
+ * Test the MPC forward integrator
+ */
+TEST(MPCTestA, ForwardIntegratorTest)
+{
+ typedef tpl::LinearOscillator<double> LinearOscillator;
+ typedef tpl::LinearOscillatorLinear<double> LinearOscillatorLinear;
+
+ try
+ {
+ // desired final state
+ Eigen::Matrix<double, state_dim, 1> x_final;
+ x_final << 20, 0;
+
+ StateVector<state_dim> x0;
+ x0.setRandom(); // initial state
+ double timeHorizon = 3.0; // final time
+
+ // set up the Optimal Control Problem
+ shared_ptr<ControlledSystem<state_dim, control_dim>> system(new LinearOscillator);
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearOscillatorLinear);
+ shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction =
+ tpl::createCostFunctionLinearOscillator<double>(x_final);
+
+ OptConProblem<state_dim, control_dim> optConProblem(system, costFunction, analyticLinearSystem);
+ optConProblem.setInitialState(x0);
+ optConProblem.setTimeHorizon(timeHorizon);
+
+ // FIRST ILQR INSTANCE FOR CALCULATING THE 'PERFECT' INITIAL GUESS
+ NLOptConSettings nloc_settings;
+ nloc_settings.dt = 0.001;
+ nloc_settings.K_sim = 1; //! required for this test
+ nloc_settings.max_iterations = 100;
+ nloc_settings.integrator = ct::core::IntegrationType::EULER;
+ nloc_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ nloc_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ nloc_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ nloc_settings.closedLoopShooting = true;
+ nloc_settings.printSummary = false;
+
+ // number of steps
+ size_t K = nloc_settings.computeK(timeHorizon);
+
+ // initial controller
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Ones());
+ StateVectorArray<state_dim> x_ref(K + 1, x0);
+ ct::core::StateFeedbackController<state_dim, control_dim> initController(x_ref, u0_ff, u0_fb, nloc_settings.dt);
+
+ NLOptConSolver<state_dim, control_dim> initSolver(optConProblem, nloc_settings);
+ initSolver.configure(nloc_settings);
+ initSolver.setInitialGuess(initController);
+ bool boolInitSuccess = initSolver.solve();
+
+ // obtain the 'perfect' init controller from first iLQR solver
+ ct::core::StateFeedbackController<state_dim, control_dim> perfectInitController = initSolver.getSolution();
+ auto perfectStateTrajectory = initSolver.getStateTrajectory();
+
+ // mpc settings
+ ct::optcon::mpc_settings settings_mpc;
+ settings_mpc.stateForwardIntegration_ = true;
+ settings_mpc.stateForwardIntegratorType_ = nloc_settings.integrator;
+ settings_mpc.stateForwardIntegration_dt_ = nloc_settings.dt;
+ settings_mpc.postTruncation_ = false;
+
+ // MPC instance
+ MPC<NLOptConSolver<state_dim, control_dim>> mpcSolver(optConProblem, nloc_settings, settings_mpc);
+
+ // initialize it with perfect initial guess
+ mpcSolver.setInitialGuess(perfectInitController);
+
+ ct::core::Time t = 0.0; // init time
+
+
+ ct::core::StateFeedbackController<state_dim, control_dim> newPolicy;
+ ct::core::Time ts_newPolicy;
+
+
+ /*!
+ * Run the first MPC cycle, in which the pre-integrator should not be active at all.
+ * after one mpc cycle the solution must still be the same (time horizon unchanged, state unchanged)
+ */
+ mpcSolver.prepareIteration(t);
+ bool success = mpcSolver.finishIteration(x0, t, newPolicy, ts_newPolicy);
+ auto mpcStateTrajectory = newPolicy.getReferenceStateTrajectory();
+
+ ASSERT_EQ(newPolicy.uff().size(), perfectInitController.uff().size());
+ ASSERT_EQ(newPolicy.getFeedforwardTrajectory().duration(),
+ perfectInitController.getFeedforwardTrajectory().duration());
+
+ for (size_t i = 0; i < mpcStateTrajectory.size(); i++)
+ {
+ ASSERT_NEAR(mpcStateTrajectory[i](0), perfectStateTrajectory[i](0), 1e-6); // positions
+ ASSERT_NEAR(mpcStateTrajectory[i](1), perfectStateTrajectory[i](1), 1e-6); // velocities
+ }
+
+
+ /*!
+ * Test the forward integration scheme employing a 'custom controller'.
+ * In this test,
+ * - we select the custom controller to be equal to the previously optimized policy
+ * - we integrate systematically across shifting time intervals
+ * - the integrated solutions need to match the original trajectories with numerical accuracy.
+ *
+ * \warning This test is only reasonnable for dt_sim = 1, and is generally problematic
+ * since the solver's forward integration is implemented step-wise.
+ */
+ std::shared_ptr<ct::core::StateFeedbackController<state_dim, control_dim>> prevController(
+ new ct::core::StateFeedbackController<state_dim, control_dim>(newPolicy));
+ double time_window = 0.2;
+ for (int i = 0; i < mpcStateTrajectory.size() - nloc_settings.computeK(time_window); i++)
+ {
+ double t_forward_start = i * nloc_settings.dt;
+ double t_forward_stop = t_forward_start + time_window;
+
+ mpcStateTrajectory.setInterpolationType(ct::core::InterpolationType::LIN);
+ ct::core::StateVector<state_dim> start_state = mpcStateTrajectory.eval(t_forward_start);
+ ct::core::StateVector<state_dim> ref_end_state = mpcStateTrajectory.eval(t_forward_stop);
+ mpcSolver.doForwardIntegration(t_forward_start, t_forward_stop, start_state, prevController);
+
+ // temporarily commented out, as hard to guarantee this condition is met
+ // ASSERT_LT((start_state-ref_end_state).array().abs().maxCoeff(), 1e-1);
+ }
+
+ } catch (std::exception& e)
+ {
+ std::cout << "caught exception: " << e.what() << std::endl;
+ FAIL();
+ }
+}
+
+
+TEST(MPCTestB, NLOC_MPC_DoublePrecision)
+{
+ typedef tpl::LinearOscillator<double> LinearOscillator;
+ typedef tpl::LinearOscillatorLinear<double> LinearOscillatorLinear;
+
+ for (int solverType = 0; solverType <= 0; solverType++)
+
+ try
+ {
+ Eigen::Vector2d x_final;
+ x_final << 20, 0;
+
+ StateVector<state_dim> x0;
+ x0.setRandom();
+
+ ct::core::Time timeHorizon = 3.0;
+
+ // set up the Optimal Control Problem
+ shared_ptr<ControlledSystem<state_dim, control_dim>> system(new LinearOscillator);
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearOscillatorLinear);
+ shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction =
+ tpl::createCostFunctionLinearOscillator<double>(x_final);
+
+ OptConProblem<state_dim, control_dim> optConProblem(system, costFunction, analyticLinearSystem);
+ optConProblem.setTimeHorizon(timeHorizon);
+ optConProblem.setInitialState(x0);
+
+
+ // FIRST ILQR INSTANCE FOR CALCULATING THE 'PERFECT' INITIAL GUESS
+
+ NLOptConSettings nloc_settings;
+ nloc_settings.dt = 0.01;
+ nloc_settings.K_sim = 1;
+ nloc_settings.K_shot = 1;
+ nloc_settings.max_iterations = 10;
+ nloc_settings.min_cost_improvement = 1e-10; // strict bounds to reach a solution very close to optimality
+ nloc_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ nloc_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ nloc_settings.closedLoopShooting = true;
+ nloc_settings.integrator = ct::core::IntegrationType::EULER;
+ nloc_settings.lineSearchSettings.active = false;
+ nloc_settings.nThreads = 1;
+ nloc_settings.nThreadsEigen = 1;
+ nloc_settings.printSummary = false;
+ nloc_settings.debugPrint = false;
+
+
+ if (solverType == 0)
+ nloc_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ else
+ nloc_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::GNMS;
+
+
+ size_t K = nloc_settings.computeK(timeHorizon); // number of steps
+
+
+ // provide initial controller
+ FeedbackArray<state_dim, control_dim> u0_fb(K, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(K, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref(K + 1, x0);
+ ct::core::StateFeedbackController<state_dim, control_dim> initController(
+ x_ref, u0_ff, u0_fb, nloc_settings.dt);
+
+
+ // solve iLQR and obtain perfect init guess
+ NLOptConSolver<state_dim, control_dim> initSolver(optConProblem, nloc_settings);
+ initSolver.configure(nloc_settings);
+ initSolver.setInitialGuess(initController);
+ initSolver.solve();
+
+ ct::core::StateFeedbackController<state_dim, control_dim> perfectInitController = initSolver.getSolution();
+ ct::core::StateTrajectory<state_dim> perfectStateTrajectory =
+ perfectInitController.getReferenceStateTrajectory();
+
+ // settings for the ilqr instance used in MPC
+ NLOptConSettings nloc_settings_mpc = nloc_settings;
+ nloc_settings_mpc.max_iterations = 1;
+
+ // mpc specific settings
+ ct::optcon::mpc_settings settings;
+ settings.stateForwardIntegration_ = true;
+ settings.stateForwardIntegratorType_ = nloc_settings.integrator;
+ settings.stateForwardIntegration_dt_ = nloc_settings.dt;
+ settings.postTruncation_ = false;
+ settings.measureDelay_ = false;
+ settings.fixedDelayUs_ = 100000; //
+ settings.delayMeasurementMultiplier_ = 1.0;
+ settings.mpc_mode = ct::optcon::MPC_MODE::FIXED_FINAL_TIME;
+ settings.coldStart_ = false;
+ settings.additionalDelayUs_ = 0;
+ settings.useExternalTiming_ = true;
+
+
+ // Create MPC object
+ MPC<NLOptConSolver<state_dim, control_dim>> mpcSolver(optConProblem, nloc_settings_mpc, settings);
+
+ mpcSolver.setInitialGuess(perfectInitController);
+
+ // outputs
+ std::vector<ct::core::StateTrajectory<state_dim>>
+ stateTrajContainer; // collection of all state trajectories
+ ct::core::StateTrajectory<state_dim> tempStateTraj;
+ std::vector<double> timeStamps; // collection of all policy-start timestamps
+ std::vector<ct::core::StateVector<state_dim>> initStates; // collection of all initial tests
+
+ size_t maxNumRuns = 2000;
+ size_t numRuns = 0;
+
+
+ // timestamp of the new optimal policy
+ ct::core::Time ts_newPolicy = 0.0;
+
+ mpcSolver.prepareIteration(0.0);
+
+
+ for (int i = 0; i < maxNumRuns; i++)
+ {
+ // we assume to have a perfect initial state (perfect state evolution)
+ if (i == 1)
+ x0 = tempStateTraj.eval(1e-6 * settings.fixedDelayUs_);
+ else if (i > 0)
+ x0 = tempStateTraj.front();
+
+ // (fake) time which has passed since start of MPC
+ double t = i * 1e-6 * settings.fixedDelayUs_;
+
+ // new optimal policy
+ ct::core::StateFeedbackController<state_dim, control_dim> newPolicy;
+
+ // run one mpc cycle
+ bool success = mpcSolver.finishIteration(x0, t, newPolicy, ts_newPolicy);
+ mpcSolver.prepareIteration(t);
+
+ tempStateTraj = newPolicy.getReferenceStateTrajectory();
+
+ // save trajectories
+ stateTrajContainer.push_back(tempStateTraj);
+ timeStamps.push_back(ts_newPolicy);
+ initStates.push_back(x0);
+
+
+ ct::core::Time relTime = ts_newPolicy;
+ ct::core::StateVector<state_dim> mpcTrajFirstState = tempStateTraj.front();
+ ct::core::StateVector<state_dim> refState = perfectStateTrajectory.eval(relTime);
+
+ // Intuition of this test:
+ // The start of every mpc state trajectory has to be close to the initial "perfect" state trajectory.
+ // we allow for some tolerance, as the optimal trajectories might slightly change with shrinking time horizon
+ ASSERT_LT(std::fabs((refState - mpcTrajFirstState)(0)), 1.0); // max pos deviation
+ ASSERT_LT(std::fabs((refState - mpcTrajFirstState)(1)), 2.0); // max vel deviation
+
+ if (mpcSolver.timeHorizonReached() || !success)
+ break;
+
+ numRuns++;
+ }
+
+
+ mpcSolver.printMpcSummary();
+
+ ASSERT_GT(numRuns, 10); // make sure that MPC runs more than 10 times
+
+
+// The resulting trajectories can be visualized in MATLAB using the script mpc_unittest_plotting.m
+#ifdef MATLAB_LOG_MPC
+#ifdef MATLAB
+ std::cout << "Saving MPC trajectories to Matlab" << std::endl;
+
+ matlab::MatFile matFile;
+ std::string dir = std::string(DATA_DIR_MPC) + "/solution.mat";
+ matFile.open(dir);
+
+ for (size_t i = 0; i < timeStamps.size(); i++)
+ {
+ std::string x_varName = "x_" + std::to_string(i); // state traj
+ std::string t_varName = "t_" + std::to_string(i); // time traj
+ std::string ts_varName = "ts_" + std::to_string(i); // time stamp
+ std::string x0_varName = "x0_" + std::to_string(i); // initial states
+ matFile.put(x_varName, stateTrajContainer[i].getDataArray().toImplementation());
+ matFile.put(t_varName, stateTrajContainer[i].getTimeArray().toEigenTrajectory());
+ matFile.put(ts_varName, timeStamps[i]);
+ }
+
+ matFile.close();
+#endif
+#endif
+
+ } catch (std::exception& e)
+ {
+ std::cout << "caught exception: " << e.what() << std::endl;
+ FAIL();
+ }
+}
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/mpc/NLOC_MPCTest_prespec.cpp b/ct_optcon/test/mpc/NLOC_MPCTest_prespec.cpp
new file mode 100644
index 0000000..ac690cf
--- /dev/null
+++ b/ct_optcon/test/mpc/NLOC_MPCTest_prespec.cpp
@@ -0,0 +1,17 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+
+#include <ct/optcon/optcon-prespec.h>
+#include "NLOC_MPCTest.h"
+
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/mpc/matfiles/mpc_unittest_plotting.m b/ct_optcon/test/mpc/matfiles/mpc_unittest_plotting.m
new file mode 100644
index 0000000..7210b45
--- /dev/null
+++ b/ct_optcon/test/mpc/matfiles/mpc_unittest_plotting.m
@@ -0,0 +1,29 @@
+close all
+clear all
+load 'solution.mat'
+
+
+% number of variables in workspace
+numVars = length(who)/4;
+
+figure(1); hold on;
+figure(2); hold on;
+
+mult = 1;
+
+for i=0:mult:numVars*mult-1
+
+ x_var = eval(genvarname(strcat('x_',num2str(i))));
+ t_var = squeeze(eval(genvarname(strcat('t_',num2str(i)))));
+ ts_var = eval(genvarname(strcat('ts_',num2str(i))));
+
+figure(1);
+plot([ts_var ts_var],[-5 20], ':k');
+plot(ts_var+t_var, x_var(1,:));
+grid on
+
+figure(2);
+plot([ts_var ts_var],[-5 20], ':k');
+plot(ts_var+t_var, x_var(2,:));
+grid on
+end
diff --git a/ct_optcon/test/mpc/mpcTestSettings.h.in b/ct_optcon/test/mpc/mpcTestSettings.h.in
new file mode 100644
index 0000000..9f1d076
--- /dev/null
+++ b/ct_optcon/test/mpc/mpcTestSettings.h.in
@@ -0,0 +1 @@
+#define DATA_DIR_MPC "@MPC_TEST_MAT_DIR@"
diff --git a/ct_optcon/test/nloc/LinearSystemTest.cpp b/ct_optcon/test/nloc/LinearSystemTest.cpp
new file mode 100644
index 0000000..ff16011
--- /dev/null
+++ b/ct_optcon/test/nloc/LinearSystemTest.cpp
@@ -0,0 +1,22 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+#include "LinearSystemTest.h"
+
+/*!
+ * This runs the Linear system unit test.
+ */
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+
+ // ct::optcon::example::singleCore();
+ // ct::optcon::example::multiCore();
+
+ return 1;
+}
diff --git a/ct_optcon/test/nloc/LinearSystemTest.h b/ct_optcon/test/nloc/LinearSystemTest.h
new file mode 100644
index 0000000..10dab6e
--- /dev/null
+++ b/ct_optcon/test/nloc/LinearSystemTest.h
@@ -0,0 +1,183 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+#include <chrono>
+
+#include <gtest/gtest.h>
+
+#include "../testSystems/LinearOscillator.h"
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+using std::shared_ptr;
+
+
+/*!
+ * This unit test considers a variety of different solver/algorithm options for NLOC,
+ * combined with a linear system. We check if the optimization converges within 1 iteration.
+ *
+ * \example LinearSystemTest.h
+ *
+ * \note visit the tutorial for a more intuitive example.
+ *
+ * \warning The HPIPM solver is not included in this unit test.
+ */
+TEST(LinearSystemsTest, NLOCSolverTest)
+{
+ typedef NLOptConSolver<state_dim, control_dim, state_dim / 2, state_dim / 2> NLOptConSolver;
+
+ // count executed tests
+ size_t testCounter = 0;
+
+ // desired final state
+ Eigen::Vector2d x_final;
+ x_final << 20, 0;
+
+ // given initial state
+ StateVector<state_dim> initState;
+ initState.setZero();
+ initState(1) = 1.0;
+
+ // provide algorithm settings
+ NLOptConSettings nloc_settings;
+ nloc_settings.epsilon = 0.0;
+ nloc_settings.recordSmallestEigenvalue = false;
+ nloc_settings.fixedHessianCorrection = false;
+ nloc_settings.dt = 0.01;
+ nloc_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER; // default approximation
+ nloc_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ nloc_settings.printSummary = true; //! this is required to have summary computed and test passed.
+
+ // loop through all solver classes
+ for (int algClass = 0; algClass < NLOptConSettings::NLOCP_ALGORITHM::NUM_TYPES; algClass++)
+ {
+ nloc_settings.nlocp_algorithm = static_cast<NLOptConSettings::NLOCP_ALGORITHM>(algClass);
+
+ // change between closed-loop and open-loop
+ for (int toggleClosedLoop = 0; toggleClosedLoop <= 1; toggleClosedLoop++)
+ {
+ nloc_settings.closedLoopShooting = (bool)toggleClosedLoop;
+
+ // switch line search on or off
+ for (int toggleLS = 0; toggleLS <= 1; toggleLS++)
+ {
+ nloc_settings.lineSearchSettings.active = (bool)toggleLS;
+
+ // toggle between single and multi-threading
+ for (size_t nThreads = 1; nThreads < 5; nThreads = nThreads + 3)
+ {
+ nloc_settings.nThreads = nThreads;
+
+ // toggle between iLQR/GNMS and hybrid methods with K_shot !=1
+ for (size_t kshot = 1; kshot < 11; kshot = kshot + 9)
+ {
+ nloc_settings.K_shot = kshot;
+
+ if (kshot > 1 && nloc_settings.nlocp_algorithm == NLOptConSettings::NLOCP_ALGORITHM::ILQR)
+ continue; // proceed to next test case
+
+ // toggle sensitivity integrator
+ for (size_t sensInt = 0; sensInt <= 1; sensInt++)
+ {
+ nloc_settings.useSensitivityIntegrator = bool(sensInt);
+
+ // toggle over simulation time-steps
+ for (size_t ksim = 1; ksim <= 5; ksim = ksim + 4)
+ {
+ nloc_settings.K_sim = ksim;
+
+ // catch special case, simulation sub-time steps only make sense when sensitivity integrator active
+ if ((nloc_settings.useSensitivityIntegrator == false) && (ksim > 1))
+ continue; // proceed to next test case
+
+ // toggle integrator type
+ for (size_t integratortype = 0; integratortype <= 1; integratortype++)
+ {
+ if (integratortype == 0)
+ nloc_settings.integrator = ct::core::IntegrationType::EULERCT;
+ else if (integratortype == 1 && nloc_settings.useSensitivityIntegrator == true)
+ {
+ // use RK4 with exactly integrated sensitivities
+ nloc_settings.integrator = ct::core::IntegrationType::RK4CT;
+ }
+ else
+ continue; // proceed to next test case
+
+ // nloc_settings.print();
+
+ shared_ptr<ControlledSystem<state_dim, control_dim>> nonlinearSystem(
+ new LinearOscillator());
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(
+ new LinearOscillatorLinear());
+ shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction =
+ tpl::createCostFunctionLinearOscillator<double>(x_final);
+
+ // times
+ ct::core::Time tf = 1.0;
+ size_t nSteps = nloc_settings.computeK(tf);
+
+ // initial controller
+ StateVectorArray<state_dim> x0(nSteps + 1, initState);
+ ControlVector<control_dim> uff;
+ uff << kStiffness * initState(0);
+ ControlVectorArray<control_dim> u0(nSteps, uff);
+
+ FeedbackArray<state_dim, control_dim> u0_fb(
+ nSteps, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(nSteps, ControlVector<control_dim>::Zero());
+
+ NLOptConSolver::Policy_t initController(x0, u0, u0_fb, nloc_settings.dt);
+
+ // construct single-core single subsystem OptCon Problem
+ OptConProblem<state_dim, control_dim> optConProblem(
+ tf, x0[0], nonlinearSystem, costFunction, analyticLinearSystem);
+
+
+ NLOptConSolver solver(optConProblem, nloc_settings);
+ solver.configure(nloc_settings);
+ solver.setInitialGuess(initController);
+
+ //! run two iterations to solve LQ problem
+ solver.runIteration(); // only this one should be required to solve LQ problem
+ solver.runIteration();
+ //! retrieve summary of the optimization
+ const SummaryAllIterations<double>& summary = solver.getBackend()->getSummary();
+ //! check that the policy improved in the first iteration
+ ASSERT_GT(summary.lx_norms.front(), 1e-9);
+ ASSERT_GT(summary.lu_norms.front(), 1e-9);
+
+ //! check that we are converged after the first iteration
+ ASSERT_LT(summary.lx_norms.back(), 1e-10);
+ ASSERT_LT(summary.lu_norms.back(), 1e-10);
+ ASSERT_LT(summary.defect_l1_norms.back(), 1e-10);
+ ASSERT_LT(summary.defect_l2_norms.back(), 1e-10);
+
+ testCounter++;
+
+ } // toggle integrator type
+ } // toggle simulation time steps
+ } // toggle sensitivity integrator
+ } // toggle k_shot
+ } // toggle multi-threading / single-threading
+ } // toggle line-search
+ } // toggle closed-loop
+ } // toggle solver class
+
+ std::cout << "Performed " << testCounter << " successful NLOC tests with linear systems" << std::endl;
+
+} // end TEST
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/nloc/LinearSystemTestPrespec.cpp b/ct_optcon/test/nloc/LinearSystemTestPrespec.cpp
new file mode 100644
index 0000000..6916e4a
--- /dev/null
+++ b/ct_optcon/test/nloc/LinearSystemTestPrespec.cpp
@@ -0,0 +1,24 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#include <ct/optcon/optcon-prespec.h>
+#include "LinearSystemTest.h"
+
+/*!
+ * This runs the GNMS unit test.
+ * \note for a more straight-forward implementation example, visit the tutorial.
+ * \example GNMSCTest.cpp
+ */
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+
+ // ct::optcon::example::singleCore();
+ // ct::optcon::example::multiCore();
+
+ return 1;
+}
diff --git a/ct_optcon/test/nloc/NonlinearSystemTest.cpp b/ct_optcon/test/nloc/NonlinearSystemTest.cpp
new file mode 100644
index 0000000..159e4e6
--- /dev/null
+++ b/ct_optcon/test/nloc/NonlinearSystemTest.cpp
@@ -0,0 +1,14 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+#include "NonlinearSystemTest.h"
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/nloc/NonlinearSystemTest.h b/ct_optcon/test/nloc/NonlinearSystemTest.h
new file mode 100644
index 0000000..ebd912b
--- /dev/null
+++ b/ct_optcon/test/nloc/NonlinearSystemTest.h
@@ -0,0 +1,245 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#include <chrono>
+#include <fenv.h>
+
+#include <gtest/gtest.h>
+
+//#define MATLAB
+//#define MATLAB_FULL_LOG
+
+#include "nloc_test_dir.h"
+
+
+/*!
+ * This file implements a NLOC unit tests.
+ * For more intuitive examples, visit the tutorial.
+ *
+ * \example NonlinearSystemTest.h
+ *
+ *
+ */
+
+/*
+ * This test implements a 1-Dimensional horizontally moving point mass with mass 1kg and attached to a spring
+ * x = [p, pd] // p - position pointing upwards, against gravity, pd - velocity
+ * dx = f(x,u)
+ * = [0 1 x + [0 + [0 u
+ * 0 0] 9.81] 1]
+ */
+namespace ct {
+namespace optcon {
+namespace example {
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+using std::shared_ptr;
+
+const size_t state_dim = 1; // position, velocity
+const size_t control_dim = 1; // force
+
+//! Dynamics class for the GNMS unit test
+class Dynamics : public ControlledSystem<state_dim, control_dim>
+{
+public:
+ Dynamics() : ControlledSystem<state_dim, control_dim>(SYSTEM_TYPE::SECOND_ORDER) {}
+ void computeControlledDynamics(const StateVector<state_dim>& state,
+ const Time& t,
+ const ControlVector<control_dim>& control,
+ StateVector<state_dim>& derivative) override
+ {
+ derivative(0) = (1.0 + state(0)) * state(0) + control(0);
+ }
+
+ Dynamics* clone() const override { return new Dynamics(); };
+};
+
+//! Linear system class for the GNMS unit test
+class LinearizedSystem : public LinearSystem<state_dim, control_dim>
+{
+public:
+ state_matrix_t A_;
+ state_control_matrix_t B_;
+
+
+ const state_matrix_t& getDerivativeState(const StateVector<state_dim>& x,
+ const ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ A_ << 1 + 2 * x(0);
+ return A_;
+ }
+
+ const state_control_matrix_t& getDerivativeControl(const StateVector<state_dim>& x,
+ const ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ B_ << 1;
+ return B_;
+ }
+
+ LinearizedSystem* clone() const override { return new LinearizedSystem(); }
+};
+
+
+TEST(NLOCTest, NonlinearSystemTest)
+{
+ typedef NLOptConSolver<state_dim, control_dim, 1, 0> NLOptConSolver;
+
+ std::cout << "setting up problem " << std::endl;
+
+ std::string configFile = std::string(NLOC_TEST_DIR) + "/config/solver.info";
+ std::string costFunctionFile = std::string(NLOC_TEST_DIR) + "/config/cost.info";
+
+ Eigen::Matrix<double, 1, 1> x_0;
+ ct::core::loadMatrix(costFunctionFile, "x_0", x_0);
+
+ Eigen::Matrix<double, 1, 1> x_f;
+ ct::core::loadMatrix(costFunctionFile, "term1.x_f.weigths.x_des", x_f);
+
+ NLOptConSettings gnms_settings;
+ gnms_settings.load(configFile, true, "gnms");
+
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.load(configFile, true, "ilqr");
+
+ std::shared_ptr<ControlledSystem<state_dim, control_dim>> nonlinearSystem(new Dynamics);
+ std::shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearizedSystem);
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>(costFunctionFile));
+
+ // times
+ ct::core::Time tf = 3.0;
+ ct::core::loadScalar(configFile, "timeHorizon", tf);
+
+ size_t nSteps = gnms_settings.computeK(tf);
+
+ // provide initial guess
+ ControlVector<control_dim> uff_init_guess;
+ uff_init_guess << -(x_0(0) + 1) * x_0(0);
+ ControlVectorArray<control_dim> u0(nSteps, uff_init_guess);
+ StateVectorArray<state_dim> x0(nSteps + 1, x_0);
+
+ int initType = 0;
+ ct::core::loadScalar(configFile, "initType", initType);
+
+ switch (initType)
+ {
+ case 0: // zero
+ break;
+
+ case 1: // linear
+ {
+ for (size_t i = 0; i < nSteps + 1; i++)
+ {
+ x0[i] = x_0 + (x_f - x_0) * double(i) / double(nSteps);
+ }
+ break;
+ }
+ case 2: // integration
+ {
+ shared_ptr<ControlledSystem<state_dim, control_dim>> systemForInit(new Dynamics);
+ ct::core::Integrator<state_dim> integratorForInit(systemForInit, ilqr_settings.integrator);
+ x0[0] = x_0;
+ for (size_t i = 1; i < nSteps + 1; i++)
+ {
+ x0[i] = x0[i - 1];
+ double dt_sim = gnms_settings.getSimulationTimestep();
+ integratorForInit.integrate_n_steps(x0[i], 0, 1, dt_sim);
+ }
+ break;
+ }
+ case 3: // random
+ {
+ for (size_t i = 1; i < nSteps + 1; i++)
+ {
+ x0[i].setRandom();
+ }
+ break;
+ }
+ case 4: // zero
+ {
+ for (size_t i = 1; i < nSteps + 1; i++)
+ {
+ x0[i].setZero();
+ }
+ break;
+ }
+ default:
+ {
+ throw std::runtime_error("illegal init type");
+ break;
+ }
+ }
+
+
+ FeedbackArray<state_dim, control_dim> u0_fb(nSteps, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(nSteps, ControlVector<control_dim>::Zero());
+ NLOptConSolver::Policy_t initController(x0, u0, u0_fb, gnms_settings.dt);
+
+ // construct single-core single subsystem OptCon Problem
+ OptConProblem<state_dim, control_dim> optConProblem1(
+ tf, x0[0], nonlinearSystem, costFunction, analyticLinearSystem);
+ OptConProblem<state_dim, control_dim> optConProblem2(
+ tf, x0[0], nonlinearSystem, costFunction, analyticLinearSystem);
+
+
+ std::cout << "initializing solvers" << std::endl;
+ NLOptConSolver gnms(optConProblem1, gnms_settings);
+ NLOptConSolver ilqr(optConProblem2, ilqr_settings);
+
+
+ gnms.configure(gnms_settings);
+ gnms.setInitialGuess(initController);
+
+ ilqr.configure(ilqr_settings);
+ ilqr.setInitialGuess(initController);
+
+
+ std::cout << "============ running solver 1 ==============" << std::endl;
+
+ int numIterations = 0;
+
+ while (numIterations < gnms_settings.max_iterations)
+ {
+ gnms.runIteration();
+
+ // test trajectories
+ StateTrajectory<state_dim> xRollout = gnms.getStateTrajectory();
+ ControlTrajectory<control_dim> uRollout = gnms.getControlTrajectory();
+
+ numIterations++;
+
+ std::cout << "x final GNMS: " << xRollout.back().transpose() << std::endl;
+ std::cout << "u final GNMS: " << uRollout.back().transpose() << std::endl;
+ }
+
+ gnms.logSummaryToMatlab("gnmsSummary");
+
+ std::cout << "============ running solver 2 ==============" << std::endl;
+
+ numIterations = 0;
+ while (numIterations < ilqr_settings.max_iterations)
+ {
+ ilqr.runIteration();
+
+ // test trajectories
+ StateTrajectory<state_dim> xRollout = ilqr.getStateTrajectory();
+ ControlTrajectory<control_dim> uRollout = ilqr.getControlTrajectory();
+
+ numIterations++;
+
+ std::cout << "x final iLQG: " << xRollout.back().transpose() << std::endl;
+ std::cout << "u final iLQG: " << uRollout.back().transpose() << std::endl;
+ }
+
+ ilqr.logSummaryToMatlab("ilqrSummary");
+}
+}
+}
+}
diff --git a/ct_optcon/test/nloc/NonlinearSystemTestPrespec.cpp b/ct_optcon/test/nloc/NonlinearSystemTestPrespec.cpp
new file mode 100644
index 0000000..9a1dffa
--- /dev/null
+++ b/ct_optcon/test/nloc/NonlinearSystemTestPrespec.cpp
@@ -0,0 +1,15 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#include <ct/optcon/optcon-prespec.h>
+#include "NonlinearSystemTest.h"
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/nloc/SymplecticTest.cpp b/ct_optcon/test/nloc/SymplecticTest.cpp
new file mode 100644
index 0000000..b1a8839
--- /dev/null
+++ b/ct_optcon/test/nloc/SymplecticTest.cpp
@@ -0,0 +1,30 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#include <chrono>
+
+#include <gtest/gtest.h>
+
+#include <ct/optcon/optcon.h>
+
+#include "SymplecticTest.h"
+
+/*!
+ * This runs the GNMS unit test.
+ * \note for a more straight-forward implementation example, visit the tutorial.
+ * \example GNMSCTest.cpp
+ */
+int main(int argc, char** argv)
+{
+ ct::optcon::example::symplecticTest();
+
+ // testing::InitGoogleTest(&argc, argv);
+ // return RUN_ALL_TESTS();
+
+ // ct::optcon::example::multiCore();
+
+ return 1;
+}
diff --git a/ct_optcon/test/nloc/SymplecticTest.h b/ct_optcon/test/nloc/SymplecticTest.h
new file mode 100644
index 0000000..470dccd
--- /dev/null
+++ b/ct_optcon/test/nloc/SymplecticTest.h
@@ -0,0 +1,215 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <chrono>
+
+#include <gtest/gtest.h>
+
+
+/*!
+ * This test implements iLQR and GNMS for a symplectic system.
+ *
+ * \example SymplecticTest.h
+ *
+ * \note visit the tutorial for a more intuitive example.
+ *
+ * \warning The HPIPM solver is not included in this unit test.
+ */
+
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+using std::shared_ptr;
+
+const size_t state_dim = 2; // position, velocity
+const size_t control_dim = 1; // force
+
+const double kStiffness = 1;
+
+//! Dynamics class for the GNMS unit test
+class Dynamics : public SymplecticSystem<1, 1, control_dim>
+{
+public:
+ Dynamics() : SymplecticSystem<1, 1, control_dim>(SYSTEM_TYPE::SECOND_ORDER) {}
+ void computePdot(const StateVector<2>& x,
+ const StateVector<1>& v,
+ const ControlVector<1>& control,
+ StateVector<1>& pDot) override
+ {
+ pDot = v;
+ }
+
+ virtual void computeVdot(const StateVector<2>& x,
+ const StateVector<1>& p,
+ const ControlVector<1>& control,
+ StateVector<1>& vDot) override
+ {
+ vDot(0) = control(0) - kStiffness * p(0) * p(0); // mass is 1 k
+ }
+
+ Dynamics* clone() const override { return new Dynamics(); };
+};
+
+
+//! Linear system class for the GNMS unit test
+class LinearizedSystem : public LinearSystem<state_dim, control_dim>
+{
+public:
+ state_matrix_t A_;
+ state_control_matrix_t B_;
+
+
+ const state_matrix_t& getDerivativeState(const StateVector<state_dim>& x,
+ const ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ A_ << 0, 1, -2 * x(0) * kStiffness, 0;
+ return A_;
+ }
+
+ const state_control_matrix_t& getDerivativeControl(const StateVector<state_dim>& x,
+ const ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ B_ << 0, 1;
+ return B_;
+ }
+
+ LinearizedSystem* clone() const override { return new LinearizedSystem(); };
+};
+
+//! Create a cost function for the GNMS unit test
+std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> createCostFunction(Eigen::Vector2d& x_final)
+{
+ Eigen::Matrix2d Q;
+ Q << 0, 0, 0, 1;
+
+ Eigen::Matrix<double, 1, 1> R;
+ R << 100.0;
+
+ Eigen::Vector2d x_nominal = Eigen::Vector2d::Zero();
+ Eigen::Matrix<double, 1, 1> u_nominal = Eigen::Matrix<double, 1, 1>::Zero();
+
+ Eigen::Matrix2d Q_final;
+ Q_final << 10, 0, 0, 10;
+
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> quadraticCostFunction(
+ new CostFunctionQuadraticSimple<state_dim, control_dim>(Q, R, x_nominal, u_nominal, x_final, Q_final));
+
+ return quadraticCostFunction;
+}
+
+
+void symplecticTest()
+{
+ std::cout << "setting up problem " << std::endl;
+
+ typedef NLOptConSolver<state_dim, control_dim, state_dim / 2, state_dim / 2> NLOptConSolver;
+
+ Eigen::Vector2d x_final;
+ x_final << 2, 0;
+
+ NLOptConSettings gnms_settings;
+ gnms_settings.nThreads = 1;
+ gnms_settings.epsilon = 0.0;
+ gnms_settings.max_iterations = 1;
+ gnms_settings.recordSmallestEigenvalue = false;
+ gnms_settings.min_cost_improvement = 1e-6;
+ gnms_settings.fixedHessianCorrection = false;
+ gnms_settings.dt = 0.05;
+ gnms_settings.K_sim = 50;
+ gnms_settings.K_shot = 1;
+ gnms_settings.integrator = ct::core::IntegrationType::EULER_SYM;
+ // gnms_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ gnms_settings.useSensitivityIntegrator = true;
+ gnms_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::GNMS;
+ gnms_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ gnms_settings.closedLoopShooting = false;
+ gnms_settings.lineSearchSettings.active = false;
+ gnms_settings.loggingPrefix = "GNMS";
+ gnms_settings.printSummary = false;
+
+
+ NLOptConSettings ilqr_settings = gnms_settings;
+ ilqr_settings.closedLoopShooting = true;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.loggingPrefix = "ILQR";
+
+
+ shared_ptr<ControlledSystem<state_dim, control_dim>> nonlinearSystem(new Dynamics);
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearizedSystem);
+ shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction = createCostFunction(x_final);
+
+ // times
+ ct::core::Time tf = 3.0;
+ size_t nSteps = std::round(tf / gnms_settings.dt);
+
+ // provide initial guess
+ StateVector<state_dim> initState;
+ initState.setZero(); //initState(1) = 1.0;
+ StateVectorArray<state_dim> x0(nSteps + 1, initState);
+ ControlVector<control_dim> uff;
+ uff << kStiffness * initState(0) * initState(0);
+ ControlVectorArray<control_dim> u0(nSteps, uff);
+ for (size_t i = 0; i < nSteps + 1; i++)
+ {
+ // x0 [i] = x_final*double(i)/double(nSteps);
+ }
+
+ FeedbackArray<state_dim, control_dim> u0_fb(nSteps, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(nSteps, ControlVector<control_dim>::Zero());
+
+ NLOptConSolver::Policy_t initController(x0, u0, u0_fb, gnms_settings.dt);
+
+ // construct single-core single subsystem OptCon Problem
+ OptConProblem<state_dim, control_dim> optConProblem(tf, x0[0], nonlinearSystem, costFunction, analyticLinearSystem);
+
+
+ // std::cout << "initializing gnms solver" << std::endl;
+ NLOptConSolver gnms(optConProblem, gnms_settings);
+ NLOptConSolver ilqr(optConProblem, ilqr_settings);
+
+
+ gnms.configure(gnms_settings);
+ gnms.setInitialGuess(initController);
+
+ ilqr.configure(ilqr_settings);
+ ilqr.setInitialGuess(initController);
+
+ // std::cout << "running gnms solver" << std::endl;
+
+ bool foundBetter = true;
+ size_t numIterations = 0;
+
+ while (numIterations < 50)
+ {
+ foundBetter = gnms.runIteration();
+ foundBetter = ilqr.runIteration();
+
+ // test trajectories
+ StateTrajectory<state_dim> xRollout = gnms.getStateTrajectory();
+ ControlTrajectory<control_dim> uRollout = gnms.getControlTrajectory();
+
+ // // test trajectories
+ // StateTrajectory<state_dim> xRollout = ilqr.getStateTrajectory();
+ // ControlTrajectory<control_dim> uRollout = ilqr.getControlTrajectory();
+
+ numIterations++;
+
+ // std::cout<<"x final iLQR: " << xRollout.back().transpose() << std::endl;
+ // std::cout<<"u final iLQR: " << uRollout.back().transpose() << std::endl;
+ }
+}
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
diff --git a/ct_optcon/test/nloc/SymplecticTestPrespec.cpp b/ct_optcon/test/nloc/SymplecticTestPrespec.cpp
new file mode 100644
index 0000000..43b2798
--- /dev/null
+++ b/ct_optcon/test/nloc/SymplecticTestPrespec.cpp
@@ -0,0 +1,30 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <chrono>
+
+#include <gtest/gtest.h>
+
+#include <ct/optcon/optcon-prespec.h>
+
+#include "SymplecticTest.h"
+
+/*!
+ * This runs the GNMS unit test.
+ * \note for a more straight-forward implementation example, visit the tutorial.
+ * \example GNMSCTest.cpp
+ */
+int main(int argc, char** argv)
+{
+ ct::optcon::example::symplecticTest();
+
+ // testing::InitGoogleTest(&argc, argv);
+ // return RUN_ALL_TESTS();
+
+ // ct::optcon::example::multiCore();
+
+ return 1;
+}
diff --git a/ct_optcon/test/nloc/config/cost.info b/ct_optcon/test/nloc/config/cost.info
new file mode 100644
index 0000000..06447bc
--- /dev/null
+++ b/ct_optcon/test/nloc/config/cost.info
@@ -0,0 +1,65 @@
+term0
+{
+ name "intermediate cost"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ (0,0) 1.0
+ }
+ R
+ {
+ (0,0) 1.0
+ }
+
+ x_des
+ {
+ (0,0) 0.0
+ }
+ }
+}
+
+
+term1
+{
+ name "final cost"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ (0,0) 10.0
+ }
+
+ x_des
+ {
+ (0,0) 0.0
+ }
+ }
+}
+
+
+
+; initial position
+x_0
+{
+ (0,0) 2.5
+}
+
+
+K_init
+{
+ (0,0) 0
+}
+
+
+
+
+
diff --git a/ct_optcon/test/nloc/config/gnmsPlot.m b/ct_optcon/test/nloc/config/gnmsPlot.m
new file mode 100644
index 0000000..93249d7
--- /dev/null
+++ b/ct_optcon/test/nloc/config/gnmsPlot.m
@@ -0,0 +1,38 @@
+clear all
+close all
+load GNMSLog1.mat
+
+%reformat
+t = squeeze(t);
+lv = squeeze(lv);
+
+figure()
+subplot(2,2,1)
+plot(t, x(1,:), 'k', 'MarkerSize',1)
+hold on;
+%plot(t, xShot(1,:), 'b', 'MarkerSize',1)
+legend('x*');
+xlabel('t [sec]')
+ylabel('x [m]')
+title('position');
+
+subplot(2,2,3)
+plot(t(1:end-1), lv)
+title('control update')
+xlabel('t [sec]')
+ylabel('F [N]')
+
+subplot(2,2,4)
+plot(t, lx(1,:)); hold on
+title('state update')
+xlabel('t [sec]')
+ylabel('dx [m]')
+legend('x update')
+
+
+%%
+figure();
+plot(t(1:end-1), d(1,:)); hold on;
+%plot(t(1:end-1), d(2,:))
+title('defect')
+
diff --git a/ct_optcon/test/nloc/config/jointPlotILQRandGNMS.m b/ct_optcon/test/nloc/config/jointPlotILQRandGNMS.m
new file mode 100644
index 0000000..677bc81
--- /dev/null
+++ b/ct_optcon/test/nloc/config/jointPlotILQRandGNMS.m
@@ -0,0 +1,42 @@
+clear all
+close all
+
+
+numIter = 19; % number of iterations to be plotted
+
+jointNr = 1;
+
+markerSize = 7;
+
+%% plot the init guess logs
+
+plotInitGuesses('GNMSLogInit.mat', ':k', 'x', markerSize, jointNr, true);
+plotInitGuesses('ILQRLogInit.mat', ':r', 'o', markerSize, jointNr, true);
+
+
+%% plot the iterations
+for iter= 0:1:numIter
+
+ % plot the current iteration
+ plotIteration(strcat(strcat('ILQRLog', num2str(iter)),'.mat'), iter, 'k', 'x', markerSize, jointNr, true);
+ plotIteration(strcat(strcat('ILQR_GNMS_5Log', num2str(iter)),'.mat'), iter, '--r', 'o', markerSize,jointNr, true);
+
+end
+
+%%
+subplot(3,3,9)
+hold on;
+plot(-1, 0.1, 'ro', 'MarkerSize', markerSize); % move x-coordinate out of axis limits
+plot(-1,-0.1, 'kx', 'MarkerSize', markerSize); % move x-coordinate out of axis limits
+ylim([-1 1])
+xlim([-0.1, 1])
+set(gca,'XTick',[])
+set(gca,'YTick',[])
+set(gca,'XTickLabel','')
+set(gca,'YTickLabel','')
+title('Legend')
+legend('GNMS', 'ILQR')
+legend boxoff
+
+
+
diff --git a/ct_optcon/test/nloc/config/plotInitGuesses.m b/ct_optcon/test/nloc/config/plotInitGuesses.m
new file mode 100644
index 0000000..b53639b
--- /dev/null
+++ b/ct_optcon/test/nloc/config/plotInitGuesses.m
@@ -0,0 +1,72 @@
+function [] = plotInitGuesses(fileName, color, markerType, markerSize, jointNr, plotDefect)
+
+load(fileName);
+
+t = linspace(0, (K)*dt, K+1);
+
+figure(1)
+subplot(3,3,1)
+plot(t, x(jointNr,:), color, 'MarkerSize',markerSize); hold on;
+xlabel('t [sec]')
+ylabel('x')
+title('position (1-dim)');
+axis tight
+
+subplot(3,3,2)
+plot(t(1:end-1), u_ff(jointNr,:), color, 'MarkerSize',markerSize); hold on;
+xlabel('t [sec]')
+ylabel('u')
+title('control input (1-dim)');
+axis tight
+
+subplot(3,3,3)
+plot(-1, cost, strcat(color,markerType), 'MarkerSize', markerSize);
+hold on;
+grid on;
+title('cost');
+xlabel('iteration')
+ylabel('cost')
+axis tight
+
+subplot(3,3,4, 'YScale', 'log');
+hold on;
+grid on;
+title('lx norm')
+xlabel('iteration')
+axis tight
+
+subplot(3,3,5, 'YScale', 'log')
+hold on; grid on;
+title('lu norm')
+xlabel('iteration')
+axis tight
+
+subplot(3,3,6)
+hold on
+title('stepsize')
+xlabel('iteration')
+axis tight
+
+if plotDefect
+subplot(3,3,7)
+plot(-1, d_norm, strcat(color,markerType), 'MarkerSize', markerSize);
+hold on; grid on
+title('total defect')
+xlabel('iter')
+ylabel('|d|')
+axis tight
+end
+
+subplot(3,3,8)
+title('defect norms')
+xlabel('t [sec]')
+ylabel('|d(t)|')
+d_sq = squeeze(d);
+normVec = sqrt(sum(d_sq.^2,1));
+plot(t, normVec, color);
+axis tight
+hold on
+
+
+end
+
diff --git a/ct_optcon/test/nloc/config/plotIteration.m b/ct_optcon/test/nloc/config/plotIteration.m
new file mode 100644
index 0000000..313ebf2
--- /dev/null
+++ b/ct_optcon/test/nloc/config/plotIteration.m
@@ -0,0 +1,51 @@
+function [] = plotIteration( fileName , iter, color, markerType, markerSize, jointNr, plotDefect)
+
+% load the current iteration
+load(fileName);
+
+t = linspace(0, (K)*dt, K+1);
+
+figure(1)
+subplot(3,3,1)
+plot(t, x(jointNr,:), color,'MarkerSize',1);
+%legend('init', '1', '2', '3');
+
+
+subplot(3,3,2)
+plot(t(1:end-1), u_ff(jointNr,:), color, 'MarkerSize',1); hold on;
+% legend('init', '1', '2', '3');
+
+subplot(3,3,3)
+plot(iter, cost, strcat(color,markerType), 'MarkerSize', markerSize);
+
+subplot(3,3,4)
+plot(iter, lx_norm, strcat(color,markerType), 'MarkerSize', markerSize);
+
+subplot(3,3,5)
+plot(iter, lu_norm, strcat(color,markerType), 'MarkerSize', markerSize);
+
+subplot(3,3,6)
+plot(iter, alphaStep, strcat(color,markerType), 'MarkerSize', markerSize); hold on
+%legend('1', '2', '3');
+
+if plotDefect
+ subplot(3,3,7, 'YScale', 'log')
+ plot(iter, d_norm, strcat(color,markerType), 'MarkerSize', markerSize); hold on
+ % legend('1', '2', '3');
+
+ subplot(3,3,8)
+ d_sq = squeeze(d);
+ temp = size(d_sq);
+ if(temp(2)>temp(1))
+ normVec = sqrt(sum(d_sq.^2,1));
+ else
+ normVec = sqrt(sum(d_sq.^2,2));
+ end
+ plot(t, normVec, color);
+ hold on;
+ %legend('1', '2', '3');
+
+end %plot defect
+
+end
+
diff --git a/ct_optcon/test/nloc/config/plotSolverSummary.m b/ct_optcon/test/nloc/config/plotSolverSummary.m
new file mode 100644
index 0000000..dbd3b95
--- /dev/null
+++ b/ct_optcon/test/nloc/config/plotSolverSummary.m
@@ -0,0 +1,55 @@
+function [] = plotSolverSummary(fileName, color, markerType, markerSize, plotDefect)
+
+load(fileName);
+
+figure(1)
+
+subplot(2,3,1)
+plot(iterations, totalCosts, strcat(color,markerType), 'MarkerSize', markerSize);
+hold on;
+grid on;
+title('cost');
+xlabel('iteration')
+ylabel('cost')
+axis tight
+
+%%
+subplot(2,3,2, 'YScale', 'log');
+plot(iterations, lx_norms, strcat(color,markerType), 'MarkerSize', markerSize);
+hold on;
+grid on;
+title('lx norm')
+xlabel('iteration')
+axis tight
+
+%%
+
+subplot(2,3,3, 'YScale', 'log')
+plot(iterations, lu_norms, strcat(color,markerType), 'MarkerSize', markerSize);
+hold on; grid on;
+title('lu norm')
+xlabel('iteration')
+axis tight
+
+%%
+
+subplot(2,3,4)
+plot(iterations, stepSizes, strcat(color,markerType), 'MarkerSize', markerSize);
+hold on
+title('stepsize')
+xlabel('iteration')
+axis tight
+
+%%
+subplot(2,3,5)
+if(plotDefect)
+plot(iterations, defect_l1_norms, strcat(color,markerType), 'MarkerSize', markerSize);
+hold on; grid on
+title('total defect')
+xlabel('iter')
+ylabel('|d|')
+axis tight
+end
+
+end
+
diff --git a/ct_optcon/test/nloc/config/plotSummary.m b/ct_optcon/test/nloc/config/plotSummary.m
new file mode 100644
index 0000000..7846020
--- /dev/null
+++ b/ct_optcon/test/nloc/config/plotSummary.m
@@ -0,0 +1,13 @@
+clear all
+close all
+
+color = 'b';
+markerType = 'o';
+markerSize = 5;
+
+plotSolverSummary('ilqrSummary.mat', ':k', 'x', markerSize, true);
+plotSolverSummary('gnmsSummary.mat', 'r', 'o', markerSize, true);
+
+
+
+
diff --git a/ct_optcon/test/nloc/config/sequentialPlotGNMS.m b/ct_optcon/test/nloc/config/sequentialPlotGNMS.m
new file mode 100644
index 0000000..f574be2
--- /dev/null
+++ b/ct_optcon/test/nloc/config/sequentialPlotGNMS.m
@@ -0,0 +1,100 @@
+clear all
+close all
+
+
+numIter = 5; % number of iterations to be plotted
+
+%% plot the init guess logs
+load GNMSLogInit.mat
+
+t = linspace(0, (K+2)*dt, K+1);
+
+figure(1)
+subplot(3,3,1)
+plot(t, x(1,:), 'k', 'MarkerSize',1); hold on;
+xlabel('t [sec]')
+ylabel('x [m]')
+title('position');
+
+subplot(3,3,2)
+plot(t(1:end-1), u_ff(1,:), 'k', 'MarkerSize',1); hold on;
+xlabel('t [sec]')
+title('control');
+
+subplot(3,3,3)
+plot(-1, d_norm, 'o', 'MarkerSize', 8); hold on
+title('defect norm')
+xlabel('t [sec]')
+ylabel('d [m]')
+
+subplot(3,3,4)
+hold on
+title('state update')
+xlabel('t [sec]')
+ylabel('dx [m]')
+
+subplot(3,3,5)
+hold on;
+title('control update')
+xlabel('t [sec]')
+ylabel('F [N]')
+
+subplot(3,3,6)
+plot(-1, cost, 'o'); hold on;
+title('cost')
+xlabel('iteration')
+ylabel('cost')
+
+subplot(3,3,7, 'XScale', 'log', 'YScale', 'log');
+hold on; grid on;
+title('lx norm')
+xlabel('iteration')
+
+subplot(3,3,8, 'XScale', 'log', 'YScale', 'log')
+hold on; grid on;
+title('lu norm')
+xlabel('iteration')
+
+%% plot the iterations
+for iter= 0:1:numIter
+
+ % load the current iteration
+ load(strcat(strcat('GNMSLog', num2str(iter)),'.mat'));
+
+ t=squeeze(t);
+
+ figure(1)
+ subplot(3,3,1)
+ plot(t, x(1,:), 'MarkerSize',1); hold on;
+ %legend('init', '1', '2', '3');
+
+ subplot(3,3,2)
+ plot(t(1:end-1), u_ff(1,:), 'MarkerSize',1); hold on;
+ % legend('init', '1', '2', '3');
+
+ subplot(3,3,3)
+ plot(iter, d_norm, 'o', 'MarkerSize', 8); hold on
+ % legend('1', '2', '3');
+
+ subplot(3,3,4)
+ plot(t, squeeze(lx(1,:))); hold on
+ %legend('1', '2', '3');
+
+ subplot(3,3,5)
+ plot(t(1:end-1), squeeze(luff)); hold on;
+ %legend('1', '2', '3');
+
+ subplot(3,3,6)
+ plot(iter, cost, 'o', 'MarkerSize', 8); hold on;
+
+ subplot(3,3,7, 'XScale', 'log', 'YScale', 'log')
+ plot(iter, lx_norm, 'o', 'MarkerSize', 8);
+
+ subplot(3,3,8)
+ plot(iter, lu_norm, 'o', 'MarkerSize', 8);
+end
+
+
+
+
+
diff --git a/ct_optcon/test/nloc/config/sequentialPlotILQR.m b/ct_optcon/test/nloc/config/sequentialPlotILQR.m
new file mode 100644
index 0000000..d900972
--- /dev/null
+++ b/ct_optcon/test/nloc/config/sequentialPlotILQR.m
@@ -0,0 +1,100 @@
+clear all
+%close all
+
+
+numIter = 4; % number of iterations to be plotted
+
+%% plot the init guess logs
+load ILQRLogInit.mat
+
+t = linspace(0, (K+2)*dt, K+1);
+
+figure(2)
+subplot(3,3,1)
+plot(t, x(1,:), 'k', 'MarkerSize',1); hold on;
+xlabel('t [sec]')
+ylabel('x [m]')
+title('position');
+
+subplot(3,3,2)
+plot(t(1:end-1), u_ff(1,:), 'k', 'MarkerSize',1); hold on;
+xlabel('t [sec]')
+title('control');
+
+subplot(3,3,3)
+plot(-1, d_norm, 'o', 'MarkerSize', 8); hold on
+title('defect norm')
+xlabel('t [sec]')
+ylabel('d [m]')
+
+subplot(3,3,4)
+hold on
+title('state update')
+xlabel('t [sec]')
+ylabel('dx [m]')
+
+subplot(3,3,5)
+hold on;
+title('control update')
+xlabel('t [sec]')
+ylabel('F [N]')
+
+subplot(3,3,6)
+plot(-1, cost, 'o'); hold on;
+title('cost')
+xlabel('iteration')
+ylabel('cost')
+
+subplot(3,3,7, 'XScale', 'log', 'YScale', 'log');
+hold on; grid on;
+title('lx norm')
+xlabel('iteration')
+
+subplot(3,3,8, 'XScale', 'log', 'YScale', 'log')
+hold on; grid on;
+title('lu norm')
+xlabel('iteration')
+
+%% plot the iterations
+for iter= 0:1:numIter
+
+ % load the current iteration
+ load(strcat(strcat('ILQRLog', num2str(iter)),'.mat'));
+
+ t=squeeze(t);
+
+ figure(2)
+ subplot(3,3,1)
+ plot(t, x(1,:), 'MarkerSize',1); hold on;
+ %legend('init', '1', '2', '3');
+
+ subplot(3,3,2)
+ plot(t(1:end-1), u_ff(1,:), 'MarkerSize',1); hold on;
+ % legend('init', '1', '2', '3');
+
+ subplot(3,3,3)
+ plot(iter, d_norm, 'o', 'MarkerSize', 8); hold on
+ % legend('1', '2', '3');
+
+ subplot(3,3,4)
+ plot(t, squeeze(lx(1,:))); hold on
+ %legend('1', '2', '3');
+
+ subplot(3,3,5)
+ plot(t(1:end-1), squeeze(luff)); hold on;
+ %legend('1', '2', '3');
+
+ subplot(3,3,6)
+ plot(iter, cost, 'o', 'MarkerSize', 8); hold on;
+
+ subplot(3,3,7, 'XScale', 'log', 'YScale', 'log')
+ plot(iter, lx_norm, 'o', 'MarkerSize', 8);
+
+ subplot(3,3,8)
+ plot(iter, lu_norm, 'o', 'MarkerSize', 8);
+end
+
+
+
+
+
diff --git a/ct_optcon/test/nloc/config/solver.info b/ct_optcon/test/nloc/config/solver.info
new file mode 100644
index 0000000..a3d29da
--- /dev/null
+++ b/ct_optcon/test/nloc/config/solver.info
@@ -0,0 +1,75 @@
+timeHorizon 3.0
+
+initType 0 ; 0 = zero, 1 = linear, 2 = integrated with zero control action, 3 = random
+
+gnms
+{
+ integrator EulerCT
+ useSensitivityIntegrator true
+ discretization Forward_euler
+ closedLoopShooting 1 ; (1 = closed loop shooting, 0 = open-loop shooting)
+ dt 0.01
+ K_shot 1
+ K_sim 10
+ epsilon 1e-6
+ max_iterations 10
+ fixedHessianCorrection false
+ recordSmallestEigenvalue false
+ min_cost_improvement 1e-12
+ maxDefectSum 1e-5
+ meritFunctionRho 1.0
+ nThreads 1
+ nThreadsEigen 4
+ ;locp_solver HPIPM_SOLVER
+ locp_solver GNRICCATI_SOLVER
+ nlocp_algorithm ILQR
+ debugPrint false
+ printSummary false
+ loggingPrefix ILQR
+
+ line_search
+ {
+ active false ;
+ adaptive false ;
+ maxIterations 10 ;
+ alpha_0 1.0 ;
+ n_alpha 0.5 ;
+ debugPrint false
+ }
+}
+
+
+ilqr
+{
+ integrator EulerCT
+ useSensitivityIntegrator true
+ discretization Forward_euler
+ closedLoopShooting 1 ; (1 = closed loop shooting, 0 = open-loop shooting)
+ dt 0.01
+ K_shot 60
+ K_sim 10
+ epsilon 1e-6
+ max_iterations 10
+ fixedHessianCorrection false
+ recordSmallestEigenvalue false
+ min_cost_improvement 1e-12
+ maxDefectSum 1e-5
+ nThreads 1
+ nThreadsEigen 4
+ ;locp_solver HPIPM_SOLVER
+ locp_solver GNRICCATI_SOLVER
+ nlocp_algorithm GNMS
+ debugPrint false
+ printSummary false
+ loggingPrefix ILQR_GNMS_5
+
+ line_search
+ {
+ active false;
+ adaptive false ;
+ maxIterations 10 ;
+ alpha_0 1.0 ;
+ n_alpha 0.5 ;
+ debugPrint false
+ }
+}
\ No newline at end of file
diff --git a/ct_optcon/test/nloc/iLQRTest.cpp b/ct_optcon/test/nloc/iLQRTest.cpp
new file mode 100644
index 0000000..f567d37
--- /dev/null
+++ b/ct_optcon/test/nloc/iLQRTest.cpp
@@ -0,0 +1,611 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#include <chrono>
+
+// Bring in gtest
+#include <gtest/gtest.h>
+
+#include <ct/optcon/optcon.h>
+
+#include "../testSystems/LinearOscillator.h"
+
+
+/* This test implements a 1-Dimensional horizontally moving point mass with mass 1kg and attached to a spring
+ x = [p, pd] // p - position pointing upwards, against gravity, pd - velocity
+ dx = f(x,u)
+ = [0 1 x + [0 + [0 u
+ 0 0] 9.81] 1]
+
+ */
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+using std::shared_ptr;
+
+
+TEST(ILQRTest, SystemLinearizationTest)
+{
+ shared_ptr<ControlledSystem<state_dim, control_dim>> nonlinearSystem(new LinearOscillator());
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearOscillatorLinear());
+ shared_ptr<LinearSystem<state_dim, control_dim>> numDiffLinearModelGeneral(
+ new SystemLinearizer<state_dim, control_dim>(nonlinearSystem, false));
+ shared_ptr<LinearSystem<state_dim, control_dim>> numDiffLinearModelSecondOrder(
+ new SystemLinearizer<state_dim, control_dim>(nonlinearSystem, true));
+ StateVector<state_dim> xRef;
+ ControlVector<control_dim> u;
+ Time t = 0;
+
+ size_t nTests = 100;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ xRef.setRandom();
+
+ // we have to set u in a way that it makes x and equilibrium
+ u(0) = kStiffness * xRef(0);
+
+ StateVector<state_dim> dxNonlinear;
+ nonlinearSystem->computeControlledDynamics(xRef, t, u, dxNonlinear);
+
+ StateVector<state_dim> dxLinear;
+ analyticLinearSystem->computeControlledDynamics(xRef, t, u, dxLinear);
+
+ StateVector<state_dim> dxLinearNumDiffGeneral;
+ numDiffLinearModelGeneral->computeControlledDynamics(xRef, t, u, dxLinearNumDiffGeneral);
+
+ StateVector<state_dim> dxLinearNumDiffSecondOrder;
+ numDiffLinearModelGeneral->computeControlledDynamics(xRef, t, u, dxLinearNumDiffSecondOrder);
+
+ ASSERT_LT((dxNonlinear - dxLinear).array().abs().maxCoeff(), 1e-6);
+
+ ASSERT_LT((dxNonlinear - dxLinearNumDiffGeneral).array().abs().maxCoeff(), 1e-6);
+
+ ASSERT_LT((dxNonlinear - dxLinearNumDiffSecondOrder).array().abs().maxCoeff(), 1e-6);
+ }
+}
+
+
+TEST(ILQRTestA, InstancesComparison)
+{
+ try
+ {
+ typedef NLOptConSolver<state_dim, control_dim> NLOptConSolver;
+
+ std::cout << "setting up problem " << std::endl;
+
+ Eigen::Vector2d x_final;
+ x_final << 20, 0;
+
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.001;
+ ilqr_settings.K_shot = 1;
+ ilqr_settings.K_sim = 1;
+ ilqr_settings.epsilon = 0.0;
+ ilqr_settings.nThreads = 1;
+ ilqr_settings.max_iterations = 5;
+ ilqr_settings.recordSmallestEigenvalue = false;
+ ilqr_settings.fixedHessianCorrection = false;
+ ilqr_settings.min_cost_improvement = 1e-12;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ ilqr_settings.closedLoopShooting = true;
+ ilqr_settings.integrator = ct::core::IntegrationType::EULER;
+ ilqr_settings.printSummary = false;
+ ilqr_settings.debugPrint = false;
+ ilqr_settings.lineSearchSettings.debugPrint = false;
+
+
+ // copy settings for MP case, but change number of threads
+ NLOptConSettings ilqr_settings_mp = ilqr_settings;
+ ilqr_settings_mp.nThreads = 4;
+
+ shared_ptr<ControlledSystem<state_dim, control_dim>> nonlinearSystem(new LinearOscillator());
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearOscillatorLinear());
+ shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction =
+ tpl::createCostFunctionLinearOscillator<double>(x_final);
+
+ // times
+ ct::core::Time tf = 3.0;
+
+ // init state
+ StateVector<state_dim> x0;
+ x0.setRandom();
+
+ // construct single-core single subsystem OptCon Problem
+ OptConProblem<state_dim, control_dim> optConProblem(
+ tf, x0, nonlinearSystem, costFunction, analyticLinearSystem);
+
+ size_t nSteps = std::round(tf / ilqr_settings.dt);
+
+ std::cout << "initializing ilqr solver" << std::endl;
+ NLOptConSolver ilqr(optConProblem, ilqr_settings);
+ NLOptConSolver ilqr_mp(optConProblem, ilqr_settings_mp);
+
+ NLOptConSolver ilqr_comp(optConProblem, ilqr_settings);
+ NLOptConSolver ilqr_mp_comp(optConProblem, ilqr_settings_mp);
+
+ // provide initial controller
+ FeedbackArray<state_dim, control_dim> u0_fb(nSteps, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(nSteps, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref(nSteps + 1, StateVector<state_dim>::Zero());
+ NLOptConSolver::Policy_t initController(x_ref, u0_ff, u0_fb, ilqr_settings.dt);
+
+ ilqr.configure(ilqr_settings);
+ ilqr.setInitialGuess(initController);
+ //! check that if retrieving solution now, we exactly get back the init guess.
+ NLOptConSolver::Policy_t mirroredInitguess = ilqr.getSolution();
+ for (size_t i = 0; i < initController.uff().size(); i++)
+ {
+ ASSERT_NEAR(mirroredInitguess.uff()[i](0), initController.uff()[i](0), 1e-3);
+ ASSERT_NEAR(mirroredInitguess.K()[i](0), initController.K()[i](0), 1e-3);
+ ASSERT_NEAR(mirroredInitguess.x_ref()[i](0), initController.x_ref()[i](0), 1e-3);
+ }
+
+ ilqr.solve();
+
+ std::cout << "now going into tests" << std::endl;
+
+
+ size_t nTests = 2;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ bool foundBetter;
+
+ if (i == 0)
+ {
+ std::cout << "Turning Line-Search off" << std::endl;
+ ilqr_settings.lineSearchSettings.active = false;
+ ilqr_settings_mp.lineSearchSettings.active = false;
+ }
+ else
+ {
+ std::cout << "Turning Line-Search on" << std::endl;
+ ilqr_settings.lineSearchSettings.active = true;
+ ilqr_settings_mp.lineSearchSettings.active = true;
+ }
+
+ ilqr.configure(ilqr_settings);
+ ilqr_mp.configure(ilqr_settings_mp);
+
+ ilqr.setInitialGuess(initController);
+ ilqr_mp.setInitialGuess(initController);
+
+ ilqr.solve();
+ ilqr_mp.solve();
+
+ // make sure this is really the optimal policy and we get a 'false' as return.
+ NLOptConSolver::Policy_t optimalPolicy = ilqr.getSolution();
+ NLOptConSolver::Policy_t optimalPolicy_mp = ilqr_mp.getSolution();
+
+
+ // now check against the comparison iLQG instances
+ ilqr_comp.configure(ilqr_settings);
+ ilqr_mp_comp.configure(ilqr_settings_mp);
+
+ ilqr_comp.setInitialGuess(optimalPolicy);
+ ilqr_mp_comp.setInitialGuess(optimalPolicy_mp);
+
+ ilqr_comp.solve();
+ ilqr_mp_comp.solve();
+
+ NLOptConSolver::Policy_t optimalPolicy_comp = ilqr_comp.getSolution();
+ NLOptConSolver::Policy_t optimalPolicy_mp_comp = ilqr_mp_comp.getSolution();
+
+
+ // compare controller sizes
+ ASSERT_EQ(optimalPolicy_comp.uff().size(), nSteps);
+
+ ASSERT_EQ(optimalPolicy_comp.uff().size(), optimalPolicy.uff().size());
+
+ // compare controller durations
+ ASSERT_EQ(optimalPolicy_comp.getFeedforwardTrajectory().duration(),
+ optimalPolicy.getFeedforwardTrajectory().duration());
+
+
+ // compare controllers for single core and mp case
+ for (size_t i = 0; i < optimalPolicy_comp.uff().size() - 1; i++)
+ {
+ ASSERT_NEAR(optimalPolicy_comp.uff()[i](0), optimalPolicy.uff()[i](0), 1e-3);
+
+ ASSERT_NEAR(optimalPolicy_comp.K()[i].array().abs().maxCoeff(),
+ optimalPolicy.K()[i].array().abs().maxCoeff(), 1e-3);
+
+ ASSERT_NEAR(optimalPolicy_mp_comp.uff()[i](0), optimalPolicy_mp.uff()[i](0), 1e-3);
+
+ ASSERT_NEAR(optimalPolicy_mp_comp.K()[i].array().abs().maxCoeff(),
+ optimalPolicy_mp.K()[i].array().abs().maxCoeff(), 1e-3);
+ }
+ }
+
+ } catch (std::exception& e)
+ {
+ std::cout << "caught exception: " << e.what() << std::endl;
+ FAIL();
+ }
+}
+
+
+TEST(ILQRTestB, SingleCoreTest)
+{
+ try
+ {
+ typedef NLOptConSolver<state_dim, control_dim> NLOptConSolver;
+ typedef StateMatrix<state_dim> state_matrix_t;
+ typedef StateControlMatrix<state_dim, control_dim> state_control_matrix_t;
+
+ std::cout << "setting up problem " << std::endl;
+
+ Eigen::Vector2d x_final;
+ x_final << 20, 0;
+
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.epsilon = 0.0;
+ ilqr_settings.nThreads = 1;
+ ilqr_settings.max_iterations = 50;
+ ilqr_settings.recordSmallestEigenvalue = false;
+ ilqr_settings.fixedHessianCorrection = false;
+ ilqr_settings.min_cost_improvement = 1e-6;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ ilqr_settings.closedLoopShooting = true;
+ ilqr_settings.integrator = ct::core::IntegrationType::RK4;
+ ilqr_settings.printSummary = false;
+
+
+ // copy settings for MP case, but change number of threads
+ NLOptConSettings ilqr_settings_mp = ilqr_settings;
+ ilqr_settings_mp.nThreads = 4;
+
+ shared_ptr<ControlledSystem<state_dim, control_dim>> nonlinearSystem(new LinearOscillator());
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearOscillatorLinear());
+ shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction =
+ tpl::createCostFunctionLinearOscillator<double>(x_final);
+
+ // times
+ ct::core::Time tf = 3.0;
+
+ // init state
+ StateVector<state_dim> x0;
+ x0.setRandom();
+
+ // construct single-core single subsystem OptCon Problem
+ OptConProblem<state_dim, control_dim> optConProblem(
+ tf, x0, nonlinearSystem, costFunction, analyticLinearSystem);
+
+ size_t nSteps = std::round(tf / ilqr_settings.dt);
+
+ std::cout << "initializing ilqr solver" << std::endl;
+ NLOptConSolver ilqr(optConProblem, ilqr_settings);
+ NLOptConSolver ilqr_mp(optConProblem, ilqr_settings_mp);
+
+
+ // provide initial controller
+ FeedbackArray<state_dim, control_dim> u0_fb(nSteps, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(nSteps, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref(nSteps + 1, StateVector<state_dim>::Zero());
+ NLOptConSolver::Policy_t initController(x_ref, u0_ff, u0_fb, ilqr_settings.dt);
+
+ ilqr.configure(ilqr_settings);
+ ilqr.setInitialGuess(initController);
+
+ bool foundBetter = true;
+
+ while (foundBetter)
+ foundBetter = ilqr.runIteration();
+
+
+ size_t nTests = 4;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ if (i == 0)
+ {
+ ilqr_settings.lineSearchSettings.active = false;
+ ilqr_settings_mp.lineSearchSettings.active = false;
+ }
+ else
+ {
+ ilqr_settings.lineSearchSettings.active = true;
+ ilqr_settings_mp.lineSearchSettings.active = true;
+ }
+
+ if (i < 2)
+ {
+ ilqr_settings.fixedHessianCorrection = false;
+ ilqr_settings_mp.fixedHessianCorrection = false;
+ }
+ else
+ {
+ ilqr_settings.fixedHessianCorrection = true;
+ ilqr_settings_mp.fixedHessianCorrection = true;
+ }
+
+ ilqr.configure(ilqr_settings);
+ ilqr_mp.configure(ilqr_settings_mp);
+
+ ilqr.setInitialGuess(initController);
+ ilqr_mp.setInitialGuess(initController);
+
+ bool foundBetter = true;
+ bool foundBetter_mp = true;
+ size_t numIterations = 0;
+
+ while (foundBetter)
+ {
+ // solve
+
+ foundBetter = ilqr.runIteration();
+ foundBetter_mp = ilqr_mp.runIteration();
+
+ // test trajectories
+
+ StateTrajectory<state_dim> xRollout = ilqr.getStateTrajectory();
+ ControlTrajectory<control_dim> uRollout = ilqr.getControlTrajectory();
+
+ StateTrajectory<state_dim> xRollout_mp = ilqr_mp.getStateTrajectory();
+ ControlTrajectory<control_dim> uRollout_mp = ilqr_mp.getControlTrajectory();
+
+ ASSERT_EQ(xRollout.size(), nSteps + 1);
+ ASSERT_EQ(uRollout.size(), nSteps);
+
+ ASSERT_EQ(xRollout_mp.size(), nSteps + 1);
+ ASSERT_EQ(uRollout_mp.size(), nSteps);
+
+
+ // test linearization
+
+ core::StateMatrixArray<state_dim> A;
+ core::StateControlMatrixArray<state_dim, control_dim> B;
+ ilqr.getBackend()->retrieveLastLinearizedModel(A, B);
+
+ core::StateMatrixArray<state_dim> A_mp;
+ core::StateControlMatrixArray<state_dim, control_dim> B_mp;
+ ilqr_mp.getBackend()->retrieveLastLinearizedModel(A_mp, B_mp);
+
+ ASSERT_EQ(A.size(), nSteps);
+ ASSERT_EQ(B.size(), nSteps);
+
+ ASSERT_EQ(A_mp.size(), nSteps);
+ ASSERT_EQ(B_mp.size(), nSteps);
+
+ // check integration
+ for (size_t j = 0; j < xRollout.size() - 1; j++)
+ {
+ ASSERT_LT((xRollout[j] - xRollout_mp[j]).array().abs().maxCoeff(), 1e-10);
+ ASSERT_LT((uRollout[j] - uRollout_mp[j]).array().abs().maxCoeff(), 1e-10);
+ }
+
+ // check linearization
+ for (size_t j = 0; j < xRollout.size() - 1; j++)
+ {
+ state_matrix_t A_analytic;
+ state_control_matrix_t B_analytic;
+
+ if (ilqr_settings.discretization == NLOptConSettings::APPROXIMATION::FORWARD_EULER)
+ {
+ A_analytic =
+ state_matrix_t::Identity() +
+ ilqr_settings.dt * analyticLinearSystem->getDerivativeState(xRollout[j], uRollout[j], 0);
+ B_analytic =
+ ilqr_settings.dt * analyticLinearSystem->getDerivativeControl(xRollout[j], uRollout[j], 0);
+ }
+ else if (ilqr_settings.discretization == NLOptConSettings::APPROXIMATION::BACKWARD_EULER)
+ {
+ state_matrix_t aNew =
+ ilqr_settings.dt * analyticLinearSystem->getDerivativeState(xRollout[j], uRollout[j], 0);
+ state_matrix_t aNewInv = (state_matrix_t::Identity() - aNew).colPivHouseholderQr().inverse();
+ A_analytic = aNewInv;
+ B_analytic = aNewInv * ilqr_settings.dt *
+ analyticLinearSystem->getDerivativeControl(xRollout[j], uRollout[j], 0);
+ }
+ else if (ilqr_settings.discretization == NLOptConSettings::APPROXIMATION::TUSTIN)
+ {
+ state_matrix_t aNew = 0.5 * ilqr_settings.dt *
+ analyticLinearSystem->getDerivativeState(xRollout[j], uRollout[j], 0);
+ state_matrix_t aNewInv = (state_matrix_t::Identity() - aNew).colPivHouseholderQr().inverse();
+ A_analytic = aNewInv * (state_matrix_t::Identity() + aNew);
+ B_analytic = aNewInv * ilqr_settings.dt *
+ analyticLinearSystem->getDerivativeControl(xRollout[j], uRollout[j], 0);
+ }
+
+ ASSERT_LT((A[j] - A_analytic).array().abs().maxCoeff(), 1e-6);
+ ASSERT_LT((A_mp[j] - A_analytic).array().abs().maxCoeff(), 1e-6);
+ ASSERT_LT((A_mp[j] - A[j]).array().abs().maxCoeff(), 1e-12);
+
+ ASSERT_LT((B[j] - B_analytic).array().abs().maxCoeff(), 1e-6);
+ ASSERT_LT((B_mp[j] - B_analytic).array().abs().maxCoeff(), 1e-6);
+ ASSERT_LT((B_mp[j] - B[j]).array().abs().maxCoeff(), 1e-12);
+ }
+
+ ASSERT_EQ(foundBetter, foundBetter_mp);
+
+ numIterations++;
+
+ //note: since this is a linear system, it should actually converge in only 1 iteration.
+
+ ASSERT_LT(numIterations, 3);
+ }
+ }
+
+ } catch (std::exception& e)
+ {
+ std::cout << "caught exception: " << e.what() << std::endl;
+ FAIL();
+ }
+}
+
+
+TEST(ILQRTestC, PolicyComparison)
+{
+ typedef NLOptConSolver<state_dim, control_dim> NLOptConSolver;
+
+
+ try
+ {
+ std::cout << "setting up problem " << std::endl;
+
+ Eigen::Vector2d x_final;
+ x_final << 20, 0;
+
+ NLOptConSettings ilqr_settings;
+ ilqr_settings.epsilon = 0.0;
+ ilqr_settings.nThreads = 1;
+ ilqr_settings.max_iterations = 50;
+ ilqr_settings.recordSmallestEigenvalue = true;
+ ilqr_settings.min_cost_improvement = 1e-6;
+ ilqr_settings.discretization = NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ ilqr_settings.nlocp_algorithm = NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ilqr_settings.lqocp_solver = NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ ilqr_settings.integrator = ct::core::IntegrationType::EULER;
+ ilqr_settings.closedLoopShooting = true;
+ ilqr_settings.fixedHessianCorrection = false;
+ ilqr_settings.printSummary = false;
+
+
+ NLOptConSettings ilqr_settings_mp = ilqr_settings;
+ ilqr_settings_mp.nThreads = 4;
+
+
+ shared_ptr<ControlledSystem<state_dim, control_dim>> nonlinearSystem(new LinearOscillator());
+ shared_ptr<LinearSystem<state_dim, control_dim>> analyticLinearSystem(new LinearOscillatorLinear());
+ shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction =
+ tpl::createCostFunctionLinearOscillator<double>(x_final);
+
+ // times
+ ct::core::Time tf = 3.0;
+
+ // init state
+ StateVector<state_dim> x0;
+ x0.setRandom();
+
+ // construct single-core single subsystem OptCon Problem
+ OptConProblem<state_dim, control_dim> optConProblem(
+ tf, x0, nonlinearSystem, costFunction, analyticLinearSystem);
+
+ size_t nSteps = std::round(tf / ilqr_settings.dt);
+
+ std::cout << "initializing ilqr solver" << std::endl;
+ NLOptConSolver ilqr(optConProblem, ilqr_settings);
+ NLOptConSolver ilqr_mp(optConProblem, ilqr_settings_mp);
+
+
+ // provide initial controller
+ FeedbackArray<state_dim, control_dim> u0_fb(nSteps, FeedbackMatrix<state_dim, control_dim>::Zero());
+ ControlVectorArray<control_dim> u0_ff(nSteps, ControlVector<control_dim>::Zero());
+ StateVectorArray<state_dim> x_ref(nSteps + 1, StateVector<state_dim>::Zero());
+ NLOptConSolver::Policy_t initController(x_ref, u0_ff, u0_fb, ilqr_settings.dt);
+
+ ilqr.configure(ilqr_settings);
+ ilqr.setInitialGuess(initController);
+ ilqr.solve();
+
+ ilqr_mp.configure(ilqr_settings_mp);
+ ilqr_mp.setInitialGuess(initController);
+ ilqr_mp.solve();
+
+ size_t nTests = 2;
+ for (size_t i = 0; i < nTests; i++)
+ {
+ if (i == 0)
+ {
+ ilqr_settings.lineSearchSettings.active = false;
+ ilqr_settings_mp.lineSearchSettings.active = false;
+ }
+ else
+ {
+ ilqr_settings.lineSearchSettings.active = true;
+ ilqr_settings_mp.lineSearchSettings.active = true;
+ }
+
+ ilqr.configure(ilqr_settings);
+ ilqr_mp.configure(ilqr_settings_mp);
+
+ ilqr.setInitialGuess(initController);
+ ilqr_mp.setInitialGuess(initController);
+
+ bool foundBetter = true;
+ bool foundBetter_mp = true;
+ size_t numIterations = 0;
+
+
+ while (foundBetter)
+ {
+ // solve
+ foundBetter = ilqr.runIteration();
+ foundBetter_mp = ilqr_mp.runIteration();
+ return;
+
+ numIterations++;
+
+ // we should converge in way less than 10 iterations
+ ASSERT_LT(numIterations, 10);
+ }
+
+
+ // test trajectories
+ StateTrajectory<state_dim> xRollout = ilqr.getStateTrajectory();
+ StateTrajectory<state_dim> xRollout_mp = ilqr_mp.getStateTrajectory();
+
+ // the optimal controller
+ std::shared_ptr<NLOptConSolver::Policy_t> optController(new NLOptConSolver::Policy_t(ilqr.getSolution()));
+ std::shared_ptr<NLOptConSolver::Policy_t> optController_mp(
+ new NLOptConSolver::Policy_t(ilqr_mp.getSolution()));
+
+ // two test systems
+ std::shared_ptr<ControlledSystem<state_dim, control_dim>> testSystem1(new LinearOscillator());
+ std::shared_ptr<ControlledSystem<state_dim, control_dim>> testSystem2(new LinearOscillator());
+
+ // set the controller
+ testSystem1->setController(optController);
+ testSystem2->setController(optController_mp);
+
+ // test integrators, the same as in iLQG
+ ct::core::Integrator<state_dim> testIntegrator1(testSystem1, ct::core::IntegrationType::RK4);
+ ct::core::Integrator<state_dim> testIntegrator2(testSystem2, ct::core::IntegrationType::RK4);
+
+ // states
+ ct::core::StateVector<state_dim> x_test_1 = x0;
+ ct::core::StateVector<state_dim> x_test_2 = x0;
+
+ // do forward integration
+ double dt_sim = ilqr_settings.getSimulationTimestep();
+ testIntegrator1.integrate_n_steps(x_test_1, 0.0, nSteps, dt_sim);
+ testIntegrator2.integrate_n_steps(x_test_2, 0.0, nSteps, dt_sim);
+
+ ASSERT_LT((x_test_1 - xRollout.back()).array().abs().maxCoeff(), 0.3);
+ ASSERT_LT((x_test_2 - xRollout_mp.back()).array().abs().maxCoeff(), 0.3);
+ }
+
+ } catch (std::exception& e)
+ {
+ std::cout << "caught exception: " << e.what() << std::endl;
+ FAIL();
+ }
+}
+
+
+} // namespace example
+} // namespace optcon
+} // namespace ct
+
+
+/*!
+ * This runs the iLQG unit test.
+ * \note for a more straight-forward implementation example, visit the tutorial.
+ * \example iLQGCTest.cpp
+ */
+int main(int argc, char** argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/nloc/nloc_test_dir.h.in b/ct_optcon/test/nloc/nloc_test_dir.h.in
new file mode 100644
index 0000000..567e8dc
--- /dev/null
+++ b/ct_optcon/test/nloc/nloc_test_dir.h.in
@@ -0,0 +1 @@
+#define NLOC_TEST_DIR "@NLOC_TEST_DIR@"
diff --git a/ct_optcon/test/solver/linear/ConstrainedLQOCSolverTest.cpp b/ct_optcon/test/solver/linear/ConstrainedLQOCSolverTest.cpp
new file mode 100644
index 0000000..4e44ea9
--- /dev/null
+++ b/ct_optcon/test/solver/linear/ConstrainedLQOCSolverTest.cpp
@@ -0,0 +1,17 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <gtest/gtest.h>
+#include <ct/optcon/optcon.h>
+
+#include "ConstrainedLQOCSolverTest.h"
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/solver/linear/ConstrainedLQOCSolverTest.h b/ct_optcon/test/solver/linear/ConstrainedLQOCSolverTest.h
new file mode 100644
index 0000000..13ff0a0
--- /dev/null
+++ b/ct_optcon/test/solver/linear/ConstrainedLQOCSolverTest.h
@@ -0,0 +1,689 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+using namespace ct;
+using namespace ct::core;
+using namespace ct::optcon;
+
+#include "../../testSystems/LinkedMasses.h"
+#include "../../testSystems/LinearOscillator.h"
+
+bool verbose = false; // optional verbose output
+
+template <size_t state_dim, size_t control_dim>
+void printSolution(const ct::core::StateVectorArray<state_dim>& x,
+ const ct::core::ControlVectorArray<control_dim>& u,
+ const ct::core::FeedbackArray<state_dim, control_dim>& K)
+{
+ std::cout << "x array:" << std::endl;
+ for (size_t j = 0; j < x.size(); j++)
+ std::cout << x[j].transpose() << std::endl;
+
+ std::cout << "u array:" << std::endl;
+ for (size_t j = 0; j < u.size(); j++)
+ std::cout << u[j].transpose() << std::endl;
+
+ std::cout << "K array:" << std::endl;
+ for (size_t j = 0; j < K.size(); j++)
+ std::cout << K[j] << std::endl << std::endl;
+}
+
+//! check that control bounds are respected
+template <size_t state_dim, size_t control_dim>
+void assertControlBounds(const ct::core::ControlVectorArray<control_dim>& u,
+ const Eigen::VectorXi sparsity,
+ const Eigen::VectorXd u_lb,
+ const Eigen::VectorXd u_ub)
+{
+ for (size_t j = 0; j < u.size(); j++)
+ {
+ for (size_t n = 0; n < sparsity.rows(); n++)
+ {
+ ASSERT_GE(u[j](sparsity(n)), u_lb(n));
+ ASSERT_LE(u[j](sparsity(n)), u_ub(n));
+ }
+ }
+}
+
+//! check that state bounds are respected
+template <size_t state_dim, size_t control_dim>
+void assertStateBounds(const ct::core::StateVectorArray<state_dim>& x,
+ const Eigen::VectorXi sparsity,
+ const Eigen::VectorXd x_lb,
+ const Eigen::VectorXd x_ub)
+{
+ for (size_t j = 0; j < x.size(); j++)
+ {
+ for (size_t n = 0; n < sparsity.rows(); n++)
+ {
+ ASSERT_GE(x[j](sparsity(n) - control_dim), x_lb(n));
+ ASSERT_LE(x[j](sparsity(n) - control_dim), x_ub(n));
+ }
+ }
+}
+
+template <size_t state_dim, size_t control_dim, typename LINEAR_SYSTEM>
+void boxConstraintsTest(ct::core::ControlVector<control_dim> u0,
+ ct::core::StateVector<state_dim> x0,
+ ct::core::StateVector<state_dim> xf,
+ int nb_u,
+ Eigen::VectorXd u_lb,
+ Eigen::VectorXd u_ub,
+ Eigen::VectorXi u_box_sparsity,
+ int nb_x,
+ Eigen::VectorXd x_lb,
+ Eigen::VectorXd x_ub,
+ Eigen::VectorXi x_box_sparsity,
+ Eigen::VectorXi x_box_sparsity_terminal)
+{
+ const size_t N = 5;
+ const double dt = 0.5;
+
+ // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> hpipmSolver(new HPIPMInterface<state_dim, control_dim>);
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> gnRiccatiSolver(new GNRiccatiSolver<state_dim, control_dim>);
+
+ NLOptConSettings nloc_settings;
+ nloc_settings.lqoc_solver_settings.num_lqoc_iterations = 50; // allow 50 iterations
+ if (verbose)
+ nloc_settings.lqoc_solver_settings.lqoc_debug_print = true;
+ hpipmSolver->configure(nloc_settings);
+
+ // create linear-quadratic optimal control problem containers
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem1(new LQOCProblem<state_dim, control_dim>(N));
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem2(new LQOCProblem<state_dim, control_dim>(N));
+
+
+ // create a continuous-time example system and discretize it
+ std::shared_ptr<core::LinearSystem<state_dim, control_dim>> exampleSystem(new LINEAR_SYSTEM());
+ core::SensitivityApproximation<state_dim, control_dim> discreteExampleSystem(
+ dt, exampleSystem, core::SensitivityApproximationSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
+
+
+ // define cost function matrices
+ StateMatrix<state_dim> Q;
+ Q.setIdentity();
+ Q *= 2.0;
+ ControlMatrix<control_dim> R;
+ R.setIdentity();
+ R *= 2 * 2.0;
+
+ // create a cost function
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionQuadraticSimple<state_dim, control_dim>(Q, R, xf, u0, xf, Q));
+
+ // solution variables needed later
+ ct::core::StateVectorArray<state_dim> xSol_hpipm;
+ ct::core::ControlVectorArray<control_dim> uSol_hpipm;
+ ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm;
+
+ if (verbose)
+ {
+ std::cout << " ================================================== " << std::endl;
+ std::cout << " TEST CASE 1: FULL BOX CONSTRAINTS ON CONTROL INPUT " << std::endl;
+ std::cout << " ================================================== " << std::endl;
+ }
+
+ // initialize the optimal control problems for both solvers
+ lqocProblem1->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+ lqocProblem2->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+
+ lqocProblem1->setIntermediateBoxConstraints(nb_u, u_lb, u_ub, u_box_sparsity);
+ lqocProblem2->setIntermediateBoxConstraints(nb_u, u_lb, u_ub, u_box_sparsity);
+
+ // check that constraint configuration is right
+ ASSERT_TRUE(lqocProblem1->isConstrained());
+ ASSERT_FALSE(lqocProblem1->isGeneralConstrained());
+ ASSERT_TRUE(lqocProblem1->isBoxConstrained());
+
+ // set and try to solve the problem for both solvers
+ hpipmSolver->configureBoxConstraints(lqocProblem1);
+ hpipmSolver->setProblem(lqocProblem1);
+ hpipmSolver->initializeAndAllocate();
+ hpipmSolver->solve();
+
+ try
+ {
+ gnRiccatiSolver->setProblem(lqocProblem2);
+ gnRiccatiSolver->solve();
+ ASSERT_TRUE(false); // should never reach to this point
+ } catch (std::exception& e)
+ {
+ std::cout << "GNRiccatiSolver failed with exception " << e.what() << std::endl;
+ ASSERT_TRUE(true);
+ }
+
+ // retrieve solutions from hpipm
+ xSol_hpipm = hpipmSolver->getSolutionState();
+ uSol_hpipm = hpipmSolver->getSolutionControl();
+ hpipmSolver->getFeedback(KSol_hpipm);
+
+
+ if (verbose)
+ printSolution<state_dim, control_dim>(xSol_hpipm, uSol_hpipm, KSol_hpipm);
+
+ // assert that the imposed boundary constraints are respected by the solution
+ assertControlBounds<state_dim, control_dim>(uSol_hpipm, u_box_sparsity, u_lb, u_ub);
+
+
+ if (verbose)
+ {
+ std::cout << " ================================================== " << std::endl;
+ std::cout << " TEST CASE 2: FULL BOX CONSTRAINTS ON STATE VECTOR " << std::endl;
+ std::cout << " ================================================== " << std::endl;
+ }
+
+ // initialize the optimal control problems for both solvers
+ lqocProblem1->setZero();
+ lqocProblem2->setZero();
+ lqocProblem1->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+ lqocProblem2->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+
+ lqocProblem1->setIntermediateBoxConstraints(nb_x, x_lb, x_ub, x_box_sparsity);
+ lqocProblem2->setIntermediateBoxConstraints(nb_x, x_lb, x_ub, x_box_sparsity);
+ lqocProblem1->setTerminalBoxConstraints(nb_x, x_lb, x_ub, x_box_sparsity_terminal);
+ lqocProblem2->setTerminalBoxConstraints(nb_x, x_lb, x_ub, x_box_sparsity_terminal);
+
+ // check that constraint configuration is right
+ ASSERT_TRUE(lqocProblem1->isConstrained());
+ ASSERT_FALSE(lqocProblem1->isGeneralConstrained());
+ ASSERT_TRUE(lqocProblem1->isBoxConstrained());
+
+ // set and try to solve the problem for both solvers
+ hpipmSolver->configureBoxConstraints(lqocProblem1);
+ hpipmSolver->setProblem(lqocProblem1);
+ hpipmSolver->initializeAndAllocate();
+ hpipmSolver->solve();
+
+ try
+ {
+ gnRiccatiSolver->setProblem(lqocProblem2);
+ gnRiccatiSolver->solve();
+ ASSERT_TRUE(false); // should never reach to this point
+ } catch (std::exception& e)
+ {
+ std::cout << "GNRiccatiSolver failed with exception " << e.what() << std::endl;
+ ASSERT_TRUE(true);
+ }
+
+ // retrieve solutions from hpipm
+ xSol_hpipm = hpipmSolver->getSolutionState();
+ uSol_hpipm = hpipmSolver->getSolutionControl();
+ hpipmSolver->getFeedback(KSol_hpipm);
+
+ if (verbose)
+ printSolution<state_dim, control_dim>(xSol_hpipm, uSol_hpipm, KSol_hpipm);
+
+ // assert that the imposed boundary constraints are respected by the solution
+ assertStateBounds<state_dim, control_dim>(xSol_hpipm, x_box_sparsity, x_lb, x_ub);
+
+
+ if (verbose)
+ {
+ std::cout << " ================================================== " << std::endl;
+ std::cout << " TEST CASE 3: BOX CONSTRAINTS ON STATE AND CONTROL " << std::endl;
+ std::cout << " ================================================== " << std::endl;
+ }
+
+ // initialize the optimal control problems for both solvers
+ lqocProblem1->setZero();
+ lqocProblem2->setZero();
+ lqocProblem1->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+ lqocProblem2->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+
+ // relax box constraints a bit for this test, otherwise there might be no solution
+ x_lb.array() -= 1.0;
+ x_ub.array() += 1.0;
+
+ // combine the box constraints manually for this test
+ int nb_ux = nb_x + nb_u;
+ Eigen::VectorXd ux_lb(nb_ux);
+ ux_lb << u_lb, x_lb;
+ Eigen::VectorXd ux_ub(nb_ux);
+ ux_ub << u_ub, x_ub;
+ Eigen::VectorXi ux_box_sparsity(nb_ux);
+ ux_box_sparsity << u_box_sparsity, x_box_sparsity;
+
+ // set the combined box constraints
+ lqocProblem1->setIntermediateBoxConstraints(nb_ux, ux_lb, ux_ub, ux_box_sparsity);
+ lqocProblem2->setIntermediateBoxConstraints(nb_ux, ux_lb, ux_ub, ux_box_sparsity);
+ lqocProblem1->setTerminalBoxConstraints(nb_x, x_lb, x_ub, x_box_sparsity_terminal);
+ lqocProblem2->setTerminalBoxConstraints(nb_x, x_lb, x_ub, x_box_sparsity_terminal);
+
+ // check that constraint configuration is right
+ ASSERT_TRUE(lqocProblem1->isConstrained());
+ ASSERT_FALSE(lqocProblem1->isGeneralConstrained());
+ ASSERT_TRUE(lqocProblem1->isBoxConstrained());
+
+ // set and try to solve the problem for both solvers
+ hpipmSolver->configureBoxConstraints(lqocProblem1);
+ hpipmSolver->setProblem(lqocProblem1);
+ hpipmSolver->initializeAndAllocate();
+ hpipmSolver->solve();
+
+ try
+ {
+ gnRiccatiSolver->setProblem(lqocProblem2);
+ gnRiccatiSolver->solve();
+ ASSERT_TRUE(false); // should never reach to this point
+ } catch (std::exception& e)
+ {
+ std::cout << "GNRiccatiSolver failed with exception " << e.what() << std::endl;
+ ASSERT_TRUE(true);
+ }
+
+ // retrieve solutions from hpipm
+ xSol_hpipm = hpipmSolver->getSolutionState();
+ uSol_hpipm = hpipmSolver->getSolutionControl();
+ hpipmSolver->getFeedback(KSol_hpipm);
+
+ if (verbose)
+ printSolution<state_dim, control_dim>(xSol_hpipm, uSol_hpipm, KSol_hpipm);
+
+ // assert that the imposed boundary constraints are respected by the solution
+ assertControlBounds<state_dim, control_dim>(uSol_hpipm, u_box_sparsity, u_lb, u_ub);
+ assertStateBounds<state_dim, control_dim>(xSol_hpipm, x_box_sparsity, x_lb, x_ub);
+}
+
+
+template <size_t state_dim, size_t control_dim, typename LINEAR_SYSTEM>
+void generalConstraintsTest(ct::core::ControlVector<control_dim> u0,
+ ct::core::StateVector<state_dim> x0,
+ ct::core::StateVector<state_dim> xf,
+ Eigen::VectorXd d_lb,
+ Eigen::VectorXd d_ub,
+ Eigen::MatrixXd C,
+ Eigen::MatrixXd D)
+{
+ const size_t N = 5;
+ const double dt = 0.5;
+
+ // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> hpipmSolver(new HPIPMInterface<state_dim, control_dim>);
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> gnRiccatiSolver(new GNRiccatiSolver<state_dim, control_dim>);
+
+ NLOptConSettings nloc_settings;
+ nloc_settings.lqoc_solver_settings.num_lqoc_iterations = 50; // allow 50 iterations
+ hpipmSolver->configure(nloc_settings);
+
+ // create linear-quadratic optimal control problem containers
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem1(new LQOCProblem<state_dim, control_dim>(N));
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem2(new LQOCProblem<state_dim, control_dim>(N));
+
+ // create a continuous-time example system and discretize it
+ std::shared_ptr<core::LinearSystem<state_dim, control_dim>> exampleSystem(new LINEAR_SYSTEM());
+ core::SensitivityApproximation<state_dim, control_dim> discreteExampleSystem(
+ dt, exampleSystem, core::SensitivityApproximationSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
+
+
+ // define cost function matrices
+ StateMatrix<state_dim> Q;
+ Q.setIdentity();
+ Q *= 2.0;
+ ControlMatrix<control_dim> R;
+ R.setIdentity();
+ R *= 2 * 2.0;
+
+ // create a cost function
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionQuadraticSimple<state_dim, control_dim>(Q, R, xf, u0, xf, Q));
+
+ // solution variables needed later
+ ct::core::StateVectorArray<state_dim> xSol_hpipm;
+ ct::core::ControlVectorArray<control_dim> uSol_hpipm;
+ ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm;
+
+ if (verbose)
+ {
+ std::cout << " ================================================== " << std::endl;
+ std::cout << " TEST CASE 1: GENERAL INEQUALITY CONSTRAINTS " << std::endl;
+ std::cout << " ================================================== " << std::endl;
+ }
+
+ // initialize the optimal control problems for both solvers
+ lqocProblem1->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+ lqocProblem2->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+ lqocProblem1->setGeneralConstraints(d_lb, d_ub, C, D);
+ lqocProblem2->setGeneralConstraints(d_lb, d_ub, C, D);
+
+ // check that constraint configuration is right
+ ASSERT_TRUE(lqocProblem1->isConstrained());
+ ASSERT_TRUE(lqocProblem1->isGeneralConstrained());
+ ASSERT_FALSE(lqocProblem1->isBoxConstrained());
+
+ // set and try to solve the problem for both solvers
+ hpipmSolver->configureBoxConstraints(lqocProblem1);
+ hpipmSolver->setProblem(lqocProblem1);
+ hpipmSolver->initializeAndAllocate();
+ hpipmSolver->solve();
+
+ try
+ {
+ gnRiccatiSolver->setProblem(lqocProblem2);
+ gnRiccatiSolver->solve();
+ ASSERT_TRUE(false); // should never reach to this point
+ } catch (std::exception& e)
+ {
+ std::cout << "GNRiccatiSolver failed with exception " << e.what() << std::endl;
+ ASSERT_TRUE(true);
+ }
+
+ // retrieve solutions from hpipm
+ xSol_hpipm = hpipmSolver->getSolutionState();
+ uSol_hpipm = hpipmSolver->getSolutionControl();
+ hpipmSolver->getFeedback(KSol_hpipm);
+
+
+ if (verbose)
+ printSolution<state_dim, control_dim>(xSol_hpipm, uSol_hpipm, KSol_hpipm);
+}
+
+TEST(ConstrainedLQOCSolverTest, BoxConstrTest_small)
+{
+ const size_t state_dim = 2;
+ const size_t control_dim = 1;
+
+ // nominal control
+ ct::core::ControlVector<control_dim> u0;
+ u0.setConstant(0);
+
+ // initial state
+ ct::core::StateVector<state_dim> x0;
+ x0 << 2.5, 0.0;
+
+ // desired final state
+ ct::core::StateVector<state_dim> xf;
+ xf.setConstant(0.0);
+
+ // dense control box constraints
+ int nb_u = control_dim;
+ Eigen::VectorXd u_lb(nb_u);
+ Eigen::VectorXd u_ub(nb_u);
+ u_lb.setConstant(-0.5);
+ u_ub.setConstant(0.5);
+ Eigen::VectorXi u_box_sparsity(nb_u);
+ u_box_sparsity << 0;
+
+ // sparse state box constraints
+ int nb_x = 1;
+ Eigen::VectorXd x_lb(nb_x);
+ Eigen::VectorXd x_ub(nb_x);
+ x_lb << 1.7;
+ x_ub.setConstant(std::numeric_limits<double>::max());
+ Eigen::VectorXi x_box_sparsity(nb_x);
+ x_box_sparsity << 1;
+ Eigen::VectorXi x_box_sparsity_terminal(nb_x);
+ x_box_sparsity_terminal << 0;
+
+ boxConstraintsTest<state_dim, control_dim, example::LinearOscillatorLinear>(
+ u0, x0, xf, nb_u, u_lb, u_ub, u_box_sparsity, nb_x, x_lb, x_ub, x_box_sparsity, x_box_sparsity_terminal);
+}
+
+TEST(ConstrainedLQOCSolverTest, BoxConstrTest_medium)
+{
+ const size_t state_dim = 8;
+ const size_t control_dim = 3;
+
+ // nominal control
+ ct::core::ControlVector<control_dim> u0;
+ u0.setConstant(0.1);
+
+ // initial state
+ ct::core::StateVector<state_dim> x0;
+ x0 << 2.5, 2.5, 0, 0, 0, 0, 0, 0;
+
+ // desired final state
+ ct::core::StateVector<state_dim> xf;
+ xf.setConstant(0.1);
+
+ // dense control box constraints
+ int nb_u = control_dim;
+ Eigen::VectorXd u_lb(nb_u);
+ Eigen::VectorXd u_ub(nb_u);
+ u_lb.setConstant(-0.5);
+ u_ub.setConstant(0.5);
+ Eigen::VectorXi u_box_sparsity(nb_u);
+ u_box_sparsity << 0, 1, 2;
+
+ // sparse state box constraints
+ int nb_x = 2;
+ Eigen::VectorXd x_lb(nb_x);
+ Eigen::VectorXd x_ub(nb_x);
+ x_lb << 1.5, 1.5;
+ x_ub.setConstant(std::numeric_limits<double>::max());
+ Eigen::VectorXi x_box_sparsity(nb_x);
+ x_box_sparsity << 3, 4;
+ Eigen::VectorXi x_box_sparsity_terminal(nb_x);
+ x_box_sparsity_terminal << 0, 1;
+
+ boxConstraintsTest<state_dim, control_dim, LinkedMasses>(
+ u0, x0, xf, nb_u, u_lb, u_ub, u_box_sparsity, nb_x, x_lb, x_ub, x_box_sparsity, x_box_sparsity_terminal);
+}
+
+TEST(ConstrainedLQOCSolverTest, GeneralConstrTest_small)
+{
+ const size_t state_dim = 2;
+ const size_t control_dim = 1;
+
+ // nominal control
+ ct::core::ControlVector<control_dim> u0;
+ u0.setConstant(0);
+
+ // initial state
+ ct::core::StateVector<state_dim> x0;
+ x0 << 2.5, 0.0;
+
+ // desired final state
+ ct::core::StateVector<state_dim> xf;
+ xf.setConstant(0.0);
+
+ // general constraints
+ // this general constraints is again nothing but an input inequality constraint:
+ Eigen::VectorXd d_lb, d_ub;
+ d_lb.resize(1);
+ d_ub.resize(1);
+ d_lb << -0.5;
+ d_ub << 0.5;
+
+ Eigen::MatrixXd C, D;
+ C.resize(1, 2);
+ D.resize(1, 1);
+ C.setZero();
+ D(0, 0) = 1;
+
+ generalConstraintsTest<state_dim, control_dim, example::LinearOscillatorLinear>(u0, x0, xf, d_lb, d_ub, C, D);
+}
+
+TEST(ConstrainedLQOCSolverTest, GeneralConstrTest_medium)
+{
+ const size_t state_dim = 8;
+ const size_t control_dim = 3;
+
+ // nominal control
+ ct::core::ControlVector<control_dim> u0;
+ u0.setConstant(0.1);
+
+ // initial state
+ ct::core::StateVector<state_dim> x0;
+ x0 << 2.5, 2.5, 0, 0, 0, 0, 0, 0;
+
+ // desired final state
+ ct::core::StateVector<state_dim> xf;
+ xf.setConstant(0.1);
+
+ // general constraints
+ // this general constraints is again nothing but an input inequality constraint:
+ Eigen::VectorXd d_lb, d_ub;
+ d_lb.resize(control_dim);
+ d_ub.resize(control_dim);
+ d_lb.setConstant(-0.5);
+ d_ub.setConstant(0.5);
+
+ Eigen::MatrixXd C, D;
+ C.resize(control_dim, state_dim);
+ D.resize(control_dim, control_dim);
+ C.setZero();
+ D.setIdentity();
+
+ generalConstraintsTest<state_dim, control_dim, LinkedMasses>(u0, x0, xf, d_lb, d_ub, C, D);
+}
+
+
+TEST(ConstrainedLQOCSolverTest, BoxConstraintUsingConstraintToolbox)
+{
+ const size_t state_dim = 8;
+ const size_t control_dim = 3;
+
+ // nominal control
+ ct::core::ControlVector<control_dim> u0;
+ u0.setConstant(0.1);
+
+ // initial state
+ ct::core::StateVector<state_dim> x0;
+ x0 << 2.5, 2.5, 0, 0, 0, 0, 0, 0;
+
+ // desired final state
+ ct::core::StateVector<state_dim> xf;
+ xf.setConstant(0.1);
+
+ // input box constraint boundaries with sparsities in constraint toolbox format
+ Eigen::VectorXi sp_control(control_dim);
+ sp_control << 1, 1, 1; // note the different way of specifying sparsity
+ Eigen::VectorXd u_lb(control_dim);
+ Eigen::VectorXd u_ub(control_dim);
+ u_lb.setConstant(-0.5);
+ u_ub = -u_lb;
+
+ // state box constraint boundaries with sparsities in constraint toolbox format
+ Eigen::VectorXi sp_state(state_dim);
+ sp_state << 1, 1, 0, 0, 0, 0, 0, 0; // note the different way of specifying sparsity
+ Eigen::VectorXd x_lb(2);
+ x_lb << 0.5, 0.5;
+ Eigen::VectorXd x_ub(2);
+ x_ub.setConstant(std::numeric_limits<double>::max());
+
+ // constrain terms
+ std::shared_ptr<ControlInputConstraint<state_dim, control_dim>> controlConstraint(
+ new ControlInputConstraint<state_dim, control_dim>(u_lb, u_ub, sp_control));
+ controlConstraint->setName("ControlInputConstraint");
+
+ std::shared_ptr<StateConstraint<state_dim, control_dim>> stateConstraint(
+ new StateConstraint<state_dim, control_dim>(x_lb, x_ub, sp_state));
+ stateConstraint->setName("StateConstraint");
+
+ // create constraint container
+ std::shared_ptr<ConstraintContainerAnalytical<state_dim, control_dim>> constraints(
+ new ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>());
+
+ // add and initialize constraint terms
+ constraints->addIntermediateConstraint(controlConstraint, verbose);
+ constraints->addIntermediateConstraint(stateConstraint, verbose);
+ constraints->addTerminalConstraint(stateConstraint, verbose);
+ constraints->initialize();
+
+ //set up lqoc problems and solvers
+ const size_t N = 5;
+ const double dt = 0.5;
+
+ // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> hpipmSolver(new HPIPMInterface<state_dim, control_dim>);
+
+ NLOptConSettings nloc_settings;
+ nloc_settings.lqoc_solver_settings.num_lqoc_iterations = 50; // allow 50 iterations
+ if (verbose)
+ nloc_settings.lqoc_solver_settings.lqoc_debug_print = true;
+ hpipmSolver->configure(nloc_settings);
+
+ // create linear-quadratic optimal control problem containers
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem1(new LQOCProblem<state_dim, control_dim>(N));
+
+ // create a continuous-time example system and discretize it
+ std::shared_ptr<core::LinearSystem<state_dim, control_dim>> exampleSystem(new LinkedMasses());
+ core::SensitivityApproximation<state_dim, control_dim> discreteExampleSystem(
+ dt, exampleSystem, core::SensitivityApproximationSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
+
+ // define cost function matrices
+ StateMatrix<state_dim> Q;
+ Q.setIdentity();
+ Q *= 2.0;
+ ControlMatrix<control_dim> R;
+ R.setIdentity();
+ R *= 2 * 2.0;
+
+ // create a cost function
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> costFunction(
+ new CostFunctionQuadraticSimple<state_dim, control_dim>(Q, R, xf, u0, xf, Q));
+
+ // solution variables needed later
+ ct::core::StateVectorArray<state_dim> xSol_hpipm;
+ ct::core::ControlVectorArray<control_dim> uSol_hpipm;
+ ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm;
+
+ if (verbose)
+ {
+ std::cout << " ================================================== " << std::endl;
+ std::cout << " LQOC PROBLEM USING CONSTRAINT TOOLBOX " << std::endl;
+ std::cout << " ================================================== " << std::endl;
+ }
+
+ // initialize the optimal control problems for both solvers
+ lqocProblem1->setZero();
+ lqocProblem1->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, xf, dt);
+
+ // evaluate relevant quantities using the constraint toolbox
+ int nb_ux_intermediate = constraints->getJacobianStateNonZeroCountIntermediate() +
+ constraints->getJacobianInputNonZeroCountIntermediate();
+ ASSERT_EQ(nb_ux_intermediate, 5);
+ int nb_x_terminal = constraints->getJacobianStateNonZeroCountTerminal();
+ ASSERT_EQ(nb_x_terminal, 2);
+
+ // get bounds
+ Eigen::VectorXd ux_lb_intermediate = constraints->getLowerBoundsIntermediate();
+ Eigen::VectorXd ux_ub_intermediate = constraints->getUpperBoundsIntermediate();
+ Eigen::VectorXd ux_lb_terminal = constraints->getLowerBoundsTerminal();
+ Eigen::VectorXd ux_ub_terminal = constraints->getUpperBoundsTerminal();
+
+ // compute sparsity as required by LQOC Solver
+ Eigen::VectorXi foo, u_sparsity_intermediate, x_sparsity_intermediate, x_sparsity_terminal;
+ constraints->sparsityPatternInputIntermediate(foo, u_sparsity_intermediate);
+ constraints->sparsityPatternStateIntermediate(foo, x_sparsity_intermediate);
+ Eigen::VectorXi ux_sparsity_intermediate(nb_ux_intermediate);
+ x_sparsity_intermediate.array() += control_dim; // shift indices to match combined decision vector [u, x]
+ ux_sparsity_intermediate << u_sparsity_intermediate, x_sparsity_intermediate;
+
+ if (verbose)
+ std::cout << "ux_sparsity_intermediate" << ux_sparsity_intermediate.transpose() << std::endl;
+
+ constraints->sparsityPatternStateTerminal(foo, x_sparsity_terminal);
+ if (verbose)
+ std::cout << "x_sparsity_terminal" << x_sparsity_terminal.transpose() << std::endl;
+
+
+ // set the combined box constraints to the LQOC problem
+ lqocProblem1->setIntermediateBoxConstraints(
+ nb_ux_intermediate, ux_lb_intermediate, ux_ub_intermediate, ux_sparsity_intermediate);
+ lqocProblem1->setTerminalBoxConstraints(nb_x_terminal, ux_lb_terminal, ux_ub_terminal, x_sparsity_terminal);
+
+ // check that constraint configuration is right
+ ASSERT_TRUE(lqocProblem1->isConstrained());
+ ASSERT_FALSE(lqocProblem1->isGeneralConstrained());
+ ASSERT_TRUE(lqocProblem1->isBoxConstrained());
+
+ // set and try to solve the problem for both solvers
+ hpipmSolver->configureBoxConstraints(lqocProblem1);
+ hpipmSolver->setProblem(lqocProblem1);
+ hpipmSolver->initializeAndAllocate();
+ hpipmSolver->solve();
+
+
+ // retrieve solutions from hpipm
+ xSol_hpipm = hpipmSolver->getSolutionState();
+ uSol_hpipm = hpipmSolver->getSolutionControl();
+ hpipmSolver->getFeedback(KSol_hpipm);
+
+ if (verbose)
+ printSolution<state_dim, control_dim>(xSol_hpipm, uSol_hpipm, KSol_hpipm);
+}
diff --git a/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h b/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
new file mode 100644
index 0000000..3cc4246
--- /dev/null
+++ b/ct_optcon/test/solver/linear/HPIPMInterfaceTest.h
@@ -0,0 +1,96 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This unit test compares HPIPM and the custom GNRiccati solver using custom defined system
+ * with state dimension 8 and control dimension 3.
+ *
+ * \warning This example is not intuitive. For a better introduction into the solver-framework,
+ * visit the tutorial.
+ */
+
+#include "../../testSystems/LinkedMasses.h"
+
+TEST(HPIPMInterfaceTest, compareSolvers)
+{
+ const size_t state_dim = 8;
+ const size_t control_dim = 3;
+
+ int N = 5;
+ double dt = 0.5;
+
+ typedef ct::optcon::LQOCProblem<state_dim, control_dim> LQOCProblem_t;
+ std::shared_ptr<LQOCProblem_t> lqocProblem_hpipm(new LQOCProblem_t(N));
+ std::shared_ptr<LQOCProblem_t> lqocProblem_gnriccati(new LQOCProblem_t(N));
+
+ // define an initial state
+ StateVector<state_dim> x0;
+ x0 << 2.5, 2.5, 0, 0, 0, 0, 0, 0;
+
+ // define a desired terminal state
+ StateVector<state_dim> stateOffset;
+ stateOffset.setConstant(0.1);
+
+ // define a nominal control
+ ControlVector<control_dim> u0;
+ u0.setConstant(-0.1);
+
+ // define cost function matrices
+ StateMatrix<state_dim> Q;
+ Q.setIdentity();
+ Q *= 2.0;
+ ControlMatrix<control_dim> R;
+ R.setIdentity();
+ R *= 2 * 2.0;
+
+ // create a cost function
+ ct::optcon::CostFunctionQuadraticSimple<state_dim, control_dim> costFunction(
+ Q, R, -stateOffset, u0, -stateOffset, Q);
+
+ // create a continuous-time example system and discretize it
+ std::shared_ptr<ct::core::LinearSystem<state_dim, control_dim>> system(new LinkedMasses());
+ ct::core::SensitivityApproximation<state_dim, control_dim> discretizedSystem(
+ dt, system, ct::optcon::NLOptConSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
+
+ // initialize the linear quadratic optimal control problems
+ lqocProblem_hpipm->setFromTimeInvariantLinearQuadraticProblem(
+ x0, u0, discretizedSystem, costFunction, stateOffset, dt);
+ lqocProblem_gnriccati->setFromTimeInvariantLinearQuadraticProblem(
+ x0, u0, discretizedSystem, costFunction, stateOffset, dt);
+
+
+ // create hpipm solver instance, set and solve problem
+ ct::optcon::HPIPMInterface<state_dim, control_dim> hpipm;
+ hpipm.setProblem(lqocProblem_hpipm);
+ hpipm.solve();
+
+ // create GNRiccati solver instance, set and solve problem
+ ct::optcon::GNRiccatiSolver<state_dim, control_dim> gnriccati;
+ gnriccati.setProblem(lqocProblem_gnriccati);
+ gnriccati.solve();
+
+ // retrieve solutions
+ ct::core::StateVectorArray<state_dim> x_sol_hpipm = hpipm.getSolutionState();
+ ct::core::StateVectorArray<state_dim> x_sol_gnrccati = gnriccati.getSolutionState();
+ ct::core::ControlVectorArray<control_dim> u_sol_hpipm = hpipm.getSolutionControl();
+ ct::core::ControlVectorArray<control_dim> u_sol_gnrccati = gnriccati.getSolutionControl();
+
+ // asser that the solution sizes the same
+ ASSERT_EQ(x_sol_hpipm.size(), x_sol_gnrccati.size());
+ ASSERT_EQ(u_sol_hpipm.size(), u_sol_gnrccati.size());
+
+ // assert that states are the same
+ for (size_t i = 0; i < x_sol_hpipm.size(); i++)
+ {
+ ASSERT_LT((x_sol_hpipm[i] - x_sol_gnrccati[i]).array().abs().maxCoeff(), 1e-6);
+ }
+
+ // assert that controls are the same
+ for (size_t i = 0; i < u_sol_hpipm.size(); i++)
+ {
+ ASSERT_LT((u_sol_hpipm[i] - u_sol_gnrccati[i]).array().abs().maxCoeff(), 1e-6);
+ }
+}
diff --git a/ct_optcon/test/solver/linear/LQOCSolverTest.cpp b/ct_optcon/test/solver/linear/LQOCSolverTest.cpp
new file mode 100644
index 0000000..4c40d09
--- /dev/null
+++ b/ct_optcon/test/solver/linear/LQOCSolverTest.cpp
@@ -0,0 +1,19 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <gtest/gtest.h>
+#include <ct/optcon/optcon.h>
+
+// several tests which compare HPIPM and GNRiccati
+#include "LQOCSolverTest.h"
+#include "HPIPMInterfaceTest.h"
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/solver/linear/LQOCSolverTest.h b/ct_optcon/test/solver/linear/LQOCSolverTest.h
new file mode 100644
index 0000000..d8710f6
--- /dev/null
+++ b/ct_optcon/test/solver/linear/LQOCSolverTest.h
@@ -0,0 +1,137 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+
+using namespace ct;
+using namespace ct::optcon;
+
+#include "../../testSystems/SpringLoadedMass.h"
+
+
+TEST(LQOCSolverTest, compareHPIPMandRiccati)
+{
+ const size_t state_dim = 2;
+ const size_t control_dim = 1;
+ const size_t N = 5;
+ const double dt = 0.5;
+
+ bool verbose = false; // optional verbose output
+
+ // create instances of HPIPM and an unconstrained Gauss-Newton Riccati solver
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> hpipmSolver(new HPIPMInterface<state_dim, control_dim>);
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> gnRiccatiSolver(new GNRiccatiSolver<state_dim, control_dim>);
+
+ // store them, and identifying names, in a vectors
+ std::vector<std::shared_ptr<LQOCSolver<state_dim, control_dim>>> lqocSolvers;
+ lqocSolvers.push_back(gnRiccatiSolver);
+ lqocSolvers.push_back(hpipmSolver);
+ std::vector<std::string> solverNames = {"Riccati", "HPIPM"};
+
+ // create linear-quadratic optimal control problem containers
+ std::vector<std::shared_ptr<LQOCProblem<state_dim, control_dim>>> problems;
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem1(new LQOCProblem<state_dim, control_dim>(N));
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem2(new LQOCProblem<state_dim, control_dim>(N));
+
+ problems.push_back(lqocProblem1);
+ problems.push_back(lqocProblem2);
+
+ // create a continuous-time example system and discretize it
+ std::shared_ptr<core::LinearSystem<state_dim, control_dim>> exampleSystem(new example::SpringLoadedMassLinear());
+ core::SensitivityApproximation<state_dim, control_dim> discreteExampleSystem(
+ dt, exampleSystem, core::SensitivityApproximationSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
+
+ // nominal control
+ ct::core::ControlVector<control_dim> u0;
+ u0 << 0.1;
+ // initial state
+ ct::core::StateVector<state_dim> x0;
+ x0 << 0.2, 0.1;
+ // desired final state
+ ct::core::StateVector<state_dim> xf;
+ xf << -1, 0;
+
+ // create pointer to a cost function
+ auto costFunction = example::createSpringLoadedMassCostFunction(xf);
+
+ ct::core::StateVector<state_dim> b;
+ b << 0.1, 0.1;
+
+ // initialize the optimal control problems for both solvers
+ problems[0]->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, b, dt);
+ problems[1]->setFromTimeInvariantLinearQuadraticProblem(x0, u0, discreteExampleSystem, *costFunction, b, dt);
+
+ // configure box constraints
+ try
+ {
+ // try to give GNRiccati a constraint...
+ lqocSolvers[1]->configureBoxConstraints(problems[1]);
+ ASSERT_TRUE(false);
+ } catch (const std::exception& e)
+ {
+ // ... should fail
+ ASSERT_TRUE(true);
+ }
+ lqocSolvers[1]->configureBoxConstraints(problems[1]);
+
+ // set the problem pointers
+ lqocSolvers[0]->setProblem(problems[0]);
+ lqocSolvers[1]->setProblem(problems[1]);
+
+ // solve the problems
+ lqocSolvers[0]->solve();
+ lqocSolvers[1]->solve();
+
+ // retrieve solutions from both solvers
+ auto xSol_riccati = lqocSolvers[0]->getSolutionState();
+ auto uSol_riccati = lqocSolvers[0]->getSolutionControl();
+ auto xSol_hpipm = lqocSolvers[1]->getSolutionState();
+ auto uSol_hpipm = lqocSolvers[1]->getSolutionControl();
+
+ ct::core::FeedbackArray<state_dim, control_dim> KSol_riccati;
+ ct::core::FeedbackArray<state_dim, control_dim> KSol_hpipm;
+ lqocSolvers[0]->getFeedback(KSol_riccati);
+ lqocSolvers[1]->getFeedback(KSol_hpipm);
+
+
+ for (size_t j = 0; j < xSol_riccati.size(); j++)
+ {
+ if (verbose)
+ {
+ std::cout << "x solution from riccati solver:" << std::endl;
+ std::cout << xSol_riccati[j].transpose() << std::endl;
+ std::cout << "x solution from hpipm solver:" << std::endl;
+ std::cout << xSol_hpipm[j].transpose() << std::endl;
+ }
+ // assert that state trajectories are identical for both solvers
+ ASSERT_NEAR((xSol_riccati[j] - xSol_hpipm[j]).array().abs().maxCoeff(), 0.0, 1e-6);
+ }
+
+ for (size_t j = 0; j < uSol_riccati.size(); j++)
+ {
+ if (verbose)
+ {
+ std::cout << "u solution from riccati solver:" << std::endl;
+ std::cout << uSol_riccati[j].transpose() << std::endl;
+ std::cout << "u solution from hpipm solver:" << std::endl;
+ std::cout << uSol_hpipm[j].transpose() << std::endl;
+ }
+ // assert that control trajectories are identical for both solvers
+ ASSERT_NEAR((uSol_riccati[j] - uSol_hpipm[j]).array().abs().maxCoeff(), 0.0, 1e-6);
+ }
+
+ for (size_t j = 0; j < KSol_riccati.size(); j++)
+ {
+ if (verbose)
+ {
+ std::cout << "K solution from riccati solver:" << std::endl;
+ std::cout << KSol_riccati[j] << std::endl << std::endl;
+ std::cout << "K solution from hpipm solver:" << std::endl;
+ std::cout << KSol_hpipm[j] << std::endl << std::endl;
+ }
+ // assert that feedback trajectories are identical for both solvers
+ ASSERT_NEAR((KSol_riccati[j] - KSol_hpipm[j]).array().abs().maxCoeff(), 0.0, 1e-6);
+ }
+}
diff --git a/ct_optcon/test/solver/linear/LQOCSolverTestPrespec.cpp b/ct_optcon/test/solver/linear/LQOCSolverTestPrespec.cpp
new file mode 100644
index 0000000..fa20179
--- /dev/null
+++ b/ct_optcon/test/solver/linear/LQOCSolverTestPrespec.cpp
@@ -0,0 +1,18 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <gtest/gtest.h>
+#include <ct/optcon/optcon-prespec.h>
+
+#include "LQOCSolverTest.h"
+
+
+int main(int argc, char **argv)
+{
+ using namespace ct::optcon::example;
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/ct_optcon/test/solver/linear/LQOCSolverTiming.cpp b/ct_optcon/test/solver/linear/LQOCSolverTiming.cpp
new file mode 100644
index 0000000..5f13783
--- /dev/null
+++ b/ct_optcon/test/solver/linear/LQOCSolverTiming.cpp
@@ -0,0 +1,187 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This executable serves to compare the run-times of our own Riccati LQ problem solver and Gianluca Frison's HPIPM solver.
+ * It is not a supposed to be a unit test, but can be used to compare runtimes on different machines.
+ */
+
+#include <ct/optcon/optcon.h>
+
+using namespace ct;
+using namespace ct::optcon;
+
+#include "../../testSystems/MIMOIntegrator.h"
+
+const double dt = 0.1;
+
+template <size_t control_dim, size_t state_dim>
+void timeSingleSolve(size_t N, std::vector<std::vector<double>>& loggedSolveTimes)
+{
+ std::cout << "time horizon: " << N << std::endl;
+ std::cout << "============= " << std::endl << std::endl;
+
+ std::vector<std::shared_ptr<LQOCSolver<state_dim, control_dim>>> lqocSolvers;
+
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> hpipmSolver(new HPIPMInterface<state_dim, control_dim>(N));
+ std::shared_ptr<LQOCSolver<state_dim, control_dim>> gnRiccatiSolver(new GNRiccatiSolver<state_dim, control_dim>(N));
+ std::vector<std::string> solverNames = {"Riccati", "HPIPM"};
+
+ NLOptConSettings gnRiccatiSolverSettings;
+ gnRiccatiSolverSettings.fixedHessianCorrection = true;
+ gnRiccatiSolverSettings.epsilon = 0;
+ gnRiccatiSolverSettings.recordSmallestEigenvalue = false;
+ gnRiccatiSolverSettings.nThreadsEigen = 1;
+ gnRiccatiSolver->configure(gnRiccatiSolverSettings);
+
+ lqocSolvers.push_back(gnRiccatiSolver);
+ lqocSolvers.push_back(hpipmSolver);
+
+ loggedSolveTimes.resize(lqocSolvers.size());
+
+ std::vector<std::shared_ptr<LQOCProblem<state_dim, control_dim>>> problems;
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem1(new LQOCProblem<state_dim, control_dim>(N));
+ std::shared_ptr<LQOCProblem<state_dim, control_dim>> lqocProblem2(new LQOCProblem<state_dim, control_dim>(N));
+
+ problems.push_back(lqocProblem1);
+ problems.push_back(lqocProblem2);
+
+ std::shared_ptr<core::LinearSystem<state_dim, control_dim>> exampleSystem(
+ new example::MIMOIntegratorLinear<state_dim, control_dim>());
+ core::SensitivityApproximation<state_dim, control_dim> discreteExampleSystem(
+ dt, exampleSystem, core::SensitivityApproximationSettings::APPROXIMATION::MATRIX_EXPONENTIAL);
+
+ ct::core::ControlVector<control_dim> u0;
+ u0.setRandom();
+ ct::core::StateVector<state_dim> x0;
+ x0.setRandom();
+ ct::core::StateVector<state_dim> xf;
+ xf.setRandom();
+
+ auto costFunction = example::createMIMOIntegratorCostFunction<state_dim, control_dim>(xf);
+
+ ct::core::StateVector<state_dim> b;
+ b.setZero();
+
+
+ for (size_t i = 0; i < lqocSolvers.size(); i++)
+ {
+ std::vector<double> setProblemTime;
+ std::vector<double> solveTime;
+ std::vector<double> getTime;
+ std::vector<double> totalTime;
+
+ size_t nRuns = 20000 / N;
+
+ for (size_t j = 0; j < nRuns; j++)
+ {
+ problems[i]->setFromTimeInvariantLinearQuadraticProblem(
+ x0, u0, discreteExampleSystem, *costFunction, b, dt);
+
+ auto start_all = std::chrono::steady_clock::now();
+
+ auto start_set = std::chrono::steady_clock::now();
+ lqocSolvers[i]->setProblem(problems[i]);
+ auto end_set = std::chrono::steady_clock::now();
+
+ auto start_solve = std::chrono::steady_clock::now();
+ lqocSolvers[i]->solve();
+ auto end_solve = std::chrono::steady_clock::now();
+
+ auto start_get = std::chrono::steady_clock::now();
+ auto xSol = lqocSolvers[i]->getSolutionState();
+ auto uSol = lqocSolvers[i]->getSolutionControl();
+ auto end_get = std::chrono::steady_clock::now();
+
+ auto end_all = std::chrono::steady_clock::now();
+
+ // record times
+ setProblemTime.push_back(std::chrono::duration<double, std::milli>(end_set - start_set).count());
+ solveTime.push_back(std::chrono::duration<double, std::milli>(end_solve - start_solve).count());
+ getTime.push_back(std::chrono::duration<double, std::milli>(end_get - start_get).count());
+ totalTime.push_back(std::chrono::duration<double, std::milli>(end_all - start_all).count());
+ }
+
+ // average times
+ double avgSetTime = std::accumulate(setProblemTime.begin(), setProblemTime.end(), 0.0) / (double)nRuns;
+ double avgSolveTime = std::accumulate(solveTime.begin(), solveTime.end(), 0.0) / (double)nRuns;
+ double avgGetTime = std::accumulate(getTime.begin(), getTime.end(), 0.0) / (double)nRuns;
+ double avgTotalTime = std::accumulate(totalTime.begin(), totalTime.end(), 0.0) / (double)nRuns;
+
+ loggedSolveTimes[i].push_back(avgTotalTime);
+
+ std::cout << "average setProblem() with " << solverNames[i] << " took\t" << avgSetTime << " ms" << std::endl;
+ std::cout << "average solve() with " << solverNames[i] << " took\t" << avgSolveTime << " ms" << std::endl;
+ std::cout << "average get() with " << solverNames[i] << " took\t" << avgGetTime << " ms" << std::endl;
+ std::cout << "average total call with " << solverNames[i] << " took\t" << avgTotalTime << " ms" << std::endl;
+
+ // std::cout << "x:" << std::endl;
+ // for (size_t j=0; j<xSol.size(); j++)
+ // std::cout<<xSol[j].transpose()<<std::endl;
+ //
+ // std::cout << "u:" << std::endl;
+ // for (size_t j=0; j<uSol.size(); j++)
+ // std::cout<<uSol[j].transpose()<<std::endl;
+
+ std::cout << std::endl;
+ }
+ std::cout << std::endl;
+}
+
+
+template <size_t control_dim, size_t state_dim>
+void timeSolvers()
+{
+ std::vector<size_t> testTimeHorizons = {5, 10, 50, 100, 500, 1000, 2000, 5000, 10000, 15000};
+
+ std::cout << "Test System: " << std::endl;
+ std::cout << "============ " << std::endl;
+ std::cout << "state dim: " << state_dim << std::endl;
+ std::cout << "control dim: " << control_dim << std::endl;
+
+ std::vector<std::vector<double>> loggedSolveTimes;
+
+
+ for (size_t i = 0; i < testTimeHorizons.size(); i++)
+ {
+ timeSingleSolve<control_dim, state_dim>(testTimeHorizons[i], loggedSolveTimes);
+ }
+
+ std::cout << std::endl << std::endl << std::endl << std::endl;
+
+ std::cout << "Matlab friendly output:" << std::endl;
+ std::cout << "stages = [";
+ for (size_t i = 0; i < testTimeHorizons.size(); i++)
+ {
+ std::cout << testTimeHorizons[i];
+ if (i < testTimeHorizons.size() - 1)
+ std::cout << ", ";
+ }
+ std::cout << "];" << std::endl;
+
+ for (size_t j = 0; j < 2; j++)
+ {
+ std::cout << "times_solver" << j << " = [";
+ for (size_t i = 0; i < testTimeHorizons.size(); i++)
+ {
+ std::cout << loggedSolveTimes[j][i];
+ if (i < testTimeHorizons.size() - 1)
+ std::cout << ", ";
+ }
+ std::cout << "];" << std::endl;
+ }
+}
+
+int main(int argc, char** argv)
+{
+ timeSolvers<3, 3>();
+ timeSolvers<6, 6>();
+ timeSolvers<12, 12>();
+ timeSolvers<24, 24>();
+ timeSolvers<12, 36>();
+
+ return 1;
+}
diff --git a/ct_optcon/test/testSystems/DiehlSystem.h b/ct_optcon/test/testSystems/DiehlSystem.h
new file mode 100644
index 0000000..e9b74bc
--- /dev/null
+++ b/ct_optcon/test/testSystems/DiehlSystem.h
@@ -0,0 +1,93 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+
+/*!
+ *
+ * We named this system after Prof. Moritz Diehl from the University of Freiburg, who gave it to us as a simple 1-dimensional test system.
+ * The system dynamics are dx/dt = (1+x)x + u + 0.1
+ *
+ */
+//! Dynamics class for the Diehl system
+class DiehlSystem : public core::ControlledSystem<1, 1>
+{
+public:
+ static const int state_dim = 1;
+ static const int control_dim = 1;
+
+ DiehlSystem() : core::ControlledSystem<1, 1>(core::SYSTEM_TYPE::SECOND_ORDER) {}
+ void computeControlledDynamics(const core::StateVector<1>& state,
+ const core::Time& t,
+ const core::ControlVector<1>& control,
+ core::StateVector<1>& derivative) override
+ {
+ derivative(0) = (1.0 + state(0)) * state(0) + control(0) + 0.1;
+ }
+
+ DiehlSystem* clone() const override { return new DiehlSystem(); };
+};
+
+//! Linear system class for the Diehl system
+class DiehlSystemLinear : public core::LinearSystem<1, 1>
+{
+public:
+ static const int state_dim = 1;
+ static const int control_dim = 1;
+
+ state_matrix_t A_;
+ state_control_matrix_t B_;
+
+
+ const state_matrix_t& getDerivativeState(const core::StateVector<1>& x,
+ const core::ControlVector<1>& u,
+ const double t = 0.0) override
+ {
+ A_ << 1 + 2 * x(0);
+ return A_;
+ }
+
+ const state_control_matrix_t& getDerivativeControl(const core::StateVector<1>& x,
+ const core::ControlVector<1>& u,
+ const double t = 0.0) override
+ {
+ B_ << 1;
+ return B_;
+ }
+
+ DiehlSystemLinear* clone() const override { return new DiehlSystemLinear(); }
+};
+
+
+//! create a cost function of appropriate Dimensions for the Diehl system
+std::shared_ptr<CostFunctionQuadratic<1, 1>> createDiehlCostFunction(const core::StateVector<1>& x_final)
+{
+ Eigen::Matrix<double, 1, 1> Q;
+ Q << 1.0;
+
+ Eigen::Matrix<double, 1, 1> R;
+ R << 1.0;
+
+ Eigen::Matrix<double, 1, 1> x_nominal = x_final;
+ Eigen::Matrix<double, 1, 1> u_nominal;
+ u_nominal.setConstant(-0.1); //Eigen::Matrix<double, 1, 1>::Zero();
+
+ Eigen::Matrix<double, 1, 1> Q_final;
+ Q_final << 10.0;
+
+ std::shared_ptr<CostFunctionQuadratic<1, 1>> quadraticCostFunction(
+ new CostFunctionQuadraticSimple<1, 1>(Q, R, x_nominal, u_nominal, x_final, Q_final));
+
+ return quadraticCostFunction;
+}
+}
+}
+}
diff --git a/ct_optcon/test/testSystems/LinearOscillator.h b/ct_optcon/test/testSystems/LinearOscillator.h
new file mode 100644
index 0000000..3ac65c8
--- /dev/null
+++ b/ct_optcon/test/testSystems/LinearOscillator.h
@@ -0,0 +1,111 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+
+using namespace ct::core;
+using namespace ct::optcon;
+
+using std::shared_ptr;
+
+const size_t state_dim = 2; // position, velocity
+const size_t control_dim = 1; // force
+
+const double kStiffness = 0.1;
+
+
+namespace tpl {
+
+template <typename SCALAR = double>
+class LinearOscillator : public ControlledSystem<state_dim, control_dim, SCALAR>
+{
+public:
+ LinearOscillator() : ControlledSystem<state_dim, control_dim, SCALAR>(SYSTEM_TYPE::GENERAL) {}
+ void computeControlledDynamics(const StateVector<state_dim, SCALAR>& state,
+ const SCALAR& t,
+ const ControlVector<control_dim, SCALAR>& control,
+ StateVector<state_dim, SCALAR>& derivative) override
+ {
+ derivative(0) = state(1);
+ derivative(1) = control(0) - kStiffness * state(0); // mass is 1 kg
+ }
+
+ LinearOscillator<SCALAR>* clone() const override { return new LinearOscillator<SCALAR>(); };
+};
+
+
+template <typename SCALAR = double>
+class LinearOscillatorLinear : public LinearSystem<state_dim, control_dim, SCALAR>
+{
+public:
+ typedef core::StateMatrix<state_dim, SCALAR> state_matrix_t;
+ typedef core::StateControlMatrix<state_dim, control_dim, SCALAR> state_control_matrix_t;
+
+ state_matrix_t A_;
+ state_control_matrix_t B_;
+
+ const state_matrix_t& getDerivativeState(const StateVector<state_dim, SCALAR>& x,
+ const ControlVector<control_dim, SCALAR>& u,
+ const SCALAR t = 0.0) override
+ {
+ A_ << 0, 1, -kStiffness, 0;
+ return A_;
+ }
+
+ const state_control_matrix_t& getDerivativeControl(const StateVector<state_dim, SCALAR>& x,
+ const ControlVector<control_dim, SCALAR>& u,
+ const SCALAR t = 0.0) override
+ {
+ B_ << 0, 1;
+ return B_;
+ }
+
+ LinearOscillatorLinear<SCALAR>* clone() const override { return new LinearOscillatorLinear<SCALAR>(); };
+};
+
+
+template <typename SCALAR = double>
+std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim, SCALAR>> createCostFunctionLinearOscillator(
+ Eigen::Matrix<SCALAR, 2, 1>& x_final)
+{
+ Eigen::Matrix<SCALAR, 2, 2> Q;
+ Q << 0, 0, 0, 1;
+
+ Eigen::Matrix<SCALAR, 1, 1> R;
+ R << 100;
+
+ ct::core::StateVector<2> x_nominal = ct::core::StateVector<2>::Zero();
+ ct::core::ControlVector<1> u_nominal = ct::core::ControlVector<1>::Zero();
+
+ Eigen::Matrix<SCALAR, 2, 2> Q_final;
+ Q_final << 1000, 0, 0, 1000;
+
+ std::shared_ptr<TermQuadratic<state_dim, control_dim>> termIntermediate(
+ new TermQuadratic<state_dim, control_dim>(Q, R, x_nominal, u_nominal));
+ std::shared_ptr<TermQuadratic<state_dim, control_dim>> termFinal(
+ new TermQuadratic<state_dim, control_dim>(Q_final, R, x_final, u_nominal));
+
+ std::shared_ptr<CostFunctionAnalytical<state_dim, control_dim>> quadraticCostFunction(
+ new CostFunctionAnalytical<state_dim, control_dim>);
+ quadraticCostFunction->addIntermediateTerm(termIntermediate);
+ quadraticCostFunction->addFinalTerm(termFinal);
+
+ return quadraticCostFunction;
+}
+
+} // namespace tpl
+
+typedef tpl::LinearOscillator<double> LinearOscillator;
+typedef tpl::LinearOscillatorLinear<double> LinearOscillatorLinear;
+
+} //example
+} //optcon
+} //ct
diff --git a/ct_optcon/test/testSystems/LinkedMasses.h b/ct_optcon/test/testSystems/LinkedMasses.h
new file mode 100644
index 0000000..c35b1b1
--- /dev/null
+++ b/ct_optcon/test/testSystems/LinkedMasses.h
@@ -0,0 +1,166 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+/*!
+ * This header implements example systems from the HPIPM library as CT systems
+ *
+ * \warning This example system is not intuitive. For a better examples, visit the tutorial.
+ */
+
+#pragma once
+
+using namespace ct::core;
+
+
+void dmcopy(int row, int col, double *A, int lda, double *B, int ldb)
+{
+ int i, j;
+ for (j = 0; j < col; j++)
+ {
+ for (i = 0; i < row; i++)
+ {
+ B[i + j * ldb] = A[i + j * lda];
+ }
+ }
+}
+
+class LinkedMasses : public LinearSystem<8, 3>
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ static const int state_dim = 8;
+ static const int control_dim = 3;
+ static const int pp = state_dim / 2; // number of masses
+
+ LinkedMasses()
+ {
+ A_.setZero();
+ B_.setZero();
+
+
+ Eigen::Matrix<double, pp, pp> TEigen;
+ TEigen.setZero();
+
+ double *T = TEigen.data();
+ int ii;
+ for (ii = 0; ii < pp; ii++)
+ T[ii * (pp + 1)] = -2;
+ for (ii = 0; ii < pp - 1; ii++)
+ T[ii * (pp + 1) + 1] = 1;
+ for (ii = 1; ii < pp; ii++)
+ T[ii * (pp + 1) - 1] = 1;
+
+ Eigen::Matrix<double, pp, pp> ZEigen;
+ ZEigen.setZero();
+ double *Z = ZEigen.data();
+
+ Eigen::Matrix<double, pp, pp> IEigen;
+ IEigen.setIdentity();
+ double *I = IEigen.data();
+
+ double *Ac = A_.data();
+ dmcopy(pp, pp, Z, pp, Ac, state_dim);
+ dmcopy(pp, pp, T, pp, Ac + pp, state_dim);
+ dmcopy(pp, pp, I, pp, Ac + pp * state_dim, state_dim);
+ dmcopy(pp, pp, Z, pp, Ac + pp * (state_dim + 1), state_dim);
+
+ Eigen::Matrix<double, control_dim, control_dim> InuEigen;
+ InuEigen.setIdentity();
+ double *Inu = InuEigen.data();
+
+ double *Bc = B_.data();
+ dmcopy(control_dim, control_dim, Inu, control_dim, Bc + pp, state_dim);
+ }
+
+
+ const state_matrix_t &getDerivativeState(const StateVector<state_dim> &x,
+ const ControlVector<control_dim> &u,
+ const double t = 0.0) override
+ {
+ return A_;
+ }
+
+ const state_control_matrix_t &getDerivativeControl(const StateVector<state_dim> &x,
+ const ControlVector<control_dim> &u,
+ const double t = 0.0) override
+ {
+ return B_;
+ }
+
+ LinkedMasses *clone() const override { return new LinkedMasses(); };
+private:
+ state_matrix_t A_;
+ state_control_matrix_t B_;
+};
+
+
+class LinkedMasses2 : public ControlledSystem<8, 3>
+{
+public:
+ static const int state_dim = 8;
+ static const int control_dim = 3;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ LinkedMasses2()
+ {
+ A_.setZero();
+ B_.setZero();
+ b_.setZero();
+
+ b_ << 0.145798, 0.150018, 0.150018, 0.145798, 0.245798, 0.200018, 0.200018, 0.245798;
+
+ static const int pp = state_dim / 2; // number of masses
+
+ Eigen::Matrix<double, pp, pp> TEigen;
+ TEigen.setZero();
+
+ double *T = TEigen.data();
+ int ii;
+ for (ii = 0; ii < pp; ii++)
+ T[ii * (pp + 1)] = -2;
+ for (ii = 0; ii < pp - 1; ii++)
+ T[ii * (pp + 1) + 1] = 1;
+ for (ii = 1; ii < pp; ii++)
+ T[ii * (pp + 1) - 1] = 1;
+
+ Eigen::Matrix<double, pp, pp> ZEigen;
+ ZEigen.setZero();
+ double *Z = ZEigen.data();
+
+ Eigen::Matrix<double, pp, pp> IEigen;
+ IEigen.setIdentity();
+ double *I = IEigen.data();
+
+ double *Ac = A_.data();
+ dmcopy(pp, pp, Z, pp, Ac, state_dim);
+ dmcopy(pp, pp, T, pp, Ac + pp, state_dim);
+ dmcopy(pp, pp, I, pp, Ac + pp * state_dim, state_dim);
+ dmcopy(pp, pp, Z, pp, Ac + pp * (state_dim + 1), state_dim);
+
+ Eigen::Matrix<double, control_dim, control_dim> InuEigen;
+ InuEigen.setIdentity();
+ double *Inu = InuEigen.data();
+
+ double *Bc = B_.data();
+ dmcopy(control_dim, control_dim, Inu, control_dim, Bc + pp, state_dim);
+ }
+
+ LinkedMasses2 *clone() const override { return new LinkedMasses2(); };
+ void computeControlledDynamics(const ct::core::StateVector<state_dim> &state,
+ const double &t,
+ const ct::core::ControlVector<control_dim> &control,
+ ct::core::StateVector<state_dim> &derivative) override
+ {
+ derivative = A_ * state + B_ * control + b_;
+ }
+
+private:
+ ct::core::StateMatrix<state_dim> A_;
+ ct::core::StateControlMatrix<state_dim, control_dim> B_;
+ ct::core::StateVector<state_dim> b_;
+};
diff --git a/ct_optcon/test/testSystems/MIMOIntegrator.h b/ct_optcon/test/testSystems/MIMOIntegrator.h
new file mode 100644
index 0000000..ff4e961
--- /dev/null
+++ b/ct_optcon/test/testSystems/MIMOIntegrator.h
@@ -0,0 +1,101 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+
+//! Dynamics class for the GNMS unit test
+
+template <size_t state_dim, size_t control_dim>
+class MIMOIntegrator : public core::ControlledSystem<state_dim, control_dim>
+{
+public:
+ MIMOIntegrator() : core::ControlledSystem<state_dim, control_dim>(core::SYSTEM_TYPE::SECOND_ORDER) {}
+ void computeControlledDynamics(const core::StateVector<state_dim>& state,
+ const core::Time& t,
+ const core::ControlVector<control_dim>& control,
+ core::StateVector<state_dim>& derivative) override
+ {
+ for (size_t i = 0; i < state_dim; i++)
+ {
+ derivative(i) = state(i);
+ }
+ for (size_t j = 0; j < control_dim; j++)
+ {
+ derivative(j) += control(j);
+ }
+ }
+
+ MIMOIntegrator* clone() const override { return new MIMOIntegrator(); };
+};
+
+//! Linear system class for the GNMS unit test
+template <size_t state_dim, size_t control_dim>
+class MIMOIntegratorLinear : public core::LinearSystem<state_dim, control_dim>
+{
+public:
+ using BASE = core::LinearSystem<state_dim, control_dim>;
+ typedef typename BASE::state_matrix_t state_matrix_t;
+ typedef typename BASE::state_control_matrix_t state_control_matrix_t;
+
+ state_matrix_t A_;
+ state_control_matrix_t B_;
+
+ MIMOIntegratorLinear()
+ {
+ A_.setIdentity();
+ B_.setIdentity();
+ }
+
+
+ const state_matrix_t& getDerivativeState(const core::StateVector<state_dim>& x,
+ const core::ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ return A_;
+ }
+
+ const state_control_matrix_t& getDerivativeControl(const core::StateVector<state_dim>& x,
+ const core::ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ return B_;
+ }
+
+ MIMOIntegratorLinear* clone() const override { return new MIMOIntegratorLinear(); };
+};
+
+
+template <size_t state_dim, size_t control_dim>
+std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> createMIMOIntegratorCostFunction(
+ const core::StateVector<state_dim>& x_final)
+{
+ Eigen::Matrix<double, state_dim, state_dim> Q;
+ Q.setIdentity();
+
+ Eigen::Matrix<double, control_dim, control_dim> R;
+ R.setIdentity();
+
+ Eigen::Matrix<double, state_dim, 1> x_nominal = x_final;
+ Eigen::Matrix<double, control_dim, 1> u_nominal;
+ u_nominal.setZero();
+
+ Eigen::Matrix<double, state_dim, state_dim> Q_final;
+ Q_final.setIdentity();
+
+ std::shared_ptr<CostFunctionQuadratic<state_dim, control_dim>> quadraticCostFunction(
+ new CostFunctionQuadraticSimple<state_dim, control_dim>(Q, R, x_nominal, u_nominal, x_final, Q_final));
+
+ return quadraticCostFunction;
+}
+}
+}
+}
+
diff --git a/ct_optcon/test/testSystems/SpringLoadedMass.h b/ct_optcon/test/testSystems/SpringLoadedMass.h
new file mode 100644
index 0000000..ea5f1b2
--- /dev/null
+++ b/ct_optcon/test/testSystems/SpringLoadedMass.h
@@ -0,0 +1,90 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+ **********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace optcon {
+namespace example {
+
+
+//! Dynamics class for the GNMS unit test
+class SpringLoadedMass : public core::ControlledSystem<2, 1>
+{
+public:
+ static const size_t state_dim = 2; // position, velocity
+ static const size_t control_dim = 1; // force
+
+ SpringLoadedMass() : core::ControlledSystem<state_dim, control_dim>(core::SYSTEM_TYPE::SECOND_ORDER) {}
+ void computeControlledDynamics(const core::StateVector<state_dim>& state,
+ const core::Time& t,
+ const core::ControlVector<control_dim>& control,
+ core::StateVector<state_dim>& derivative) override
+ {
+ derivative(0) = state(1);
+ derivative(1) = control(0) - kStiffness * state(0) + 0.1; // mass is 1 kg
+ }
+
+ SpringLoadedMass* clone() const override { return new SpringLoadedMass(); };
+ static constexpr double kStiffness = 10;
+};
+
+//! Linear system class for the GNMS unit test
+class SpringLoadedMassLinear : public core::LinearSystem<2, 1>
+{
+public:
+ static const size_t state_dim = 2; // position, velocity
+ static const size_t control_dim = 1; // force
+
+ static constexpr double kStiffness = 10;
+
+ state_matrix_t A_;
+ state_control_matrix_t B_;
+
+
+ const state_matrix_t& getDerivativeState(const core::StateVector<state_dim>& x,
+ const core::ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ A_ << 0, 1, -kStiffness, 0;
+ return A_;
+ }
+
+ const state_control_matrix_t& getDerivativeControl(const core::StateVector<state_dim>& x,
+ const core::ControlVector<control_dim>& u,
+ const double t = 0.0) override
+ {
+ B_ << 0, 1;
+ return B_;
+ }
+
+ SpringLoadedMassLinear* clone() const override { return new SpringLoadedMassLinear(); };
+};
+
+
+std::shared_ptr<CostFunctionQuadratic<2, 1>> createSpringLoadedMassCostFunction(const core::StateVector<2>& x_final)
+{
+ Eigen::Matrix<double, 2, 2> Q;
+ Q << 1.0, 0, 0, 1.0;
+
+ Eigen::Matrix<double, 1, 1> R;
+ R << 1.0;
+
+ Eigen::Matrix<double, 2, 1> x_nominal = x_final;
+ Eigen::Matrix<double, 1, 1> u_nominal;
+ u_nominal.setZero();
+
+ Eigen::Matrix<double, 2, 2> Q_final;
+ Q_final << 10.0, 0, 0, 10.0;
+
+ std::shared_ptr<CostFunctionQuadratic<2, 1>> quadraticCostFunction(
+ new CostFunctionQuadraticSimple<2, 1>(Q, R, x_nominal, u_nominal, x_final, Q_final));
+
+ return quadraticCostFunction;
+}
+}
+}
+}
diff --git a/ct_optcon/test/testSystems/costDiehlSystem.info b/ct_optcon/test/testSystems/costDiehlSystem.info
new file mode 100644
index 0000000..9d6983b
--- /dev/null
+++ b/ct_optcon/test/testSystems/costDiehlSystem.info
@@ -0,0 +1,65 @@
+term0
+{
+ name "intermediate cost"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ (0,0) 1.0
+ }
+ R
+ {
+ (0,0) 1.0
+ }
+
+ x_des
+ {
+ (0,0) 0.0
+ }
+ }
+}
+
+
+term1
+{
+ name "final cost"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ (0,0) 10.0
+ }
+
+ x_des
+ {
+ (0,0) 0.0
+ }
+ }
+}
+
+
+
+; initial position
+x_0
+{
+ (0,0) 0.05
+}
+
+
+K_init
+{
+ (0,0) 0
+}
+
+
+
+
+