diff --git a/README.md b/README.md index fe586f98c..c08222b81 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,7 @@ The CT was designed with the following features in mind: - solve large scale optimal control problems in MPC fashion. - **Robot Modelling, Rigid Body Kinematics and Dynamics**: - - straight-forward interface to the state-of the art rigid body dynamics modelling tool RobCoGen. + - straight-forward interface to the state-of the art rigid body dynamics modelling tool [RobCoGen][robcogen-url]. - implementation of a basic nonlinear-programming inverse kinematics solver for fix-base robots. - **Automatic Differentiation**: @@ -196,9 +196,10 @@ The four different main modules are detailed in the following. - Operational Space Controllers - Basic soft **auto-differentiable contact model** for arbitrary frames (ct::rbd::EEContactModel) - **Actuator dynamics** (ct::rbd::ActuatorDynamics) - - Backend uses RobCoGen \cite frigerioCodeGen, + - Backend uses [RobCoGen][robcogen-url], a highly efficient Rigid Body Dynamics library +[robcogen-url]: https://robcogenteam.bitbucket.io/ ### ct_models diff --git a/ct/CMakeLists.txt b/ct/CMakeLists.txt index 0df155c5c..8890c7609 100644 --- a/ct/CMakeLists.txt +++ b/ct/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.14.7) +cmake_minimum_required(VERSION 3.5) project(ct VERSION 3.0.2 LANGUAGES CXX) #Make sure metapackage does not fail when building documentation diff --git a/ct/cmake/explicitTemplateHelpers.cmake b/ct/cmake/explicitTemplateHelpers.cmake index e9a2bdee3..a6c4c6635 100644 --- a/ct/cmake/explicitTemplateHelpers.cmake +++ b/ct/cmake/explicitTemplateHelpers.cmake @@ -1,21 +1,30 @@ # reads the templates from file function(ct_getDimensionsFromLine LineContent) - string(REGEX REPLACE "," ";" LineContent ${LineContent}) + #message(WARNING "received line content: \n " ${LineContent}) + + STRING(REGEX REPLACE ";" "\\\\;" contents "${contents}") + STRING(REGEX REPLACE "\n" ";" LineContent "${LineContent}") + + foreach(NameAndValue ${LineContent}) - foreach(NameAndValue ${LineContent}) # Strip leading spaces string(REGEX REPLACE "^[ ]+" "" NameAndValue ${NameAndValue}) # Find variable name string(REGEX MATCH "^[^=]+" Name ${NameAndValue}) # Find the value string(REPLACE "${Name}=" "" Value ${NameAndValue}) - - # Set the variable + # strip unnecessaray white space from Value + string(REGEX REPLACE " " "" Value ${Value}) + + # Set the variables + if(Name STREQUAL "MANIFOLD") + set(MANIFOLD_PRESPEC "${Value}" PARENT_SCOPE) + endif() + if(Name STREQUAL "STATE_DIM") set(STATE_DIM_PRESPEC "${Value}" PARENT_SCOPE) endif() - # Set the variable if(Name STREQUAL "CONTROL_DIM") set(CONTROL_DIM_PRESPEC "${Value}" PARENT_SCOPE) endif() @@ -32,7 +41,6 @@ function(ct_getDimensionsFromLine LineContent) set(VEL_DIM_PRESPEC "${Value}" PARENT_SCOPE) endif() endforeach() - endfunction() @@ -42,12 +50,22 @@ function(ct_configure_explicit_templates ConfigFile ConfigDir LibPrefix) FILE(READ "${ConfigFile}" contents) STRING(REGEX REPLACE ";" "\\\\;" contents "${contents}") - STRING(REGEX REPLACE "\n" ";" contents "${contents}") + STRING(REGEX REPLACE "\n\n" ";" contents "${contents}") - #message(WARNING "file content: ${contents}") + #message(WARNING "loaded file content: \n ${contents}") foreach(line ${contents}) - #message(WARNING "extracting variables from line: ${line}") + + # check if line contains comments (starting with #) + string(FIND ${line} "#" _idx_comment_start) + # if comment found, skip that line + if(${_idx_comment_start} GREATER -1) + message(ERROR " Found a comment '#' in explicit template spec -- please remove it, comments are not (yet) supported in prespec configs.") + continue() + endif() + + #message(WARNING "extracting variables from line:\n ${line}") + set(MANIFOLD_PRESPEC "") set(STATE_DIM_PRESPEC "") set(CONTROL_DIM_PRESPEC "") set(SCALAR_PRESPEC "") @@ -57,19 +75,23 @@ function(ct_configure_explicit_templates ConfigFile ConfigDir LibPrefix) ct_getDimensionsFromLine(${line}) - #message(WARNING "extracted: STATE_DIM=${STATE_DIM_PRESPEC}, CONTROL_DIM=${STATE_DIM_PRESPEC}, SCALAR=${SCALAR_PRESPEC}") + #message(WARNING "extracted: MANIFOLD=${MANIFOLD_PRESPEC} STATE_DIM=${STATE_DIM_PRESPEC}, CONTROL_DIM=${CONTROL_DIM_PRESPEC}, SCALAR=${SCALAR_PRESPEC}") string(REGEX REPLACE "[^0-9a-zA-Z]+" "" SCALAR_PRESPEC_CLEAN ${SCALAR_PRESPEC}) - set(CURRENT_LIB_NAME "${LibPrefix}-${STATE_DIM_PRESPEC}-${CONTROL_DIM_PRESPEC}-${SCALAR_PRESPEC_CLEAN}-${POS_DIM_PRESPEC}-${VEL_DIM_PRESPEC}") - - if(STATE_DIM_PRESPEC AND CONTROL_DIM_PRESPEC AND SCALAR_PRESPEC) - #message(WARNING "Will configure now") - ct_configureFiles(${ConfigDir} ${STATE_DIM_PRESPEC}, ${CONTROL_DIM_PRESPEC}, ${SCALAR_PRESPEC} ${POS_DIM_PRESPEC} ${VEL_DIM_PRESPEC}) + string(REGEX REPLACE "[^0-9a-zA-Z]+" "" MANIFOLD_PRESPEC_CLEAN ${MANIFOLD_PRESPEC}) + + set(CURRENT_LIB_NAME "${LibPrefix}-${MANIFOLD_PRESPEC_CLEAN}-${CONTROL_DIM_PRESPEC}-${SCALAR_PRESPEC_CLEAN}-${POS_DIM_PRESPEC}-${VEL_DIM_PRESPEC}") + #message(WARNING "Current lib name: \n ${CURRENT_LIB_NAME}") + + if(MANIFOLD_PRESPEC AND CONTROL_DIM_PRESPEC AND SCALAR_PRESPEC) + message(WARNING "Will configure now") + ct_configureFiles(${ConfigDir} ${MANIFOLD_PRESPEC}, ${CONTROL_DIM_PRESPEC}, ${SCALAR_PRESPEC} ${POS_DIM_PRESPEC} ${VEL_DIM_PRESPEC}) elseif() #message(WARNING "Nothing to configure") endif() if(CURRENT_SRCS) + #message(WARNING "the current sources are \n ${CURRENT_SRCS}") set(${CURRENT_LIB_NAME}_SRCS ${CURRENT_SRCS} PARENT_SCOPE) list(APPEND LIB_NAMES "${CURRENT_LIB_NAME}") endif() @@ -81,15 +103,16 @@ endfunction() # finds cpp.in files and configures them -function(ct_configureFiles ConfigDir STATE_DIM_PRESPEC, CONTROL_DIM_PRESPEC, SCALAR_PRESPEC, POS_DIM_PREPSEC, VEL_DIM_PRESPEC) +function(ct_configureFiles ConfigDir MANIFOLD_PRESPEC, CONTROL_DIM_PRESPEC, SCALAR_PRESPEC, POS_DIM_PREPSEC, VEL_DIM_PRESPEC) set(CURRENT_SRCS "") file(GLOB_RECURSE files "${ConfigDir}*.in") #message(WARNING "files to configure in directory ${ConfigDir}:\n ${files}") foreach(file ${files}) + string(REGEX REPLACE "[^0-9a-zA-Z]+" "" MANIFOLD_PRESPEC_CLEAN ${MANIFOLD_PRESPEC}) string(REGEX REPLACE "[^0-9a-zA-Z]+" "" SCALAR_PRESPEC_CLEAN ${SCALAR_PRESPEC}) string(REPLACE "${CMAKE_CURRENT_SOURCE_DIR}" "${CMAKE_CURRENT_BINARY_DIR}" outputFile ${file}) string(REPLACE ".cpp.in" "" outputFile ${outputFile}) - set(outputFile "${outputFile}-${STATE_DIM_PRESPEC}-${CONTROL_DIM_PRESPEC}-${SCALAR_PRESPEC_CLEAN}-${POS_DIM_PRESPEC}-${VEL_DIM_PRESPEC}.cpp") + set(outputFile "${outputFile}-${MANIFOLD_PRESPEC_CLEAN}-${CONTROL_DIM_PRESPEC}-${SCALAR_PRESPEC_CLEAN}-${POS_DIM_PRESPEC}-${VEL_DIM_PRESPEC}.cpp") #message(WARNING "configuring file \n ${file} to \n ${outputFile} ") set(DOUBLE_OR_FLOAT false) if((SCALAR_PRESPEC MATCHES "double") OR (SCALAR_PRESPEC MATCHES "float")) #STREQUAL did not work diff --git a/ct/config/explicit_templates.cfg b/ct/config/explicit_templates.cfg index c9bca24cf..748e7e26c 100644 --- a/ct/config/explicit_templates.cfg +++ b/ct/config/explicit_templates.cfg @@ -1,4 +1,32 @@ -STATE_DIM=2, CONTROL_DIM=1, POS_DIM=1, VEL_DIM=1, SCALAR=double -STATE_DIM=12, CONTROL_DIM=4, POS_DIM=6, VEL_DIM=6, SCALAR=double +MANIFOLD=ct::core::EuclideanState<2, double> +CONTROL_DIM=1 +POS_DIM=1 +VEL_DIM=1 +SCALAR=double + +MANIFOLD=ct::core::ManifoldState +CONTROL_DIM=6 +POS_DIM=6 +VEL_DIM=6 +SCALAR=double + +MANIFOLD=ct::core::ManifoldState ## todo this case should be covered +CONTROL_DIM=3 +POS_DIM=3 +VEL_DIM=3 +SCALAR=double + +#MANIFOLD=ManifoldState ## todo this case should be covered +#CONTROL_DIM=3 +#POS_DIM=3 +#VEL_DIM=3 +#SCALAR=double + +# todo rewrite all of this +#STATE_DIM=2, CONTROL_DIM=1, POS_DIM=1, VEL_DIM=1, SCALAR=double +#STATE_DIM=12, CONTROL_DIM=4, POS_DIM=6, VEL_DIM=6, SCALAR=double #STATE_DIM=12, CONTROL_DIM=6, POS_DIM=6, VEL_DIM=6, SCALAR=double #STATE_DIM=2, CONTROL_DIM=2, POS_DIM=1, VEL_DIM=1, SCALAR=ct::core::ADScalar + + + diff --git a/ct_core/CMakeLists.txt b/ct_core/CMakeLists.txt index 89840ce8f..139453ca3 100755 --- a/ct_core/CMakeLists.txt +++ b/ct_core/CMakeLists.txt @@ -1,18 +1,20 @@ -cmake_minimum_required (VERSION 3.14.7) +cmake_minimum_required (VERSION 3.5) 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_core VERSION 3.0.2 LANGUAGES CXX) - set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -std=c++14 -Wall -Wfatal-errors") set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pthread -std=c++14 -Wall -Wfatal-errors") set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE) +option(CT_DISABLE_NUMERIC_MANIFOLD_CHECKS off) +if(CT_DISABLE_NUMERIC_MANIFOLD_CHECKS) + list(APPEND ct_core_COMPILE_DEFINITIONS CT_DISABLE_NUMERIC_MANIFOLD_CHECKS) +endif() set(ct_core_LIBS "") @@ -21,6 +23,13 @@ find_package(Eigen3 REQUIRED) find_package(Boost COMPONENTS REQUIRED) ## find and include optional dependencies +find_package(manif QUIET) +if(manif_FOUND) + message(STATUS "Found manif") + list(APPEND ct_core_COMPILE_DEFINITIONS CT_USE_MANIF) + list(APPEND ct_core_target_include_dirs ${manif_INCLUDE_DIRS}) +endif(manif_FOUND) + find_package(llvm QUIET) find_package(clang QUIET) if(LLVM_FOUND AND CLANG_FOUND) @@ -63,18 +72,30 @@ else() message(STATUS "COMPILING WITHOUT QWT") endif() -find_package(Python COMPONENTS Development NumPy QUIET) -if (Python_FOUND AND ${Python_VERSION_MAJOR} EQUAL 3) - message(STATUS "Found python " ${Python_VERSION}) - list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED) - set(PLOTTING_ENABLED ON) - message(STATUS "Python library path ... " ${Python_LIBRARIES}) - list(APPEND ct_core_LIBS ${Python_LIBRARIES}) +if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + find_package(PythonLibs 3 QUIET) + if (PYTHONLIBS_FOUND) + message(STATUS "Found python 3") + list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED) + set(PLOTTING_ENABLED ON) + message(STATUS "Python library path ... " ${PYTHON_LIBRARY}) + list(APPEND ct_core_LIBS ${PYTHON_LIBRARY}) + else() + message(STATUS "Python 3 not found, plotting will not be available") + endif() else() - message(STATUS "Python 3 not found, plotting will not be available") + find_package(Python COMPONENTS Development NumPy QUIET) + if (Python_FOUND AND ${Python_VERSION_MAJOR} EQUAL 3) + message(STATUS "Found python " ${Python_VERSION}) + list(APPEND ct_core_COMPILE_DEFINITIONS PLOTTING_ENABLED) + set(PLOTTING_ENABLED ON) + message(STATUS "Python library path ... " ${Python_LIBRARIES}) + list(APPEND ct_core_LIBS ${Python_LIBRARIES}) + else() + message(STATUS "Python 3 not found, plotting will not be available") + endif() endif() - ## configure files required for code-generation set(CODEGEN_TEMPLATE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/templates") set(CODEGEN_OUTPUT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/generated") @@ -87,13 +108,18 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/ct/core/templateDir.h.in ${CM ## define the directories to be included in all ct_core targets list(APPEND ct_core_target_include_dirs ${EIGEN3_INCLUDE_DIR}) -list(APPEND ct_core_target_include_dirs ${Python_INCLUDE_DIRS}) -list(APPEND ct_core_target_include_dirs ${Python_NumPy_INCLUDE_DIRS}) +if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + list(APPEND ct_core_target_include_dirs ${PYTHON_INCLUDE_DIRS}) +else() + list(APPEND ct_core_target_include_dirs ${Python_INCLUDE_DIRS}) + list(APPEND ct_core_target_include_dirs ${Python_NumPy_INCLUDE_DIRS}) +endif() list(APPEND ct_core_target_include_dirs ${QWT_INCLUDE_DIR}) list(APPEND ct_core_target_include_dirs $) list(APPEND ct_core_target_include_dirs $) list(APPEND ct_core_target_include_dirs $) + ## declare prespec libraries set(PRESPEC_LIB_NAMES "") @@ -113,7 +139,7 @@ if(USE_PRESPEC) endif() -## create ct_core libraries +# plotting add_library(ct_plot SHARED src/core/plot/plot.cpp) target_include_directories(ct_plot PUBLIC ${ct_core_target_include_dirs}) target_compile_definitions(ct_plot PUBLIC ${ct_core_COMPILE_DEFINITIONS}) @@ -125,12 +151,13 @@ target_compile_definitions(ct_core INTERFACE ${ct_core_COMPILE_DEFINITIONS}) target_link_libraries(ct_core INTERFACE ${ct_core_LIBS} ${PRESPEC_LIB_NAMES} + #ct_core_explicit ct_plot pthread dl # required for gcc compatibility ) - + ################## # BUILD EXAMPLES # ################## @@ -198,4 +225,3 @@ endif() ################# add_subdirectory(doc) - diff --git a/ct_core/examples/include/ct/core/examples/CustomController.h b/ct_core/examples/include/ct/core/examples/CustomController.h index 0f19b7a04..8914da485 100644 --- a/ct_core/examples/include/ct/core/examples/CustomController.h +++ b/ct_core/examples/include/ct/core/examples/CustomController.h @@ -6,30 +6,31 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once /*! - * A simple custom PD controller for an oscillator + * A simple custom PD controller * * see \ref DampedOscillatorCustomController.cpp on how to use it. * * \example CustomController.h */ //! an example controller class that takes a 2-dimensional state and outputs a 1-dimensional control action -class CustomController : public ct::core::Controller<2, 1> +class CustomController : public ct::core::Controller, 1, ct::core::CONTINUOUS_TIME> { public: static const size_t state_dim = 2; // two states static const size_t control_dim = 1; // one control action + using State = ct::core::EuclideanState; + using ControlVector = ct::core::ControlVector; + //! default constructor - CustomController(const ct::core::ControlVector& uff, // feedforward control - const double& kp, // P gain - const double& kd // D gain + CustomController(const ControlVector& uff, // feedforward control + const double& kp, // P gain + const double& kd // D gain ) : uff_(uff), kp_(kp), kd_(kd) { } - //! destructor - ~CustomController() {} //! copy constructor CustomController(const CustomController& other) : uff_(other.uff_), kp_(other.kp_), kd_(other.kd_) {} //! clone method, needs to be implemented, overrides ct::core::Controller::clone() @@ -39,16 +40,14 @@ class CustomController : public ct::core::Controller<2, 1> } //! override the compute control method with a custom control law - void computeControl(const ct::core::StateVector& state, - const double& t, - ct::core::ControlVector& controlAction) override + void computeControl(const State& state, const double& t, ControlVector& controlAction) override { controlAction = uff_; // apply feedforward control controlAction(0) -= kp_ * state(0) + kd_ * state(1); // add feedback control } private: - ct::core::ControlVector uff_; //! feedforward control action - double kp_; //! P gain - double kd_; //! D gain + ControlVector uff_; //! feedforward control action + double kp_; //! P gain + double kd_; //! D gain }; diff --git a/ct_core/examples/include/ct/core/examples/Masspoint.h b/ct_core/examples/include/ct/core/examples/Masspoint.h index 9702d8167..0656a5b82 100644 --- a/ct_core/examples/include/ct/core/examples/Masspoint.h +++ b/ct_core/examples/include/ct/core/examples/Masspoint.h @@ -1,49 +1,50 @@ /*! * \brief This example shows how to define your own system. * - * \note This generates a "System", not a "ControlledSystem". If you wanted to use - * this system for controller design, you should derive from "ControlledSystem" + * The system created here is a simple mass-point second-order dynamics in one dimension. + * Therefore, the state dimension is 2, and the control dimension is 1 + * + * \note This generates a "System" in continuous-time, not a "ControlledSystem". + * If you wanted to use this system for controller design, you should derive from "ControlledSystem" + * instead. * * \example Masspoint.h */ #pragma once -#include // as usual, include CT +#include -// create a class that derives from ct::core::System -class Masspoint : public ct::core::System<2> +// create a class that derives from ct::core::System in continuous time +class Masspoint : public ct::core::System, ct::core::CONTINUOUS_TIME> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - static const size_t STATE_DIM = 2; - // constructor - Masspoint(double mass, double d) : mass_(mass), d_(d) {} + // convenience definitions, with state-space dimension 2. Define control dimension=1 + static constexpr size_t state_dim = 2; + using State = ct::core::EuclideanState; + using StateDerivative = State::Tangent; - // copy constructor + // constructor + Masspoint(const double mass, const double damping) : mass_(mass), d_(damping) {} + // copy constructor (required) Masspoint(const Masspoint& other) : mass_(other.mass_), d_(other.d_) {} - - // destructor - ~Masspoint() = default; - - // clone method for deep copying + // clone method for deep copying (required) Masspoint* clone() const override { return new Masspoint(*this); // calls copy constructor } - // The system dynamics. We override this method which gets called by e.g. the Integrator - void computeDynamics(const ct::core::StateVector& x, - const ct::core::Time& t, - ct::core::StateVector& derivative) override + // The system dynamics. We override this method, which gets called by e.g. the Integrator, + // or by the system linearizers + void computeDynamics(const State& x, const double& time, StateDerivative& dxdt) override { // first part of state derivative is the velocity - derivative(0) = x(1); + dxdt(0) = x(1); // second part is the acceleration which is caused by damper forces - derivative(1) = -d_ / mass_ * x(1); + dxdt(1) = -d_ / mass_ * x(1); } private: diff --git a/ct_core/examples/src/DampedOscillator.cpp b/ct_core/examples/src/DampedOscillator.cpp index 7f5b915db..580bb63b1 100644 --- a/ct_core/examples/src/DampedOscillator.cpp +++ b/ct_core/examples/src/DampedOscillator.cpp @@ -13,7 +13,8 @@ int main(int argc, char** argv) const size_t state_dim = ct::core::SecondOrderSystem::STATE_DIM; // = 2 // create a state - ct::core::StateVector x; + using State = ct::core::EuclideanState; + State x; // we initialize it at a point with unit deflection and zero velocity x(0) = 1.0; @@ -24,7 +25,7 @@ int main(int argc, char** argv) std::shared_ptr oscillator(new ct::core::SecondOrderSystem(w_n)); // create an integrator - ct::core::Integrator integrator(oscillator); + ct::core::Integrator integrator(oscillator); // simulate 1000 steps double dt = 0.001; diff --git a/ct_core/examples/src/DampedOscillatorCustomController.cpp b/ct_core/examples/src/DampedOscillatorCustomController.cpp index de69239c4..95f24ac8d 100644 --- a/ct_core/examples/src/DampedOscillatorCustomController.cpp +++ b/ct_core/examples/src/DampedOscillatorCustomController.cpp @@ -20,7 +20,8 @@ int main(int argc, char** argv) const size_t control_dim = ct::core::SecondOrderSystem::CONTROL_DIM; // = 1 // create a state - ct::core::StateVector x; + using State = ct::core::EuclideanState; + State x; // we initialize it at a point with unit deflection and zero velocity x(0) = 1.0; @@ -41,7 +42,7 @@ int main(int argc, char** argv) oscillator->setController(controller); // create an integrator - ct::core::Integrator integrator(oscillator, ct::core::IntegrationType::RK4); + ct::core::Integrator integrator(oscillator, ct::core::IntegrationType::RK4); // simulate 1000 steps double dt = 0.001; diff --git a/ct_core/examples/src/MasspointIntegration.cpp b/ct_core/examples/src/MasspointIntegration.cpp index 7c059f704..48298a85b 100644 --- a/ct_core/examples/src/MasspointIntegration.cpp +++ b/ct_core/examples/src/MasspointIntegration.cpp @@ -9,13 +9,14 @@ int main(int argc, char** argv) { // a damped oscillator has two states, position and velocity - const size_t state_dim = Masspoint::STATE_DIM; // = 2 + const size_t state_dim = Masspoint::state_dim; // = 2 // create a state - ct::core::StateVector x; + using State = Masspoint::State; + State x; - // we initialize it at 0 - x.setZero(); + // we initialize it at 1 + x << 1, 0; // create our mass point instance double mass = 1.0; @@ -23,7 +24,7 @@ int main(int argc, char** argv) std::shared_ptr masspoint(new Masspoint(mass, d)); // create an integrator - ct::core::Integrator integrator(masspoint); + ct::core::Integrator integrator(masspoint); // simulate 1000 steps double dt = 0.001; diff --git a/ct_core/include/ct/core/Common b/ct_core/include/ct/core/Common index c96582cc1..017bc09b2 100644 --- a/ct_core/include/ct/core/Common +++ b/ct_core/include/ct/core/Common @@ -14,4 +14,7 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include "common/Interpolation.h" #include "common/linspace.h" #include "common/activations/Activations.h" +#include "common/EigenIsNan.h" +#include "common/EigenFileExport.h" + diff --git a/ct_core/include/ct/core/Control b/ct_core/include/ct/core/Control index 906768f02..fc8a9c8f5 100644 --- a/ct_core/include/ct/core/Control +++ b/ct_core/include/ct/core/Control @@ -5,14 +5,11 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once -#include "control/continuous_time/Controller.h" -#include "control/discrete_time/DiscreteController.h" - +#include "control/Controller.h" +#include "control/ConstantController.h" #include "control/StateFeedbackController.h" -#include "control/continuous_time/ConstantController.h" -#include "control/continuous_time/ConstantStateFeedbackController.h" -#include "control/continuous_time/ConstantTrajectoryController.h" -#include "control/continuous_time/siso/PIDController.h" -#include "control/continuous_time/siso/PIDController-impl.h" -#include "control/continuous_time/siso/StepInputController.h" -#include "control/continuous_time/mimo/LinearFunctions.h" + +//#include "control/continuous_time/siso/PIDController.h" +//#include "control/continuous_time/siso/PIDController-impl.h" +//#include "control/continuous_time/siso/StepInputController.h" +//#include "control/continuous_time/mimo/LinearFunctions.h" diff --git a/ct_core/include/ct/core/Control-impl b/ct_core/include/ct/core/Control-impl index 9c7393b62..994c7d77f 100644 --- a/ct_core/include/ct/core/Control-impl +++ b/ct_core/include/ct/core/Control-impl @@ -5,5 +5,5 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once +#include "control/ConstantController-impl.h" #include "control/StateFeedbackController-impl.h" -#include "control/continuous_time/ConstantController-impl.h" diff --git a/ct_core/include/ct/core/Integration b/ct_core/include/ct/core/Integration index 729abf394..1e8b0069c 100644 --- a/ct_core/include/ct/core/Integration +++ b/ct_core/include/ct/core/Integration @@ -5,12 +5,24 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once +#include "integration/internal/StepperBase.h" +#include "integration/internal/StepperCTBase.h" +#include "integration/internal/StepperEulerCT.h" +#include "integration/internal/StepperRK4CT.h" +#include "integration/internal/StepperODEInt.h" +#include "integration/internal/StepperODEIntDenseOutput.h" +#include "integration/internal/StepperODEIntControlled.h" + #include "integration/Observer.h" -#include "integration/Integrator.h" -#include "integration/IntegratorSymplectic.h" + +#include "integration/EventHandler.h" #include "integration/EventHandlers/KillIntegrationEventHandler.h" #include "integration/EventHandlers/MaxStepsEventHandler.h" #include "integration/EventHandlers/SubstepRecorder.h" + +#include "integration/Integrator.h" +#include "integration/IntegratorSymplectic.h" + #include "integration/sensitivity/Sensitivity.h" #include "integration/sensitivity/SensitivityApproximation.h" #include "integration/sensitivity/SensitivityIntegrator.h" diff --git a/ct_core/include/ct/core/Integration-impl b/ct_core/include/ct/core/Integration-impl index 5c23c0dee..264592958 100644 --- a/ct_core/include/ct/core/Integration-impl +++ b/ct_core/include/ct/core/Integration-impl @@ -5,6 +5,18 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once +#include "integration/internal/StepperBase-impl.h" +#include "integration/internal/StepperCTBase-impl.h" +#include "integration/internal/StepperEulerCT-impl.h" +#include "integration/internal/StepperRK4CT-impl.h" +#include "integration/internal/StepperODEInt-impl.h" +#include "integration/internal/StepperODEIntControlled-impl.h" +#include "integration/internal/StepperODEIntDenseOutput-impl.h" + +#include "integration/EventHandlers/KillIntegrationEventHandler-impl.h" +#include "integration/EventHandlers/MaxStepsEventHandler-impl.h" +#include "integration/EventHandlers/SubstepRecorder-impl.h" + #include "integration/Observer-impl.h" #include "integration/Integrator-impl.h" #include "integration/IntegratorSymplectic-impl.h" diff --git a/ct_core/include/ct/core/Internal b/ct_core/include/ct/core/Internal index 7ef7c4059..6abbde1a6 100644 --- a/ct_core/include/ct/core/Internal +++ b/ct_core/include/ct/core/Internal @@ -13,4 +13,5 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include "internal/traits/TraitSelector.h" #include "internal/traits/TraitSelectorSpecs.h" #include "internal/traits/DoubleTrait.h" +#include "internal/traits/EvaluatorOutTypeTraits.h" diff --git a/ct_core/include/ct/core/Simulation-impl b/ct_core/include/ct/core/Simulation-impl deleted file mode 100644 index 589e8bc23..000000000 --- a/ct_core/include/ct/core/Simulation-impl +++ /dev/null @@ -1,8 +0,0 @@ -/********************************************************************************************************************** -This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. -Licensed under the BSD-2 license (see LICENSE file in main directory) -**********************************************************************************************************************/ - -#pragma once - -// put impl files here diff --git a/ct_core/include/ct/core/Systems b/ct_core/include/ct/core/Systems index 4879af7d7..902673511 100644 --- a/ct_core/include/ct/core/Systems +++ b/ct_core/include/ct/core/Systems @@ -5,37 +5,22 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once -#include "systems/continuous_time/System.h" -#include "systems/continuous_time/ControlledSystem.h" -#include "systems/continuous_time/SecondOrderSystem.h" -#include "systems/continuous_time/ControlledSystem.h" -#include "systems/continuous_time/SwitchedControlledSystem.h" -#include "systems/continuous_time/linear/LinearSystem.h" -#include "systems/continuous_time/linear/SwitchedLinearSystem.h" -#include "systems/continuous_time/linear/LTISystem.h" +#include "systems/System.h" +#include "systems/ControlledSystem.h" +#include "systems/SecondOrderSystem.h" +#include "systems/SymplecticSystem.h" +#include "systems/LinearSystem.h" +#include "systems/LTISystem.h" +#include "systems/SwitchedControlledSystem.h" +#include "systems/SwitchedLinearSystem.h" #include "systems/linearizer/DynamicsLinearizerNumDiff.h" +#include "systems/linearizer/SystemLinearizer.h" #ifdef CPPADCG -#include "systems/linearizer/DynamicsLinearizerAD.h" -#include "systems/linearizer/DynamicsLinearizerADCG.h" + #include "systems/linearizer/DynamicsLinearizerAD.h" + #include "systems/linearizer/AutoDiffLinearizer.h" + #include "systems/linearizer/DynamicsLinearizerADCG.h" + #include "systems/linearizer/ADCodegenLinearizer.h" #endif -#include "systems/continuous_time/linear/SystemLinearizer.h" -#ifdef CPPADCG -#include "systems/continuous_time/linear/AutoDiffLinearizer.h" -#include "systems/continuous_time/linear/ADCodegenLinearizer.h" -#endif - -#include "systems/discrete_time/DiscreteControlledSystem.h" -#include "systems/discrete_time/SwitchedDiscreteControlledSystem.h" -#include "systems/discrete_time/SystemDiscretizer.h" - -#include "systems/discrete_time/linear/DiscreteLinearSystem.h" -#include "systems/discrete_time/linear/DiscreteLTISystem.h" -#include "systems/discrete_time/linear/SwitchedDiscreteLinearSystem.h" -#include "systems/discrete_time/linear/DiscreteSystemLinearizer.h" - -#ifdef CPPADCG -#include "systems/discrete_time/linear/DiscreteSystemLinearizerAD.h" -#include "systems/discrete_time/linear/DiscreteSystemLinearizerADCG.h" -#endif +#include "systems/discretizer/SystemDiscretizer.h" diff --git a/ct_core/include/ct/core/Systems-impl b/ct_core/include/ct/core/Systems-impl index c16e5ad27..f59cbcc54 100644 --- a/ct_core/include/ct/core/Systems-impl +++ b/ct_core/include/ct/core/Systems-impl @@ -5,5 +5,8 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once -// put impl files here -#include "systems/discrete_time/SystemDiscretizer-impl.h" +#include "systems/System-impl.h" +#include "systems/ControlledSystem-impl.h" +#include "systems/SecondOrderSystem-impl.h" + +#include "systems/discretizer/SystemDiscretizer-impl.h" diff --git a/ct_core/include/ct/core/Types b/ct_core/include/ct/core/Types index 87f226eac..2e21a2ffb 100644 --- a/ct_core/include/ct/core/Types +++ b/ct_core/include/ct/core/Types @@ -8,8 +8,12 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include "types/Time.h" #include "types/AutoDiff.h" + +#include "types/ManifoldState.h" +#include "types/EuclideanState.h" +#include "types/EuclideanStateSymplectic.h" + #include "types/ControlVector.h" -#include "types/StateVector.h" #include "types/OutputVector.h" #include "types/FeedbackMatrix.h" #include "types/StateMatrix.h" @@ -21,10 +25,14 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include "types/arrays/ScalarArray.h" #include "types/arrays/TimeArray.h" #include "types/arrays/MatrixArrays.h" +#include "types/arrays/ManifArrays.h" #include "types/arrays/ArrayHelpers.h" -#include "types/trajectories/DiscreteTrajectoryBase.h" +#include "types/trajectories/DiscreteTrajectory.h" #include "types/trajectories/ScalarTrajectory.h" #include "types/trajectories/MatrixTrajectories.h" +#include "types/TypeTraits.h" +#include "types/TypeMacros.h" + diff --git a/ct_core/include/ct/core/Types-impl b/ct_core/include/ct/core/Types-impl index 1fd96dfcb..cc9497949 100644 --- a/ct_core/include/ct/core/Types-impl +++ b/ct_core/include/ct/core/Types-impl @@ -5,5 +5,5 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once -// put implementations here +#include "types/ManifoldState-impl.h" diff --git a/ct_core/include/ct/core/common/EigenFileExport.h b/ct_core/include/ct/core/common/EigenFileExport.h new file mode 100644 index 000000000..7d0c9aab2 --- /dev/null +++ b/ct_core/include/ct/core/common/EigenFileExport.h @@ -0,0 +1,132 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#include +#include +#include + +#include + +namespace ct::core { + +// +// @brief Save Eigen-types or vectors of Eigen-types to text file. +// Currently ordinary Eigen matrices and quaternion types are supported. +// +// Usage examples: +// // writing an eigen matrix +// Eigen::Matrix3d m = Eigen::Matrix3d::Random(); +// EigenFileExport::mat_to_file(EigenFileExport::OctaveFormat(), "/tmp/eigen_mat.csv", m, "test mat"); +// +// // writing a vector of eigen matrices +// std::vector> m_vec(3, Eigen::Matrix3d::Random()); +// EigenFileExport::mat_to_file(EigenFileExport::OctaveFormat(), "/tmp/eigen_mat_vec.csv", m_vec, "some title."); +// +// // writing a quaternion +// Eigen::Quaterniond q(Eigen::Quaterniond::Identity()); +// EigenFileExport::quat_to_file(EigenFileExport::OctaveFormat(), "/tmp/quat.csv", q, "test quat"); +// +// // writing a vector of quaternions +// std::vector> q_vec( +// 5, Eigen::Quaterniond::Identity()); +// EigenFileExport::quat_to_file(EigenFileExport::OctaveFormat(), "/tmp/quat_vec.csv", q_vec); +// +class EigenFileExport +{ +public: + // Create CSV formatting option, precision can be custimized if desired. + static const Eigen::IOFormat CSVFormat(int precision = Eigen::StreamPrecision) + { + return Eigen::IOFormat(precision, Eigen::DontAlignCols, ", ", "\n"); + } + // Create Octave formatting option, precision can be custimized if desired. + static const Eigen::IOFormat OctaveFormat(int precision = Eigen::StreamPrecision) + { + return Eigen::IOFormat(precision, 0, ", ", ";\n", "", "", "[", "]"); + } + // Create comma init formatting option, precision can be custimized if desired. + static const Eigen::IOFormat CommaInitFormat(int precision = Eigen::StreamPrecision) + { + return Eigen::IOFormat(precision, Eigen::DontAlignCols, ", ", ", ", "", "", " << ", ";"); + } + // Create a clean formatting option, precision can be custimized if desired. + static const Eigen::IOFormat CleanFormat(int precision = Eigen::StreamPrecision) + { + return Eigen::IOFormat(precision, 0, ", ", "\n", "[", "]"); + } + // create default full-info formatting option, precision can be custimized if desired. + static const Eigen::IOFormat HeavyFormat(int precision = Eigen::FullPrecision) + { + return Eigen::IOFormat(precision, 0, ", ", ";\n", "[", "]", "[", "]"); + } + + //! Save a single matrix to single text file. + template + static void mat_to_file(const Eigen::IOFormat fmt, + const std::string& filename, + const Eigen::MatrixBase& m, + const std::string& heading = "") + { + std::ofstream file(filename.c_str()); + if (!heading.empty()) + file << heading << "\n"; + file << m.format(fmt); + + file.close(); + } + + //! Save a single quaternion to a single text file. + template + static void quat_to_file(const Eigen::IOFormat fmt, + const std::string& filename, + const Eigen::QuaternionBase& q, + const std::string& heading = "") + { + std::ofstream file(filename.c_str()); + if (!heading.empty()) + file << heading << "\n"; + file << q.coeffs().transpose().format(fmt); + + file.close(); + } + + // Save a vector of matrices to a single text file. + template > + static void mat_to_file(const Eigen::IOFormat fmt, + const std::string& filename, + const std::vector& m_vec, + const std::string& heading = "") + { + std::ofstream file(filename.c_str()); + + if (!heading.empty()) + file << heading << "\n"; + + for (size_t i = 0; i < m_vec.size(); i++) + file << m_vec[i].format(fmt) << "\n"; + + file.close(); + } + + // Save a vector of quaternions to a single text file. + template >> + static void quat_to_file(const Eigen::IOFormat fmt, + const std::string& filename, + const std::vector, Alloc>& q_vec, + const std::string& heading = "") + { + std::ofstream file(filename.c_str()); + + if (!heading.empty()) + file << heading << "\n"; + + for (size_t i = 0; i < q_vec.size(); i++) + file << q_vec[i].coeffs().transpose().format(fmt) << "\n"; + + file.close(); + } +}; + +} // namespace ct::core diff --git a/ct_core/include/ct/core/common/EigenIsNan.h b/ct_core/include/ct/core/common/EigenIsNan.h new file mode 100644 index 000000000..811a28c7d --- /dev/null +++ b/ct_core/include/ct/core/common/EigenIsNan.h @@ -0,0 +1,19 @@ +#pragma once + +#include +#include + +namespace ct { +namespace core { + +/* + * \brief returns true if an eigen-type is nan + */ +template +inline bool eigen_is_nan(const Eigen::MatrixBase& x) +{ + return ((x.array() != x.array())).all(); +} + +} // namespace core +} // namespace ct \ No newline at end of file diff --git a/ct_core/include/ct/core/common/Interpolation.h b/ct_core/include/ct/core/common/Interpolation.h index 98dd00026..4b887be64 100644 --- a/ct_core/include/ct/core/common/Interpolation.h +++ b/ct_core/include/ct/core/common/Interpolation.h @@ -44,18 +44,15 @@ class Interpolation //! Copy Constructor Interpolation(const Interpolation& arg) : index_(arg.index_), type_(arg.type_) {} //! This method performs the interpolation - /*! - * @param timeArray timing information of the data points - * @param dataArray the data points in form of a DiscreteArray - * @param enquiryTime the time where to evaluate the interpolation - * @param enquiryData the result of the interpolation - * @param greatestLessTimeStampIndex the smallest index corresponding to a time smaller than the inquired Time - */ + + + template void interpolate(const tpl::TimeArray& timeArray, const DiscreteArray_t& dataArray, const SCALAR& enquiryTime, Data_T& enquiryData, - int greatestLessTimeStampIndex = -1) + LAMBDA&& interp_functional, + const int greatestLessTimeStampIndex = -1) { if (timeArray.size() == 0) throw std::runtime_error("Interpolation.h : TimeArray is size 0."); @@ -78,11 +75,13 @@ class Interpolation int ind; if (greatestLessTimeStampIndex == -1) - ind = findIndex(timeArray, enquiryTime); + { + ind = this->findIndex(timeArray, enquiryTime); + } else { - ind = greatestLessTimeStampIndex; - index_ = greatestLessTimeStampIndex; + this->index_ = greatestLessTimeStampIndex; + ind = this->findIndex(timeArray, enquiryTime); } if (enquiryTime < timeArray.front()) @@ -99,11 +98,29 @@ class Interpolation SCALAR alpha = (enquiryTime - timeArray.at(ind + 1)) / (timeArray.at(ind) - timeArray.at(ind + 1)); + // perform the actual evaluation of the interpolation + enquiryData = std::forward(interp_functional)(dataArray.at(ind), dataArray.at(ind + 1), alpha); + } + - if (type_ == InterpolationType::LIN) - enquiryData = alpha * dataArray.at(ind) + (1 - alpha) * dataArray.at(ind + 1); - else if (type_ == InterpolationType::ZOH) - enquiryData = dataArray.at(ind); + void interpolate(const tpl::TimeArray& timeArray, + const DiscreteArray_t& dataArray, + const SCALAR& enquiryTime, + Data_T& enquiryData, + const int greatestLessTimeStampIndex = -1) + { + // define elementary lambda routines for interpolation between two data points + // ZOH-interpolation + auto interpolate_ZOH = [](const auto& p0, const auto& p1, const auto& alpha) { return p0; }; + // LIN-interpolation + auto interpolate_LIN = [](const auto& p0, const auto& p1, const auto& alpha) { + return p1 + alpha * (p0 - p1); // manif-compatible notation + }; + + if (this->type_ == InterpolationType::LIN) + interpolate(timeArray, dataArray, enquiryTime, enquiryData, interpolate_LIN, greatestLessTimeStampIndex); + else if (this->type_ == InterpolationType::ZOH) + interpolate(timeArray, dataArray, enquiryTime, enquiryData, interpolate_ZOH, greatestLessTimeStampIndex); else throw std::runtime_error("Unknown Interpolation type!"); } @@ -163,5 +180,5 @@ class Interpolation }; -} // namespace ct } // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/common/linspace.h b/ct_core/include/ct/core/common/linspace.h index b8271c9dd..6715bb771 100644 --- a/ct_core/include/ct/core/common/linspace.h +++ b/ct_core/include/ct/core/common/linspace.h @@ -5,35 +5,36 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once +#include +#include + namespace ct { namespace core { //! replicates the well-known linspace command from MATLAB in C++ /*! - * linspace provides exactly the same properties and functionality like in MATLAB. - * - * - N denotes the number of points, so you will obtain N-1 intervals - * - a is the start of the interval - * - b is the end of the interval - * - * Unit test \ref LinspaceTest.cpp illustrates the use of linspace. - * - * */ -template -TRAJECTORY_T linspace(const typename TRAJECTORY_T::value_type& a, - const typename TRAJECTORY_T::value_type& b, - const size_t N) + * linspace provides exactly the same properties and functionality like in MATLAB. + * + * - N denotes the number of points, so you will obtain N-1 intervals + * - a is the start of the interval + * - b is the end of the interval + * + * Unit test \ref LinspaceTest.cpp illustrates the use of linspace. + * + */ +template +ARRAY_T linspace(const typename ARRAY_T::value_type& a, const typename ARRAY_T::value_type& b, const size_t N) { if (N < 1) throw std::runtime_error("ERROR in CT_LINSPACE: N<1."); - typename TRAJECTORY_T::value_type h = (b - a) / (N - 1); - TRAJECTORY_T traj(N); + auto h = (b - a) / (N - 1); + ARRAY_T traj(N); - typename TRAJECTORY_T::iterator it; - typename TRAJECTORY_T::value_type val; + typename ARRAY_T::iterator it; + typename ARRAY_T::value_type val; - for (it = traj.begin(), val = a; it != traj.end(); ++it, val += h) + for (it = traj.begin(), val = a; it != traj.end(); ++it, val = val + h) *it = val; return traj; diff --git a/ct_core/include/ct/core/control/ConstantController-impl.h b/ct_core/include/ct/core/control/ConstantController-impl.h new file mode 100644 index 000000000..fcee49843 --- /dev/null +++ b/ct_core/include/ct/core/control/ConstantController-impl.h @@ -0,0 +1,61 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +namespace ct { +namespace core { + +template +ConstantController::ConstantController() +{ + u_.setZero(); +} + +template +ConstantController::ConstantController(control_vector_t& u) : Base(), u_(u) +{ +} + +template +ConstantController::ConstantController(const ConstantController& other) + : Base(other), u_(other.u_) +{ +} + +template +ConstantController* ConstantController::clone() const +{ + return new ConstantController(*this); +} + +template +void ConstantController::computeControl(const MANIFOLD& state, + const Time_t& tn, + control_vector_t& u) +{ + u = u_; +} + +template +void ConstantController::setControl(const control_vector_t& u) +{ + u_ = u; +} + +template +auto ConstantController::getControl() const -> const control_vector_t& +{ + return u_; +} + +template +auto ConstantController::getDerivativeU0(const MANIFOLD& state, + const Time_t tn) -> ControlMatrix +{ + return ControlMatrix::Identity(); +} +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/control/continuous_time/ConstantController.h b/ct_core/include/ct/core/control/ConstantController.h similarity index 62% rename from ct_core/include/ct/core/control/continuous_time/ConstantController.h rename to ct_core/include/ct/core/control/ConstantController.h index 01c4ecace..e322663dd 100644 --- a/ct_core/include/ct/core/control/continuous_time/ConstantController.h +++ b/ct_core/include/ct/core/control/ConstantController.h @@ -16,13 +16,17 @@ namespace core { * This class is useful to integrate a ControlledSystem forward subject to a * constant control input. */ -template -class ConstantController : public Controller, - public DiscreteController +template +class ConstantController final : public Controller { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using SCALAR = typename MANIFOLD::Scalar; + using Base = Controller; + using Time_t = typename Base::Time_t; + using control_vector_t = typename Base::control_vector_t; + //! Default constructor /*! * Sets the control signal to zero @@ -34,20 +38,20 @@ class ConstantController : public Controller, * Initializes the control to a fixed value * @param u The fixed control signal */ - ConstantController(ControlVector& u); + ConstantController(control_vector_t& u); //! Copy constructor - ConstantController(const ConstantController& other); + ConstantController(const ConstantController& other); //! Destructor - virtual ~ConstantController(); + virtual ~ConstantController() = default; //! Clone operator /*! * Clones the controller. Used for cloning ControlledSystem's * @return pointer to cloned controller */ - ConstantController* clone() const override; + ConstantController* clone() const override; //! Computes current control /*! @@ -57,34 +61,26 @@ class ConstantController : public Controller, * @param t The time of the system (ignored) * @param controlAction The (fixed) control action */ - void computeControl(const StateVector& state, - const SCALAR& t, - ControlVector& controlAction) override; - - //! Equivalent method for discrete version - void computeControl(const StateVector& state, - const int n, - ControlVector& controlAction) override; + void computeControl(const MANIFOLD& state, const Time_t& tn, control_vector_t& controlAction) override; //! Sets the control signal /*! * * @param u The fixed control signal */ - void setControl(const ControlVector& u); + void setControl(const control_vector_t& u); //! Get the fixed control signal /*! * * @param u The control input to write the signal to. */ - const ControlVector& getControl() const; + const control_vector_t& getControl() const; - virtual ControlMatrix getDerivativeU0(const StateVector& state, - const SCALAR time) override; + virtual ControlMatrix getDerivativeU0(const MANIFOLD& state, const Time_t tn) override; private: - ControlVector u_; + control_vector_t u_; }; } // namespace core } // namespace ct diff --git a/ct_core/include/ct/core/control/continuous_time/Controller.h b/ct_core/include/ct/core/control/Controller.h similarity index 77% rename from ct_core/include/ct/core/control/continuous_time/Controller.h rename to ct_core/include/ct/core/control/Controller.h index 633916c0b..a8e0e3490 100644 --- a/ct_core/include/ct/core/control/continuous_time/Controller.h +++ b/ct_core/include/ct/core/control/Controller.h @@ -7,12 +7,9 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include -#include -#include #include #include - namespace ct { namespace core { @@ -22,20 +19,25 @@ namespace core { * ControlledSystem. Any custom controller should derive from this class * to ensure it is compatible with ControlledSystem and the Integrator. */ -template +template class Controller { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - //! Default constructor - Controller(){}; + + using SCALAR = typename MANIFOLD::Scalar; + using control_vector_t = ControlVector; + using control_matrix_t = ControlMatrix; + + // determin Time_t to be either scalar or int + using Time_t = typename std::conditional_t; + + Controller() = default; + virtual ~Controller() = default; //! Copy constructor Controller(const Controller& other){}; - //! Destructor - virtual ~Controller(){}; - //! Deep cloning /*! * Has to be implemented by any custom controller. @@ -53,9 +55,7 @@ class Controller * @param t current time of the system * @param controlAction the corresponding control action */ - virtual void computeControl(const StateVector& state, - const SCALAR& t, - ControlVector& controlAction) = 0; + virtual void computeControl(const MANIFOLD& state, const Time_t& t, control_vector_t& u) = 0; /** * @brief Returns the the derivative of the control with respect to the @@ -66,8 +66,7 @@ class Controller * * @return The derivatives with respect to u0. */ - virtual ControlMatrix getDerivativeU0(const StateVector& state, - const SCALAR time) + virtual control_matrix_t getDerivativeU0(const MANIFOLD& state, const Time_t time) { throw std::runtime_error("getDerivativeU0() not implemented for the current controller"); } @@ -81,8 +80,7 @@ class Controller * * @return The derivatives with respect to uF. */ - virtual ControlMatrix getDerivativeUf(const StateVector& state, - const SCALAR time) + virtual control_matrix_t getDerivativeUf(const MANIFOLD& state, const Time_t time) { throw std::runtime_error("getDerivativeUf() not implemented for the current controller"); } diff --git a/ct_core/include/ct/core/control/StateFeedbackController-impl.h b/ct_core/include/ct/core/control/StateFeedbackController-impl.h index ef6930f59..fcb6e61ee 100644 --- a/ct_core/include/ct/core/control/StateFeedbackController-impl.h +++ b/ct_core/include/ct/core/control/StateFeedbackController-impl.h @@ -6,43 +6,49 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) namespace ct { namespace core { -template -StateFeedbackController::StateFeedbackController( - const StateFeedbackController& other) - : ContinuousBase(), DiscreteBase(), x_ref_(other.x_ref_), uff_(other.uff_), K_(other.K_) +template +StateFeedbackController::StateFeedbackController() : Base() { } -template -StateFeedbackController::StateFeedbackController( - const StateVectorArray& x_ref, - const ControlVectorArray& uff, - const FeedbackArray& K, - const SCALAR deltaT, - const SCALAR t0, - const InterpolationType& intType) - : x_ref_(x_ref, deltaT, t0, intType), uff_(uff, deltaT, t0, intType), K_(K, deltaT, t0, intType) +template +StateFeedbackController::~StateFeedbackController() +{ +} + +template +StateFeedbackController::StateFeedbackController(const MANIFOLD& x_ref, + const ct::core::ControlVector& uff, + const ct::core::FeedbackMatrix& K) + : StateFeedbackController(ct::core::DiscreteArray(1, x_ref), + control_vector_array_t(1, uff), + FeedbackArray(1, K), + 1.0, + 0, + ZOH) { } -template -void StateFeedbackController::computeControl(const state_vector_t& state, - const SCALAR& t, - control_vector_t& controlAction) +template +StateFeedbackController::StateFeedbackController( + const StateFeedbackController& other) + : Base(), x_ref_(other.x_ref_), uff_(other.uff_), K_(other.K_) { - controlAction = uff_.eval(t) + K_.eval(t) * (state - x_ref_.eval(t)); } -template -void StateFeedbackController::computeControl(const state_vector_t& state, - const int n, - control_vector_t& controlAction) +template +StateFeedbackController::StateFeedbackController(const DiscreteArray& x_ref, + const ControlVectorArray& uff, + const FeedbackArray& K, + const SCALAR deltaT, + const SCALAR t0, + const InterpolationType& intType) + : x_ref_(x_ref, deltaT, t0, intType), uff_(uff, deltaT, t0, intType), K_(K, deltaT, t0, intType) { - controlAction = uff_[n] + K_[n] * (state - x_ref_[n]); } -template -void StateFeedbackController::update(const DiscreteArray& x_ref, +template +void StateFeedbackController::update(const DiscreteArray& x_ref, const DiscreteArray& uff, const DiscreteArray>& K, const tpl::TimeArray& t) @@ -63,87 +69,91 @@ void StateFeedbackController::update(const Discr K_.setTime(tshort); } - -template -StateFeedbackController* -StateFeedbackController::clone() const +template +void StateFeedbackController::computeControl(const MANIFOLD& state, + const Time_t& tn, + control_vector_t& u) { - return new StateFeedbackController(*this); + computeControl_specialized(state, tn, u); } +template +StateFeedbackController* StateFeedbackController::clone() + const +{ + return new StateFeedbackController(*this); +} -template -const DiscreteArray::state_vector_t>& -StateFeedbackController::x_ref() const +template +const DiscreteArray& StateFeedbackController::x_ref() const { return x_ref_.getDataArray(); } -template -const DiscreteArray::control_vector_t>& -StateFeedbackController::uff() const +template +const DiscreteArray::control_vector_t>& +StateFeedbackController::uff() const { return uff_.getDataArray(); } -template -const DiscreteArray>& -StateFeedbackController::K() const +template +auto StateFeedbackController::K() const + -> const DiscreteArray>& { return K_.getDataArray(); } -template -const tpl::TimeArray& StateFeedbackController::time() const +template +auto StateFeedbackController::time() const -> const tpl::TimeArray& { return x_ref_.getTimeArray(); } -template -StateTrajectory& -StateFeedbackController::getReferenceStateTrajectory() +template +DiscreteTrajectory& StateFeedbackController::getReferenceStateTrajectory() { return x_ref_; } -template -const StateTrajectory& -StateFeedbackController::getReferenceStateTrajectory() const +template +auto StateFeedbackController::getReferenceStateTrajectory() const + -> const DiscreteTrajectory& { return x_ref_; } -template -ControlTrajectory& -StateFeedbackController::getFeedforwardTrajectory() +template +auto StateFeedbackController::getFeedforwardTrajectory() + -> ControlTrajectory& { return uff_; } -template -const ControlTrajectory& -StateFeedbackController::getFeedforwardTrajectory() const +template +auto StateFeedbackController::getFeedforwardTrajectory() const + -> const ControlTrajectory& { return uff_; } -template -FeedbackTrajectory& -StateFeedbackController::getFeedbackTrajectory() +template +auto StateFeedbackController::getFeedbackTrajectory() + -> FeedbackTrajectory& { return K_; } -template -const FeedbackTrajectory& -StateFeedbackController::getFeedbackTrajectory() const +template +auto StateFeedbackController::getFeedbackTrajectory() const + -> const FeedbackTrajectory& { return K_; } -template -void StateFeedbackController::extractControlTrajectory( - const StateTrajectory& x_traj, +template +void StateFeedbackController::extractControlTrajectory( + const DiscreteTrajectory& x_traj, ControlTrajectory& u_traj) { u_traj.clear(); diff --git a/ct_core/include/ct/core/control/StateFeedbackController.h b/ct_core/include/ct/core/control/StateFeedbackController.h index d04ce7bf8..3478e3148 100644 --- a/ct_core/include/ct/core/control/StateFeedbackController.h +++ b/ct_core/include/ct/core/control/StateFeedbackController.h @@ -5,6 +5,9 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once +#include "Controller.h" +#include + namespace ct { namespace core { @@ -35,27 +38,25 @@ namespace core { * @tparam CONTROL_DIM control vector size * @tparam SCALAR primitive type */ -template -class StateFeedbackController : public Controller, - public DiscreteController +template +class StateFeedbackController : public Controller { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef Controller ContinuousBase; - typedef DiscreteController DiscreteBase; + static constexpr size_t STATE_DIM = MANIFOLD::TangentDim; + using SCALAR = typename MANIFOLD::Scalar; - typedef typename DiscreteBase::state_vector_t state_vector_t; - typedef typename DiscreteBase::control_vector_t control_vector_t; + typedef Controller Base; + typedef typename Base::control_vector_t control_vector_t; + using Time_t = typename Base::Time_t; - typedef StateVectorArray state_vector_array_t; typedef ControlVectorArray control_vector_array_t; typedef FeedbackArray feedback_array_t; - //! default constructor - StateFeedbackController() : ContinuousBase(), DiscreteBase() {} + StateFeedbackController(); //! copy constructor - StateFeedbackController(const StateFeedbackController& other); + StateFeedbackController(const StateFeedbackController& other); //! constructor /*! @@ -70,18 +71,38 @@ class StateFeedbackController : public Controller& x_ref, const control_vector_array_t& uff, const feedback_array_t& K, const SCALAR deltaT, const SCALAR t0 = 0.0, const InterpolationType& intType = ZOH); + StateFeedbackController(const MANIFOLD& x_ref, + const ct::core::ControlVector& uff, + const ct::core::FeedbackMatrix& K); + //! destructor - virtual ~StateFeedbackController(){}; + virtual ~StateFeedbackController(); //! deep cloning - StateFeedbackController* clone() const override; + StateFeedbackController* clone() const override; + + template + typename std::enable_if::type computeControl_specialized(const MANIFOLD& state, + const Time_t& t, + control_vector_t& u) + { + u = uff_.eval(t) + K_.eval(t) * (state - x_ref_.eval(t)); // TODO: move to impl + } + + template + typename std::enable_if::type computeControl_specialized(const MANIFOLD& state, + const Time_t& n, + control_vector_t& u) + { + u = uff_[n] + K_[n] * (state - x_ref_[n]); // TODO: move to impl + } //! computes the control action in the continuous case /*! @@ -90,18 +111,7 @@ class StateFeedbackController : public Controller& x_ref, + void update(const DiscreteArray& x_ref, const DiscreteArray& uff, const DiscreteArray>& K, const tpl::TimeArray& t); //! get reference state vector array (without timings) - const DiscreteArray& x_ref() const; + const DiscreteArray& x_ref() const; //! get feedforward array (without timings) const DiscreteArray& uff() const; @@ -128,10 +138,10 @@ class StateFeedbackController : public Controller& time() const; //! get a reference to the feedforward trajectory - StateTrajectory& getReferenceStateTrajectory(); + DiscreteTrajectory& getReferenceStateTrajectory(); //! get a reference to the feedforward trajectory - const StateTrajectory& getReferenceStateTrajectory() const; + const DiscreteTrajectory& getReferenceStateTrajectory() const; //! get a reference to the feedforward trajectory ControlTrajectory& getFeedforwardTrajectory(); @@ -146,14 +156,15 @@ class StateFeedbackController : public Controller& getFeedbackTrajectory() const; //! extracts a physically meaningful control trajectory from the given state-feedback law and a reference state trajectory - void extractControlTrajectory(const StateTrajectory& x_traj, + void extractControlTrajectory(const DiscreteTrajectory& x_traj, ControlTrajectory& u_traj); protected: - StateTrajectory x_ref_; //! state reference trajectory + DiscreteTrajectory x_ref_; //! state reference trajectory ControlTrajectory uff_; //! feedforward control trajectory FeedbackTrajectory K_; //! feedback control trajectory }; + } // namespace core } // namespace ct diff --git a/ct_core/include/ct/core/control/continuous_time/ConstantController-impl.h b/ct_core/include/ct/core/control/continuous_time/ConstantController-impl.h deleted file mode 100644 index e29e9d333..000000000 --- a/ct_core/include/ct/core/control/continuous_time/ConstantController-impl.h +++ /dev/null @@ -1,77 +0,0 @@ -/********************************************************************************************************************** -This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. -Licensed under the BSD-2 license (see LICENSE file in main directory) -**********************************************************************************************************************/ - -#pragma once - -namespace ct { -namespace core { - - -template -ConstantController::ConstantController() -{ - u_.setZero(); -} - -template -ConstantController::ConstantController(ControlVector& u) : u_(u) -{ -} - -template -ConstantController::ConstantController( - const ConstantController& other) - : Controller(other), u_(other.u_) -{ -} - -template -ConstantController::~ConstantController() -{ -} - -template -ConstantController* ConstantController::clone() const -{ - return new ConstantController(*this); -} - -template -void ConstantController::computeControl(const StateVector& state, - const SCALAR& t, - ControlVector& controlAction) -{ - controlAction = u_; -} - -template -void ConstantController::computeControl(const StateVector& state, - const int n, - ControlVector& controlAction) -{ - computeControl(state, SCALAR(n), controlAction); -} - -template -void ConstantController::setControl(const ControlVector& u) -{ - u_ = u; -} - -template -const ControlVector& ConstantController::getControl() const -{ - return u_; -} - -template -ControlMatrix ConstantController::getDerivativeU0( - const StateVector& state, - const SCALAR time) -{ - return ControlMatrix::Identity(); -} -} // namespace core -} // namespace ct diff --git a/ct_core/include/ct/core/control/continuous_time/ConstantStateFeedbackController.h b/ct_core/include/ct/core/control/continuous_time/ConstantStateFeedbackController.h deleted file mode 100644 index ee306d4a4..000000000 --- a/ct_core/include/ct/core/control/continuous_time/ConstantStateFeedbackController.h +++ /dev/null @@ -1,127 +0,0 @@ -/********************************************************************************************************************** -This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. -Licensed under the BSD-2 license (see LICENSE file in main directory) -**********************************************************************************************************************/ - -#pragma once - -#include "Controller.h" -#include - -namespace ct { -namespace core { - -//! A constant state feedback controller -/*! - * Implements a state feedback controller which works on a constant setpoint, i.e. it features - * - one constant pure feedforward term - * - one constant state reference - * - one constant Feedback matrix - * - * and takes the form - * - * \f[ - * - * u(x) = u_{ff} + K (x - x_{ref}) - * - * \f] - */ -template -class ConstantStateFeedbackController : public Controller -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - //! Default constructor - /*! - * Sets the control signal to zero - */ - ConstantStateFeedbackController() - { - u_ff_.setZero(); - x_ref_.setZero(); - K_.setZero(); - } - - - //! Constructor - /*! - * Initializes the control to a fixed value - * @param u The fixed feedforward control signal - * @param x The fixed reference state - * @param K the fixed state feedback gain - */ - ConstantStateFeedbackController(const ControlVector& uff, - const StateVector& x, - const FeedbackMatrix& K) - : u_ff_(uff), x_ref_(x), K_(K) - { - } - - - //! Copy constructor - ConstantStateFeedbackController(const ConstantStateFeedbackController& other) - : Controller(other), u_ff_(other.u_ff_), x_ref_(other.x_ref_), K_(other.K_) - { - } - - - //! Destructor - ~ConstantStateFeedbackController() {} - //! Clone operator - /*! - * Clones the controller. Used for cloning ControlledSystem's - * @return pointer to cloned controller - */ - ConstantStateFeedbackController* clone() const override - { - return new ConstantStateFeedbackController(*this); - } - - - //! Computes current control - /*! - * Returns the fixed control signal. Therefore, the return value is invariant - * to the parameters. - * @param state The current state of the system (ignored) - * @param t The time of the system (ignored) - * @param controlAction The (fixed) control action - */ - void computeControl(const StateVector& x, - const SCALAR& t, - ControlVector& controlAction) override - { - controlAction = u_ff_ + K_ * (x - x_ref_); - } - - - //! update the control law with a new set of parameters - /*! - * @param u The fixed feedforward control signal - * @param x The fixed reference state - * @param K the fixed state feedback gain - */ - void updateControlLaw(const ControlVector& uff, - const StateVector& x, - const FeedbackMatrix& K = - FeedbackMatrix::Zero()) - { - u_ff_ = uff; - x_ref_ = x; - K_ = K; - } - - -private: - //! feedforward control input - ControlVector u_ff_; - - //! reference state - StateVector x_ref_; - - //! state feedback matrix - FeedbackMatrix K_; -}; - -} // namespace core -} // namespace ct diff --git a/ct_core/include/ct/core/control/continuous_time/ConstantTrajectoryController.h b/ct_core/include/ct/core/control/continuous_time/ConstantTrajectoryController.h deleted file mode 100644 index bfa097670..000000000 --- a/ct_core/include/ct/core/control/continuous_time/ConstantTrajectoryController.h +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************************************************************** -This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. -Licensed under the BSD-2 license (see LICENSE file in main directory) -**********************************************************************************************************************/ - -#pragma once - -#include "Controller.h" - -namespace ct { -namespace core { - -//! A constant controller -/*! - * Implements a controller that is time and state invariant, i.e. fully constant. - * This class is useful to integrate a ControlledSystem forward subject to a - * constant control input. - */ -template -class ConstantTrajectoryController : public Controller -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - //! Default constructor - /*! - * Sets the control signal to zero - */ - ConstantTrajectoryController() {} - //! Constructor - /*! - * Initializes the control to a fixed value - * @param u The fixed control signal - */ - ConstantTrajectoryController(const ControlVectorArray& u, - const StateVectorArray& x) - : uff_(u), x_(x) - { - } - - //! Copy constructor - ConstantTrajectoryController(const ConstantTrajectoryController& other) - : Controller(other), x_(other.x_), uff_(other.uff_) - { - } - - //! Destructor - ~ConstantTrajectoryController() {} - //! Clone operator - /*! - * Clones the controller. Used for cloning ControlledSystem's - * @return pointer to cloned controller - */ - ConstantTrajectoryController* clone() const override - { - return new ConstantTrajectoryController(*this); - } - - //! Computes current control - /*! - * Returns the fixed control signal. Therefore, the return value is invariant - * to the parameters. - * @param state The current state of the system (ignored) - * @param t The time of the system (ignored) - * @param controlAction The (fixed) control action - */ - void computeControl(const StateVector& state, - const SCALAR& t, - ControlVector& controlAction) override - { - throw std::runtime_error("not implemented"); - } - - //! Sets the control signal - /*! - * - * @param u The fixed control signal - */ - void setControlVectorArray(const ControlVectorArray& u) { uff_ = u; } - //! Get the fixed control signal - /*! - */ - const ControlVectorArray& getControlVectorArray() const { return uff_; } - //! Sets the state trajectory - /*! - * - * @param u The fixed state trajectory - */ - void setStateVectorArray(const StateVectorArray& x) { x_ = x; } - //! Get the fixed state trajectory - /*! - * - * - */ - const StateVectorArray& getStateVectorArray() const { return x_; } -private: - ControlVectorArray uff_; //! feedforward control trajectory - StateVectorArray x_; -}; -} -} diff --git a/ct_core/include/ct/core/control/discrete_time/DiscreteController.h b/ct_core/include/ct/core/control/discrete_time/DiscreteController.h deleted file mode 100644 index b5e458a0b..000000000 --- a/ct_core/include/ct/core/control/discrete_time/DiscreteController.h +++ /dev/null @@ -1,60 +0,0 @@ -/********************************************************************************************************************** -This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. -Licensed under the BSD-2 license (see LICENSE file in main directory) -**********************************************************************************************************************/ - -#pragma once - -#include -#include - - -namespace ct { -namespace core { - -//! Interface class for all controllers -/*! - * This is a pure interface class for Controllers that can be fed to any - * ControlledSystem. Any custom controller should derive from this class - * to ensure it is compatible with ControlledSystem and the Integrator. - */ -template -class DiscreteController -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef StateVector state_vector_t; - typedef ControlVector control_vector_t; - - //! Default constructor - DiscreteController(){}; - - //! Copy constructor - DiscreteController(const DiscreteController& other){}; - - //! Destructor - virtual ~DiscreteController(){}; - - //! Deep cloning - /*! - * Has to be implemented by any custom controller. - */ - virtual DiscreteController* clone() const = 0; - - //! Compute control signal - /*! - * Evaluate the given controller for a given state and time index - * returns the computed control action. - * - * This function has to be implemented by any custom controller - * - * @param state current state of the system - * @param n current time index of the system - * @param controlAction the corresponding control action - */ - virtual void computeControl(const state_vector_t& state, const int n, control_vector_t& controlAction) = 0; -}; - -} // namespace core -} // namespace ct diff --git a/ct_core/include/ct/core/core.h b/ct_core/include/ct/core/core.h index c6b80e279..c2f080949 100644 --- a/ct_core/include/ct/core/core.h +++ b/ct_core/include/ct/core/core.h @@ -38,7 +38,6 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include "Geometry" #include "Internal" #include "Math" -#include "Simulation" #include "Switching" #include "templateDir.h" @@ -57,7 +56,6 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include "Internal-impl" #include "Math-impl" #include "Geometry-impl" -#include "Simulation-impl" // keep standard header guard (easy debugging) // header guard is identical to the one in core-prespec.h diff --git a/ct_core/include/ct/core/integration/EventHandler.h b/ct_core/include/ct/core/integration/EventHandler.h index 9f7fb5f8d..568b6270f 100644 --- a/ct_core/include/ct/core/integration/EventHandler.h +++ b/ct_core/include/ct/core/integration/EventHandler.h @@ -5,7 +5,8 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once -#include +#include +#include namespace ct { namespace core { @@ -20,14 +21,18 @@ namespace core { * * @tparam STATE_DIM dimensionality of the state vector */ -template +template class EventHandler { public: + using SCALAR = typename MANIFOLD::Scalar; + //! Default constructor - EventHandler() {} + EventHandler() = default; + //! destructor - virtual ~EventHandler() {} + virtual ~EventHandler() = default; + virtual bool callOnSubsteps() = 0; //! reset event handler @@ -40,7 +45,7 @@ class EventHandler * @param t current time * @return true if an event has happened */ - virtual bool checkEvent(const StateVector& state, const SCALAR& t) = 0; + virtual bool checkEvent(const MANIFOLD& state, const SCALAR& t) = 0; //! handle the event /*! @@ -48,11 +53,11 @@ class EventHandler * @param state current state of the system * @param t current time */ - virtual void handleEvent(const StateVector& state, const SCALAR& t) = 0; + virtual void handleEvent(const MANIFOLD& state, const SCALAR& t) = 0; private: - StateVectorArray stateTrajectory_; //! state trajectory for recording - tpl::TimeArray timeTrajectory_; //! time trajectory for recording + ct::core::DiscreteArray stateTrajectory_; //! state trajectory for recording + tpl::TimeArray timeTrajectory_; //! time trajectory for recording }; -} -} +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler-impl.h b/ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler-impl.h new file mode 100644 index 000000000..7bdfb5457 --- /dev/null +++ b/ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler-impl.h @@ -0,0 +1,59 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +namespace ct { +namespace core { + +template +KillIntegrationEventHandler::KillIntegrationEventHandler() : killIntegration_(false) +{ +} + +template +KillIntegrationEventHandler::~KillIntegrationEventHandler() +{ +} + +template +bool KillIntegrationEventHandler::callOnSubsteps() +{ + return false; +} + +template +bool KillIntegrationEventHandler::checkEvent(const MANIFOLD& state, const double& t) +{ + return killIntegration_; +} + +template +void KillIntegrationEventHandler::handleEvent(const MANIFOLD& state, const double& t) +{ + /* throw an exception which stops the integration */ + throw std::runtime_error("Integration terminated due to external event specified by user."); +} + +template +void KillIntegrationEventHandler::setEvent() +{ + killIntegration_ = true; +} + +template +void KillIntegrationEventHandler::resetEvent() +{ + killIntegration_ = false; +} + +template +void KillIntegrationEventHandler::reset() +{ + resetEvent(); +}; + +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler.h b/ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler.h index da97fa9e6..33b23878e 100644 --- a/ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler.h +++ b/ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler.h @@ -18,40 +18,37 @@ namespace core { * * @tparam STATE_DIM size of the state vector */ -template -class KillIntegrationEventHandler : public EventHandler +template +class KillIntegrationEventHandler : public EventHandler { public: - typedef Eigen::Matrix State_T; + KillIntegrationEventHandler(); - //! default constructor - /*! - * sets kill event to false - */ - KillIntegrationEventHandler() : killIntegration_(false) {} //! default destructor - virtual ~KillIntegrationEventHandler() {} - virtual bool callOnSubsteps() override { return false; } + virtual ~KillIntegrationEventHandler(); + + virtual bool callOnSubsteps() override; + //! checks the kill flag - bool checkEvent(const State_T& state, const double& t) override { return killIntegration_; } + bool checkEvent(const MANIFOLD& state, const double& t) override; + //! interrupts integration /*! * interrupts the integration by throwing a std::runtime_error * @param state current state (ignored) * @param t current time (ignored) */ - void handleEvent(const State_T& state, const double& t) override - { - /* throw an exception which stops the integration */ - throw std::runtime_error("Integration terminated due to external event specified by user."); - } + void handleEvent(const MANIFOLD& state, const double& t) override; //! enables killing at next call - void setEvent() { killIntegration_ = true; } + void setEvent(); + //! disable killing at next call - void resetEvent() { killIntegration_ = false; } + void resetEvent(); + //! resets kill flag to false - virtual void reset() override { resetEvent(); }; + virtual void reset() override; + private: bool killIntegration_; //! kill flag }; diff --git a/ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler-impl.h b/ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler-impl.h new file mode 100644 index 000000000..e17054b1a --- /dev/null +++ b/ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler-impl.h @@ -0,0 +1,55 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +namespace ct { +namespace core { + + +template +MaxStepsEventHandler::MaxStepsEventHandler(const size_t& maxStepsPerSec) + : maxNumSteps_(maxStepsPerSec), stepsTaken_(0) +{ +} + +template +MaxStepsEventHandler::~MaxStepsEventHandler() +{ +} + +template +bool MaxStepsEventHandler::callOnSubsteps() +{ + return false; +} + +template +void MaxStepsEventHandler::reset() +{ + stepsTaken_ = 0; +}; + +template +bool MaxStepsEventHandler::checkEvent(const MANIFOLD& state, const double& t) +{ + stepsTaken_++; + return (stepsTaken_ > maxNumSteps_); // todo: fix this +} + +template +void MaxStepsEventHandler::handleEvent(const MANIFOLD& state, const double& t) +{ + throw std::runtime_error("integration terminated: max number of steps reached.\n"); +} + +template +void MaxStepsEventHandler::setMaxNumSteps(size_t maxNumSteps) +{ + maxNumSteps_ = maxNumSteps; +} + +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler.h b/ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler.h index 4a9e5ba3f..eccc76ac2 100644 --- a/ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler.h +++ b/ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler.h @@ -16,26 +16,23 @@ namespace core { * * @tparam STATE_DIM state vector size */ -template -class MaxStepsEventHandler : public ct::core::EventHandler +template +class MaxStepsEventHandler : public ct::core::EventHandler { public: - typedef ct::core::StateVector State_T; - //! default constructor /*! * @param maxStepsPerSec allowed number of steps */ - MaxStepsEventHandler(const size_t& maxStepsPerSec = std::numeric_limits::max()) - : maxNumSteps_(maxStepsPerSec), stepsTaken_(0) - { - } + MaxStepsEventHandler(const size_t& maxStepsPerSec = std::numeric_limits::max()); + + virtual ~MaxStepsEventHandler(); + + virtual bool callOnSubsteps() override; - //! destructor - virtual ~MaxStepsEventHandler() {} - virtual bool callOnSubsteps() override { return false; } //! resets the number of steps taken - virtual void reset() override { stepsTaken_ = 0; }; + virtual void reset() override; + //! checks if number of steps is exceeded /*! * @@ -43,23 +40,17 @@ class MaxStepsEventHandler : public ct::core::EventHandler * @param t current time (gets ignored) * @return true if number of steps higher than maximum allowed number */ - virtual bool checkEvent(const State_T& state, const double& t) override - { - stepsTaken_++; - return (stepsTaken_ > maxNumSteps_); // todo: fix this - } + virtual bool checkEvent(const MANIFOLD& state, const double& t) override; //! throws a std::runtime_error to terminate the integration - virtual void handleEvent(const State_T& state, const double& t) override - { - throw std::runtime_error("integration terminated: max number of steps reached.\n"); - } + virtual void handleEvent(const MANIFOLD& state, const double& t) override; //! set maximum number of steps /*! * @param maxNumSteps maximum number of steps allowed */ - void setMaxNumSteps(size_t maxNumSteps) { maxNumSteps_ = maxNumSteps; } + void setMaxNumSteps(size_t maxNumSteps); + private: size_t maxNumSteps_; //! maximum number of steps allowed size_t stepsTaken_; //! counter how many steps have passed diff --git a/ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder-impl.h b/ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder-impl.h new file mode 100644 index 000000000..200d12a2e --- /dev/null +++ b/ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder-impl.h @@ -0,0 +1,81 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +namespace ct { +namespace core { + +template +SubstepRecorder::SubstepRecorder(std::shared_ptr system, + bool activated, + std::shared_ptr states, + std::shared_ptr controls, + std::shared_ptr times) + : activated_(activated), system_(system), states_(states), controls_(controls), times_(times) +{ +} + +template +SubstepRecorder::~SubstepRecorder() +{ +} + +template +bool SubstepRecorder::callOnSubsteps() +{ + return true; +} + +template +void SubstepRecorder::setControlledSystem(const std::shared_ptr& system) +{ + system_ = system; +} + +template +bool SubstepRecorder::checkEvent(const MANIFOLD& state, const SCALAR& t) +{ + return activated_; +} + +template +void SubstepRecorder::handleEvent(const MANIFOLD& state, const SCALAR& t) +{ + states_->push_back(state); + controls_->push_back(system_->getLastControlAction()); + times_->push_back(t); +} + +template +void SubstepRecorder::setEnable(bool activated) +{ + activated_ = activated; +} + +template +void SubstepRecorder::reset() +{ + states_ = std::shared_ptr(new ManifoldArray_t()); + controls_ = std::shared_ptr>( + new ct::core::ControlVectorArray); + times_ = std::shared_ptr>(new ct::core::tpl::TimeArray); +} + +template +auto SubstepRecorder::getSubstates() const -> const std::shared_ptr& +{ + return states_; +} + +template +auto SubstepRecorder::getSubcontrols() const + -> const std::shared_ptr>& +{ + return controls_; +} + +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder.h b/ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder.h index ab8ec3b8d..f4411cca7 100644 --- a/ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder.h +++ b/ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder.h @@ -6,6 +6,7 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once #include +#include namespace ct { namespace core { @@ -16,72 +17,60 @@ namespace core { * * @tparam STATE_DIM size of the state vector */ -template -class SubstepRecorder : public EventHandler +template +class SubstepRecorder : public EventHandler { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef StateVector state_t; + using SCALAR = typename MANIFOLD::Scalar; - SubstepRecorder(std::shared_ptr> system = nullptr, + using ManifoldArray_t = DiscreteArray; + using ControlVectorArray_t = ControlVectorArray; + using TimeArray_t = tpl::TimeArray; + using ControlledSystem_t = ControlledSystem; + + SubstepRecorder(std::shared_ptr system = nullptr, bool activated = true, - std::shared_ptr> states = - std::shared_ptr>( - new ct::core::StateVectorArray), - std::shared_ptr> controls = - std::shared_ptr>( - new ct::core::ControlVectorArray), - std::shared_ptr> times = std::shared_ptr>( - new ct::core::tpl::TimeArray)) - : activated_(activated), system_(system), states_(states), controls_(controls), times_(times) - { - } - - //! default destructor - virtual ~SubstepRecorder() {} - virtual bool callOnSubsteps() override { return true; } - void setControlledSystem(const std::shared_ptr>& system) - { - system_ = system; - } + std::shared_ptr states = std::shared_ptr(new ManifoldArray_t()), + std::shared_ptr controls = std::shared_ptr( + new ControlVectorArray_t()), + std::shared_ptr times = std::shared_ptr(new TimeArray_t())); + + virtual ~SubstepRecorder(); + + virtual bool callOnSubsteps() override; + + void setControlledSystem(const std::shared_ptr& system); //! checks the kill flag - virtual bool checkEvent(const state_t& state, const SCALAR& t) override { return activated_; } + virtual bool checkEvent(const MANIFOLD& state, const SCALAR& t) override; + //! records the state - virtual void handleEvent(const state_t& state, const SCALAR& t) override - { - states_->push_back(state); - controls_->push_back(system_->getLastControlAction()); - times_->push_back(t); - } - - void setEnable(bool activated) { activated_ = activated; } + virtual void handleEvent(const MANIFOLD& state, const SCALAR& t) override; + + void setEnable(bool activated); + //! resets kill flag to false - virtual void reset() override - { - states_ = std::shared_ptr>( - new ct::core::StateVectorArray); - controls_ = std::shared_ptr>( - new ct::core::ControlVectorArray); - times_ = std::shared_ptr>(new ct::core::tpl::TimeArray); - }; - - const std::shared_ptr>& getSubstates() const { return states_; } - const std::shared_ptr>& getSubcontrols() const - { - return controls_; - } + virtual void reset() override; + + const std::shared_ptr& getSubstates() const; + + const std::shared_ptr>& getSubcontrols() const; private: bool activated_; - std::shared_ptr> system_; + std::shared_ptr system_; + + //!< container for logging the state + std::shared_ptr states_; + + //!< container for logging the control + std::shared_ptr controls_; - std::shared_ptr> states_; //!< container for logging the state - std::shared_ptr> - controls_; //!< container for logging the control - std::shared_ptr> times_; //!< container for logging the time + //!< container for logging the time + std::shared_ptr times_; }; -} -} +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/Integrator-impl.h b/ct_core/include/ct/core/integration/Integrator-impl.h index 13078866a..7153335c5 100644 --- a/ct_core/include/ct/core/integration/Integrator-impl.h +++ b/ct_core/include/ct/core/integration/Integrator-impl.h @@ -5,12 +5,11 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once - namespace ct { namespace core { -template -Integrator::Integrator(const std::shared_ptr>& system, +template +Integrator::Integrator(const SystemPtr_t& system, const IntegrationType& intType, const EventHandlerPtrVector& eventHandlers) : system_(system), observer_(eventHandlers) @@ -19,8 +18,8 @@ Integrator::Integrator(const std::shared_ptr -Integrator::Integrator(const std::shared_ptr>& system, +template +Integrator::Integrator(const SystemPtr_t& system, const IntegrationType& intType, const EventHandlerPtr& eventHandler) : system_(system), observer_(EventHandlerPtrVector(1, eventHandler)) @@ -29,8 +28,8 @@ Integrator::Integrator(const std::shared_ptr -void Integrator::changeIntegrationType(const IntegrationType& intType) +template +void Integrator::changeIntegrationType(const IntegrationType& intType) { initializeCTSteppers(intType); initializeAdaptiveSteppers(intType); @@ -39,19 +38,18 @@ void Integrator::changeIntegrationType(const IntegrationType& throw std::runtime_error("Unknown integration type"); } - -template -void Integrator::setApadativeErrorTolerances(const SCALAR absErrTol, const SCALAR& relErrTol) +template +void Integrator::setApadativeErrorTolerances(const SCALAR absErrTol, const SCALAR& relErrTol) { integratorStepper_->setAdaptiveErrorTolerances(absErrTol, relErrTol); } -template -void Integrator::integrate_n_steps(StateVector& state, +template +void Integrator::integrate_n_steps(MANIFOLD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory) { reset(); @@ -60,22 +58,20 @@ void Integrator::integrate_n_steps(StateVector -void Integrator::integrate_n_steps(StateVector& state, - const SCALAR& startTime, - size_t numSteps, - SCALAR dt) +template +void Integrator::integrate_n_steps(MANIFOLD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt) { reset(); integratorStepper_->integrate_n_steps(observer_.observeWrap, systemFunction_, state, startTime, numSteps, dt); } -template -void Integrator::integrate_const(StateVector& state, + +template +void Integrator::integrate_const(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, SCALAR dt, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory) { reset(); @@ -84,21 +80,18 @@ void Integrator::integrate_const(StateVector -void Integrator::integrate_const(StateVector& state, - const SCALAR& startTime, - const SCALAR& finalTime, - SCALAR dt) +template +void Integrator::integrate_const(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, SCALAR dt) { reset(); integratorStepper_->integrate_const(observer_.observeWrap, systemFunction_, state, startTime, finalTime, dt); } -template -void Integrator::integrate_adaptive(StateVector& state, +template +void Integrator::integrate_adaptive(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory, const SCALAR dtInitial) { @@ -109,8 +102,8 @@ void Integrator::integrate_adaptive(StateVector -void Integrator::integrate_adaptive(StateVector& state, +template +void Integrator::integrate_adaptive(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, SCALAR dtInitial) @@ -120,10 +113,10 @@ void Integrator::integrate_adaptive(StateVector -void Integrator::integrate_times(StateVector& state, +template +void Integrator::integrate_times(MANIFOLD& state, const tpl::TimeArray& timeTrajectory, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, SCALAR dtInitial) { reset(); @@ -133,22 +126,22 @@ void Integrator::integrate_times(StateVector -void Integrator::initializeCTSteppers(const IntegrationType& intType) +template +void Integrator::initializeCTSteppers(const IntegrationType& intType) { switch (intType) { case EULERCT: { - integratorStepper_ = std::shared_ptr, SCALAR>>( - new internal::StepperEulerCT, SCALAR>()); + integratorStepper_ = std::shared_ptr>( + new internal::StepperEulerCT()); break; } case RK4CT: { - integratorStepper_ = std::shared_ptr, SCALAR>>( - new internal::StepperRK4CT, SCALAR>()); + integratorStepper_ = std::shared_ptr>( + new internal::StepperRK4CT()); break; } @@ -157,41 +150,41 @@ void Integrator::initializeCTSteppers(const IntegrationType& } } -template -void Integrator::reset() +template +void Integrator::reset() { observer_.reset(); } -template -void Integrator::retrieveTrajectoriesFromObserver( - StateVectorArray& stateTrajectory, +template +void Integrator::retrieveTrajectoriesFromObserver(DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory) { stateTrajectory.swap(observer_.states_); timeTrajectory.swap(observer_.times_); } -template -void Integrator::retrieveStateVectorArrayFromObserver( - StateVectorArray& stateTrajectory) +template +void Integrator::retrieveStateVectorArrayFromObserver(DiscreteArray& stateTrajectory) { stateTrajectory.swap(observer_.states_); } -template -void Integrator::setupSystem() +template +void Integrator::setupSystem() { - systemFunction_ = [this]( - const Eigen::Matrix& x, Eigen::Matrix& dxdt, SCALAR t) { - const StateVector& xState(static_cast&>(x)); - StateVector& dxdtState(static_cast&>(dxdt)); + systemFunction_ = [this](const MANIFOLD& x, TANGENT& dxdt, SCALAR t) { + const MANIFOLD& xState(static_cast(x)); + + TANGENT& dxdtState(static_cast(dxdt)); + system_->computeDynamics(xState, t, dxdtState); + observer_.observeInternal(xState, t); }; reset(); } -} -} +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/Integrator.h b/ct_core/include/ct/core/integration/Integrator.h index 4f37d8995..b26b96bf5 100644 --- a/ct_core/include/ct/core/integration/Integrator.h +++ b/ct_core/include/ct/core/integration/Integrator.h @@ -8,17 +8,24 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include #include #include +#include #include "EventHandler.h" #include "Observer.h" #include "eigenIntegration.h" +#include "manifIntegration.h" -#include "internal/StepperBase.h" - -#include "internal/SteppersODEInt.h" -#include "internal/SteppersCT.h" +#include "internal/StepperODEInt.h" +#include "internal/StepperODEIntDenseOutput.h" +#include "internal/StepperODEIntControlled.h" +#include "internal/StepperEulerCT.h" +#include "internal/StepperRK4CT.h" #include +#include +#include + +#include namespace ct { namespace core { @@ -51,20 +58,24 @@ enum IntegrationType * \dot{x} = f(x,t) * \f] * - * * Unit test \ref IntegrationTest.cpp illustrates the use of Integrator.h * - * * @tparam STATE_DIM the size of the state vector * @tparam SCALAR The scalar type */ -template +template class Integrator { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef std::shared_ptr> EventHandlerPtr; - typedef std::vector> EventHandlerPtrVector; + + using SCALAR = typename MANIFOLD::Scalar; + using TANGENT = typename MANIFOLD::Tangent; + using System_t = System; + using SystemPtr_t = std::shared_ptr; + + using EventHandlerPtr = std::shared_ptr>; + using EventHandlerPtrVector = std::vector>; //! constructor /*! @@ -75,13 +86,16 @@ class Integrator * @param absErrTol optional absolute error tolerance (for variable step solvers) * @param relErrTol optional relative error tolerance (for variable step solvers) */ - Integrator(const std::shared_ptr>& system, + Integrator(const SystemPtr_t& system, const IntegrationType& intType = IntegrationType::EULERCT, const EventHandlerPtrVector& eventHandlers = EventHandlerPtrVector(0)); - Integrator(const std::shared_ptr>& system, - const IntegrationType& intType, - const EventHandlerPtr& eventHandler); + Integrator(const SystemPtr_t& system, const IntegrationType& intType, const EventHandlerPtr& eventHandler); + + /** + * @brief Construct a new Integrator object + */ + Integrator(const Integrator& other) = delete; /** * @brief Changes the integration type @@ -90,7 +104,6 @@ class Integrator */ void changeIntegrationType(const IntegrationType& intType); - /** * @brief Sets the adaptive error tolerances * @@ -113,11 +126,11 @@ class Integrator * @param stateTrajectory state evolution over time * @param timeTrajectory time trajectory corresponding to state trajectory */ - void integrate_n_steps(StateVector& state, + void integrate_n_steps(MANIFOLD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory); //! Equidistant integration based on number of time steps and step length @@ -130,7 +143,7 @@ class Integrator * @param numSteps number of steps to integrate forward * @param dt step size (fixed also for variable step solvers) */ - void integrate_n_steps(StateVector& state, const SCALAR& startTime, size_t numSteps, SCALAR dt); + void integrate_n_steps(MANIFOLD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt); //! Equidistant integration based on initial and final time as well as step length /*! @@ -146,11 +159,11 @@ class Integrator * @param stateTrajectory state evolution over time * @param timeTrajectory time trajectory corresponding to state trajectory */ - void integrate_const(StateVector& state, + void integrate_const(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, SCALAR dt, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory); //! Equidistant integration based on initial and final time as well as step length @@ -163,10 +176,7 @@ class Integrator * @param finalTime the final time of the integration * @param dt step size (fixed also for variable step solvers) */ - void integrate_const(StateVector& state, - const SCALAR& startTime, - const SCALAR& finalTime, - SCALAR dt); + void integrate_const(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, SCALAR dt); //! integrate forward from an initial to a final time using an adaptive scheme /*! @@ -185,10 +195,10 @@ class Integrator * @param timeTrajectory time trajectory corresponding to state trajectory * @param dtInitial step size (initial guess, for fixed step integrators it is fixed) */ - void integrate_adaptive(StateVector& state, + void integrate_adaptive(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory, const SCALAR dtInitial = SCALAR(0.01)); @@ -205,7 +215,7 @@ class Integrator * @param finalTime the final time of the integration * @param dtInitial step size (initial guess, for fixed step integrators it is fixed) */ - void integrate_adaptive(StateVector& state, + void integrate_adaptive(MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, SCALAR dtInitial = SCALAR(0.01)); @@ -221,9 +231,9 @@ class Integrator * @param stateTrajectory the resulting state trajectory corresponding to the times provided * @param dtInitial an initial guess for a time step (fixed for fixed step integrators) */ - void integrate_times(StateVector& state, + void integrate_times(MANIFOLD& state, const tpl::TimeArray& timeTrajectory, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, SCALAR dtInitial = SCALAR(0.01)); private: @@ -240,57 +250,18 @@ class Integrator * @param[in] intType The integration type * */ - template - typename std::enable_if::value, void>::type initializeAdaptiveSteppers( - const IntegrationType& intType) - { - switch (intType) - { - case ODE45: - { - integratorStepper_ = - std::shared_ptr, - Eigen::Matrix, SCALAR>>( - new internal::StepperODEIntControlled, - Eigen::Matrix, SCALAR>()); - break; - } - case RK5VARIABLE: - { - integratorStepper_ = std::shared_ptr, Eigen::Matrix, SCALAR>>( - new internal::StepperODEIntDenseOutput, - Eigen::Matrix, SCALAR>()); - break; - } + template ::value, bool>::type = true> + void initializeAdaptiveSteppers(const IntegrationType& intType); - case BULIRSCHSTOER: - { - integratorStepper_ = - std::shared_ptr, - Eigen::Matrix, SCALAR>>( - new internal::StepperODEInt, - Eigen::Matrix, SCALAR>()); - break; - } - default: - break; - } - } - - template - typename std::enable_if::value, void>::type initializeAdaptiveSteppers( - const IntegrationType& intType) - { - } + template ::value), bool>::type = true> + void initializeAdaptiveSteppers(const IntegrationType& intType); #ifdef CPPADCG template typename std::enable_if::value, void>::type initializeODEIntSteppers( - const IntegrationType& intType) - { - } + const IntegrationType& intType); #endif /** @@ -302,54 +273,7 @@ class Integrator */ template typename std::enable_if::value, void>::type initializeODEIntSteppers( - const IntegrationType& intType) - { - switch (intType) - { - case EULER: - { - integratorStepper_ = std::shared_ptr, - Eigen::Matrix, SCALAR>>( - new internal::StepperODEInt, - Eigen::Matrix, SCALAR>()); - break; - } - - case RK4: - { - integratorStepper_ = - std::shared_ptr, - Eigen::Matrix, SCALAR>>( - new internal::StepperODEInt, - Eigen::Matrix, SCALAR>()); - break; - } - - case MODIFIED_MIDPOINT: - { - integratorStepper_ = - std::shared_ptr, - Eigen::Matrix, SCALAR>>( - new internal::StepperODEInt, - Eigen::Matrix, SCALAR>()); - break; - } - - case RK78: - { - integratorStepper_ = - std::shared_ptr, - Eigen::Matrix, SCALAR>>( - new internal::StepperODEInt, - Eigen::Matrix, SCALAR>()); - - break; - } - default: - break; - } - } - + const IntegrationType& intType); //! resets the observer void reset(); @@ -360,23 +284,157 @@ class Integrator * @param stateTrajectory The state trajectory * @param timeTrajectory The time trajectory */ - void retrieveTrajectoriesFromObserver(StateVectorArray& stateTrajectory, + void retrieveTrajectoriesFromObserver(DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory); + /** * @brief Retrieves the state trajectory from the observer * * @param stateTrajectory The state trajectory */ - void retrieveStateVectorArrayFromObserver(StateVectorArray& stateTrajectory); + void retrieveStateVectorArrayFromObserver(DiscreteArray& stateTrajectory); //! sets up the lambda function void setupSystem(); - std::shared_ptr> system_; //! pointer to the system - std::function&, Eigen::Matrix&, SCALAR)> - systemFunction_; //! the system function to integrate - std::shared_ptr, SCALAR>> integratorStepper_; - Observer observer_; //! observer + SystemPtr_t system_; //! pointer to the system + + //! the system function to integrate + typename internal::StepperBase::SystemFunction_t systemFunction_; + + std::shared_ptr> integratorStepper_; + + Observer observer_; //! observer }; + + +#ifdef CPPADCG +template +template +typename std::enable_if::value, void>::type Integrator::initializeODEIntSteppers( + const IntegrationType& intType) +{ + // do nothing +} +#endif + + +template +template +typename std::enable_if::value, void>::type Integrator::initializeODEIntSteppers( + const IntegrationType& intType) +{ + switch (intType) + { + case EULER: + { + integratorStepper_ = std::shared_ptr, MANIFOLD>>( + new internal::StepperODEInt, MANIFOLD>()); + break; + } + case RK4: + { + integratorStepper_ = + std::shared_ptr, MANIFOLD>>( + new internal::StepperODEInt, MANIFOLD>()); + break; + } + case MODIFIED_MIDPOINT: + { + if (is_euclidean::value) + { + integratorStepper_ = + std::shared_ptr, MANIFOLD>>( + new internal::StepperODEInt, MANIFOLD>()); + } + else + throw std::runtime_error( + "MODIFIED_MIDPOINT stepping not supported on manifolds. Use a different stepper."); + break; + } + + case RK78: + { + integratorStepper_ = + std::shared_ptr, MANIFOLD>>( + new internal::StepperODEInt, MANIFOLD>()); + + break; + } + default: + break; + } } + + +template +template ::value), bool>::type> +void Integrator::initializeAdaptiveSteppers(const IntegrationType& intType) +{ + switch (intType) + { + case ODE45: + { + throw std::runtime_error( + "Integrator ODE45 on manifolds currently not supported yet. Implementation of error measures in " + "manifIntegration.h required."); + break; + } + case RK5VARIABLE: + { + throw std::runtime_error( + "Integrator RK5VARIABLE on manifolds currently not supported yet. Implementation of error measures in " + "manifIntegration.h required."); + break; + } + case BULIRSCHSTOER: + { + throw std::runtime_error( + "Integrator BULIRSCHSTOER on manifolds currently not supported yet. Implementation of error measures " + "in manifIntegration.h required."); + break; + } + default: + break; + } +} + +template +template ::value, bool>::type> +void Integrator::initializeAdaptiveSteppers(const IntegrationType& intType) +{ + switch (intType) + { + case ODE45: + { + integratorStepper_ = + std::shared_ptr, MANIFOLD>>( + new internal::StepperODEIntControlled, MANIFOLD>()); + break; + } + + case RK5VARIABLE: + { + integratorStepper_ = + std::shared_ptr, MANIFOLD>>( + new internal::StepperODEIntDenseOutput, MANIFOLD>()); + break; + } + + case BULIRSCHSTOER: + { + integratorStepper_ = + std::shared_ptr, MANIFOLD>>( + new internal::StepperODEInt, MANIFOLD>()); + break; + } + default: + break; + } } + +template +using EuclideanIntegrator = Integrator>; + +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/IntegratorSymplectic-impl.h b/ct_core/include/ct/core/integration/IntegratorSymplectic-impl.h index 713791473..693c4b541 100644 --- a/ct_core/include/ct/core/integration/IntegratorSymplectic-impl.h +++ b/ct_core/include/ct/core/integration/IntegratorSymplectic-impl.h @@ -9,18 +9,18 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) namespace ct { namespace core { -template -IntegratorSymplectic::IntegratorSymplectic( - const std::shared_ptr> system, +template +IntegratorSymplectic::IntegratorSymplectic( + const std::shared_ptr system, const EventHandlerPtrVector& eventHandlers) : systemSymplectic_(system), observer_(eventHandlers) { setupSystem(); } -template -IntegratorSymplectic::IntegratorSymplectic( - const std::shared_ptr> system, +template +IntegratorSymplectic::IntegratorSymplectic( + const std::shared_ptr system, const EventHandlerPtr& eventHandler) : systemSymplectic_(system), observer_(EventHandlerPtrVector(1, eventHandler)) { @@ -28,13 +28,12 @@ IntegratorSymplectic::Integrator } -template -void IntegratorSymplectic::integrate_n_steps( - StateVector& state, +template +void IntegratorSymplectic::integrate_n_steps(SYM_MFD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory) { pair_t xPair; @@ -57,9 +56,8 @@ void IntegratorSymplectic::integ } } -template -void IntegratorSymplectic::integrate_n_steps( - StateVector& state, +template +void IntegratorSymplectic::integrate_n_steps(SYM_MFD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt) @@ -79,14 +77,14 @@ void IntegratorSymplectic::integ } } -template -void IntegratorSymplectic::reset() +template +void IntegratorSymplectic::reset() { observer_.reset(); } -template -void IntegratorSymplectic::setupSystem() +template +void IntegratorSymplectic::setupSystem() { systemFunctionPosition_ = [this]( const Eigen::Matrix& v, Eigen::Matrix& dxdt) { diff --git a/ct_core/include/ct/core/integration/IntegratorSymplectic.h b/ct_core/include/ct/core/integration/IntegratorSymplectic.h index c63a03f00..1727c7842 100644 --- a/ct_core/include/ct/core/integration/IntegratorSymplectic.h +++ b/ct_core/include/ct/core/integration/IntegratorSymplectic.h @@ -12,8 +12,9 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #include #include "eigenIntegration.h" +#include "manifIntegration.h" -#include +#include #include "internal/SteppersODEIntDefinitions.h" @@ -30,15 +31,27 @@ namespace core { * @tparam CONTROL_DIM The control dimension * @tparam Stepper The stepper type */ -template +template class IntegratorSymplectic { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef typename std::pair, Eigen::Matrix> pair_t; - typedef std::shared_ptr> EventHandlerPtr; - typedef std::vector> EventHandlerPtrVector; + static constexpr size_t STATE_DIM = SYM_MFD::TangentDim; + static constexpr size_t POS_DIM = SYM_MFD::PosDim; + static constexpr size_t VEL_DIM = SYM_MFD::VelDim; + + static_assert(is_symplectic::value, "Symplectic system: manifold must be defined as symplectic."); + + static_assert(STATE_DIM == (POS_DIM + VEL_DIM), + "Symplectic system: state_dim must be the sum of position and velocity dim."); + + using SCALAR = typename SYM_MFD::Scalar; + using pair_t = typename std::pair, Eigen::Matrix>; + + using SymplecticSystem_t = SymplecticSystem; + using EventHandlerPtr = std::shared_ptr>; + using EventHandlerPtrVector = std::vector>; /** @@ -47,7 +60,7 @@ class IntegratorSymplectic * * @param[in] system A core::system */ - IntegratorSymplectic(const std::shared_ptr> system, + IntegratorSymplectic(const std::shared_ptr system, const EventHandlerPtrVector& eventHandlers = EventHandlerPtrVector(0)); /** @@ -56,8 +69,7 @@ class IntegratorSymplectic * * @param[in] system A core::system */ - IntegratorSymplectic(const std::shared_ptr> system, - const EventHandlerPtr& eventHandler); + IntegratorSymplectic(const std::shared_ptr system, const EventHandlerPtr& eventHandler); /** @@ -72,11 +84,11 @@ class IntegratorSymplectic * @param[out] stateTrajectory The resulting state trajectory * @param[out] timeTrajectory The resulting time trajectory */ - void integrate_n_steps(StateVector& state, + void integrate_n_steps(SYM_MFD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt, - StateVectorArray& stateTrajectory, + DiscreteArray& stateTrajectory, tpl::TimeArray& timeTrajectory); /** @@ -89,10 +101,7 @@ class IntegratorSymplectic * @param[in] numSteps The number of integration steps * @param[in] dt The integration time step */ - void integrate_n_steps(StateVector& state, - const SCALAR& startTime, - size_t numSteps, - SCALAR dt); + void integrate_n_steps(SYM_MFD& state, const SCALAR& startTime, size_t numSteps, SCALAR dt); void reset(); @@ -102,18 +111,21 @@ class IntegratorSymplectic * position and velocity update */ void setupSystem(); - StateVector xCached_; //! The cached state. This will be used for the system function + SYM_MFD xCached_; //! The cached state. This will be used for the system function + //! the position system function std::function&, Eigen::Matrix&)> - systemFunctionPosition_; //! the position system function + systemFunctionPosition_; + + //! the velocity system function std::function&, Eigen::Matrix&)> - systemFunctionVelocity_; //! the velocity system function + systemFunctionVelocity_; - std::shared_ptr> systemSymplectic_; + std::shared_ptr systemSymplectic_; Stepper stepper_; - Observer observer_; //! observer + Observer observer_; }; @@ -121,12 +133,10 @@ class IntegratorSymplectic * Defining the integrators *******************************************************************/ -template -using IntegratorSymplecticEuler = - IntegratorSymplectic, SCALAR>; +template +using IntegratorSymplecticEuler = IntegratorSymplectic>; -template -using IntegratorSymplecticRk = - IntegratorSymplectic, SCALAR>; +template +using IntegratorSymplecticRk = IntegratorSymplectic>; } } diff --git a/ct_core/include/ct/core/integration/Observer-impl.h b/ct_core/include/ct/core/integration/Observer-impl.h index 0e06cb5fe..8248dc5db 100644 --- a/ct_core/include/ct/core/integration/Observer-impl.h +++ b/ct_core/include/ct/core/integration/Observer-impl.h @@ -8,10 +8,10 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) namespace ct { namespace core { -template -Observer::Observer(const EventHandlerPtrVector& eventHandlers) - : observeWrap([this](const StateVector& x, const SCALAR& t) { this->observe(x, t); }), - observeWrapWithLogging([this](const StateVector& x, const SCALAR& t) { +template +Observer::Observer(const EventHandlerPtrVector& eventHandlers) + : observeWrap([this](const MANIFOLD& x, const SCALAR& t) { this->observe(x, t); }), + observeWrapWithLogging([this](const MANIFOLD& x, const SCALAR& t) { this->log(x, t); this->observe(x, t); }) @@ -20,8 +20,8 @@ Observer::Observer(const EventHandlerPtrVector& eventHandlers eventHandlers_ = eventHandlers; } -template -void Observer::reset() +template +void Observer::reset() { for (size_t i = 0; i < eventHandlers_.size(); i++) eventHandlers_[i]->reset(); @@ -30,8 +30,8 @@ void Observer::reset() times_.clear(); } -template -void Observer::observe(const StateVector& x, const SCALAR& t) +template +void Observer::observe(const MANIFOLD& x, const SCALAR& t) { for (size_t i = 0; i < eventHandlers_.size(); i++) { @@ -40,15 +40,15 @@ void Observer::observe(const StateVector& } } -template -void Observer::log(const StateVector& x, const SCALAR& t) +template +void Observer::log(const MANIFOLD& x, const SCALAR& t) { states_.push_back(x); times_.push_back(t); } -template -void Observer::observeInternal(const StateVector& x, const SCALAR& t) +template +void Observer::observeInternal(const MANIFOLD& x, const SCALAR& t) { for (size_t i = 0; i < eventHandlers_.size(); i++) { diff --git a/ct_core/include/ct/core/integration/Observer.h b/ct_core/include/ct/core/integration/Observer.h index 192f3f18d..94e9d0e93 100644 --- a/ct_core/include/ct/core/integration/Observer.h +++ b/ct_core/include/ct/core/integration/Observer.h @@ -13,9 +13,12 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) namespace ct { namespace core { -template + +// forward declaration of the integrator class +template class Integrator; + //! Observer for Integrator /*! * Implements a general observer as required by boost::odeint. This wraps all event handlers and calls them. @@ -23,17 +26,18 @@ class Integrator; * * @tparam STATE_DIM The size of the state vector */ -template +template class Observer { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - friend class Integrator; + friend class Integrator; + + using SCALAR = typename MANIFOLD::Scalar; - typedef std::vector>, - Eigen::aligned_allocator>>> - EventHandlerPtrVector; + using EventHandlerPtrVector = std::vector>, + Eigen::aligned_allocator>>>; //! default constructor /*! @@ -44,22 +48,22 @@ class Observer //! reset the observer void reset(); - void observe(const StateVector& x, const SCALAR& t); + void observe(const MANIFOLD& x, const SCALAR& t); - void log(const StateVector& x, const SCALAR& t); + void log(const MANIFOLD& x, const SCALAR& t); - void observeInternal(const StateVector& x, const SCALAR& t); + void observeInternal(const MANIFOLD& x, const SCALAR& t); private: //! Lambda to pass to odeint (odeint takes copies of the observer so we can't pass the class - std::function& x, const SCALAR& t)> observeWrap; - std::function& x, const SCALAR& t)> observeWrapWithLogging; - - ct::core::StateVectorArray states_; //!< container for logging the state - ct::core::tpl::TimeArray times_; //!< container for logging the time + std::function observeWrap; + std::function observeWrapWithLogging; + ct::core::DiscreteArray states_; //!< container for logging the state + ct::core::tpl::TimeArray times_; //!< container for logging the time EventHandlerPtrVector eventHandlers_; //! list of event handlers }; -} -} + +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/eigenIntegration.h b/ct_core/include/ct/core/integration/eigenIntegration.h index 6bd13d519..e9e83b3c3 100644 --- a/ct_core/include/ct/core/integration/eigenIntegration.h +++ b/ct_core/include/ct/core/integration/eigenIntegration.h @@ -1,18 +1,12 @@ -/* - * eigenIntegration.h - * - * Created on: Jun 22, 2015 - * Author: farbod - */ - #pragma once - #include #include #include #include +#include + // Necessary routines for Eigen matrices to work with vector_space_algebra // from odeint @@ -99,7 +93,7 @@ inline const typename Eigen:: #endif -} // end Eigen namespace +} // namespace Eigen namespace boost { @@ -120,6 +114,18 @@ struct vector_space_norm_inf> } }; + +template +struct vector_space_norm_inf> +{ + using result_type = typename ct::core::EuclideanState::Scalar; + + result_type operator()(const ct::core::EuclideanState& m) const + { + return m.template lpNorm(); // TODO: can we call above method instead? + } +}; + #else // old boost @@ -137,7 +143,21 @@ struct vector_space_reduce> } }; +template +struct vector_space_reduce> +{ + template + SCALAR operator()(const ct::core::EuclideanState& x, Op op, SCALAR init) const + { + for (int i = 0; i < STATE_DIM; i++) + { + init = op(init, x(i)); + } + return init; + } +}; + #endif -} -} -} // end boost::numeric::odeint namespace +} // namespace odeint +} // namespace numeric +} // namespace boost diff --git a/ct_core/include/ct/core/integration/internal/StepperBase-impl.h b/ct_core/include/ct/core/integration/internal/StepperBase-impl.h new file mode 100644 index 000000000..fd7278d13 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperBase-impl.h @@ -0,0 +1,104 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +namespace ct { +namespace core { +namespace internal { + +template +StepperBase::StepperBase() : absErrTol_(SCALAR(1e-8)), relErrTol_(SCALAR(1e-8)) +{ +} + +template +StepperBase::~StepperBase() +{ +} + +template +void StepperBase::integrate_n_steps(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) +{ + throw std::runtime_error("Integrate_n_steps not implemented for the stepper type"); +} + +template +void StepperBase::integrate_n_steps(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) +{ + throw std::runtime_error("Integrate_n_steps not implemented for the stepper type"); +} + +template +void StepperBase::integrate_const(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dt) +{ + throw std::runtime_error("integrate_const not implemented for the stepper type"); +} + +template +void StepperBase::integrate_const(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dt) +{ + throw std::runtime_error("integrate_const not implemented for the stepper type"); +} + +template +void StepperBase::integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dtInitial) +{ + throw std::runtime_error("integrate_adaptive not implemented for the stepper type"); +} + +template +void StepperBase::integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + const SCALAR dtInitial) +{ + throw std::runtime_error("integrate_adaptive not implemented for the stepper type"); +} + +template +void StepperBase::integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const tpl::TimeArray& timeTrajectory, + SCALAR dtInitial) +{ + throw std::runtime_error("integrate_times not implemented for the stepper type"); +} + +template +void StepperBase::setAdaptiveErrorTolerances(const SCALAR absErrTol, const SCALAR& relErrTol) +{ + absErrTol_ = absErrTol; + relErrTol_ = relErrTol; +} + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperBase.h b/ct_core/include/ct/core/integration/internal/StepperBase.h index ba57e3cb8..c3eb1db36 100644 --- a/ct_core/include/ct/core/integration/internal/StepperBase.h +++ b/ct_core/include/ct/core/integration/internal/StepperBase.h @@ -16,14 +16,21 @@ namespace internal { * @tparam MATRIX The matrix type to be integrated * @tparam SCALAR The scalar type */ -template +template class StepperBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - StepperBase() : absErrTol_(SCALAR(1e-8)), relErrTol_(SCALAR(1e-8)) {} - virtual ~StepperBase() {} + using SCALAR = typename MANIFOLD::Scalar; + using Tangent = typename MANIFOLD::Tangent; + using SystemFunction_t = std::function; + using ObserverFunction_t = std::function; + + StepperBase(); + + virtual ~StepperBase(); + /** * @brief Performs numSteps integration steps * @@ -33,14 +40,11 @@ class StepperBase * @param[in] numSteps The number of integration steps * @param[in] dt The integration timestep */ - virtual void integrate_n_steps(const std::function& rhs, - MATRIX& state, + virtual void integrate_n_steps(const SystemFunction_t& rhs, + MANIFOLD& state, const SCALAR& startTime, size_t numSteps, - SCALAR dt) - { - throw std::runtime_error("Integrate_n_steps not implemented for the stepper type"); - } + SCALAR dt); /** * @brief Performs numSteps integration steps @@ -52,16 +56,12 @@ class StepperBase * @param[in] numSteps The number steps * @param[in] dt The integration timestep */ - virtual void integrate_n_steps(std::function observer, - const std::function& rhs, - MATRIX& state, + virtual void integrate_n_steps(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, const SCALAR& startTime, size_t numSteps, - SCALAR dt) - { - throw std::runtime_error("Integrate_n_steps not implemented for the stepper type"); - } - + SCALAR dt); /** * @brief Equidistant integration based on initial and final time as well as step length @@ -72,14 +72,11 @@ class StepperBase * @param[in] finalTime The final time * @param[in] dt The integration timestep */ - virtual void integrate_const(const std::function& rhs, - MATRIX& state, + virtual void integrate_const(const SystemFunction_t& rhs, + MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, - SCALAR dt) - { - throw std::runtime_error("integrate_const not implemented for the stepper type"); - } + SCALAR dt); /** * @brief Equidistant integration based on initial and final time as well as step length @@ -91,15 +88,12 @@ class StepperBase * @param[in] finalTime The final time * @param[in] dt The integration timestep */ - virtual void integrate_const(std::function observer, - const std::function& rhs, - MATRIX& state, + virtual void integrate_const(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, - SCALAR dt) - { - throw std::runtime_error("integrate_const not implemented for the stepper type"); - } + SCALAR dt); /** * @brief Integrates forward in time from an initial to a final @@ -114,14 +108,11 @@ class StepperBase * @param[in] finalTime The final time * @param[in] dtInitial The initial integration timestep */ - virtual void integrate_adaptive(const std::function& rhs, - MATRIX& state, + virtual void integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, - SCALAR dtInitial = SCALAR(0.01)) - { - throw std::runtime_error("integrate_adaptive not implemented for the stepper type"); - } + SCALAR dtInitial = SCALAR(0.01)); /** * @brief Integrates forward in time from an initial to a final @@ -137,15 +128,12 @@ class StepperBase * @param[in] finalTime The final time * @param[in] dtInitial The initial integration timestep */ - virtual void integrate_adaptive(std::function observer, - const std::function& rhs, - MATRIX& state, + virtual void integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, const SCALAR& startTime, const SCALAR& finalTime, - const SCALAR dtInitial = SCALAR(0.01)) - { - throw std::runtime_error("integrate_adaptive not implemented for the stepper type"); - } + const SCALAR dtInitial = SCALAR(0.01)); /** * @brief Integrates a system using a given time sequence @@ -156,14 +144,11 @@ class StepperBase * @param[in] timeTrajectory The time trajectory * @param[in] dtInitial The initial integration timestep */ - virtual void integrate_times(std::function observer, - const std::function& rhs, - MATRIX& state, + virtual void integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, const tpl::TimeArray& timeTrajectory, - SCALAR dtInitial = SCALAR(0.01)) - { - throw std::runtime_error("integrate_times not implemented for the stepper type"); - } + SCALAR dtInitial = SCALAR(0.01)); /** * @brief Sets the adaptive error tolerances. @@ -171,16 +156,13 @@ class StepperBase * @param[in] absErrTol The absolute error tolerance * @param[in] relErrTol The relative error tolerance */ - void setAdaptiveErrorTolerances(const SCALAR absErrTol, const SCALAR& relErrTol) - { - absErrTol_ = absErrTol; - relErrTol_ = relErrTol; - } + void setAdaptiveErrorTolerances(const SCALAR absErrTol, const SCALAR& relErrTol); protected: SCALAR absErrTol_; SCALAR relErrTol_; }; -} -} -} + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperCTBase-impl.h b/ct_core/include/ct/core/integration/internal/StepperCTBase-impl.h new file mode 100644 index 000000000..20b91e9d0 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperCTBase-impl.h @@ -0,0 +1,57 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +namespace ct { +namespace core { +namespace internal { + +template +StepperCTBase::StepperCTBase() +{ +} + +template +StepperCTBase::~StepperCTBase() +{ +} + +template +void StepperCTBase::integrate_n_steps(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) +{ + SCALAR time = startTime; + for (size_t i = 0; i < numSteps; ++i) + { + do_step(rhs, state, time, dt); + time += dt; + } +} + +template +void StepperCTBase::integrate_n_steps(ObserverFunction_t observe, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) +{ + SCALAR time = startTime; + + for (size_t i = 0; i < numSteps; ++i) + { + do_step(rhs, state, time, dt); + time += dt; + observe(state, time); + } +} + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperCTBase.h b/ct_core/include/ct/core/integration/internal/StepperCTBase.h new file mode 100644 index 000000000..cafedd5f7 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperCTBase.h @@ -0,0 +1,61 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +#include "StepperBase.h" + +namespace ct { +namespace core { +namespace internal { + + +/** + * @brief The stepper interface for custom steppers + * + * @tparam MATRIX The Matrix type to be integrated + * @tparam SCALAR The scalar type + */ +template +class StepperCTBase : public StepperBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using SCALAR = typename MANIFOLD::Scalar; + using Tangent = typename MANIFOLD::Tangent; + using SystemFunction_t = typename StepperBase::SystemFunction_t; + using ObserverFunction_t = typename StepperBase::ObserverFunction_t; + + StepperCTBase(); + virtual ~StepperCTBase(); + + virtual void integrate_n_steps(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) override; + + virtual void integrate_n_steps(ObserverFunction_t observe, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) override; + + /** + * @brief Implements a single step of the integration scheme + * + * @param[in] rhs The ODE + * @param[in, out] stateInOut The state + * @param[in] time The integration time + * @param[in] dt The integration timestep + */ + virtual void do_step(const SystemFunction_t& rhs, MANIFOLD& stateInOut, const SCALAR time, const SCALAR dt) = 0; +}; + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperEulerCT-impl.h b/ct_core/include/ct/core/integration/internal/StepperEulerCT-impl.h new file mode 100644 index 000000000..6a8fc7a18 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperEulerCT-impl.h @@ -0,0 +1,31 @@ +#pragma once + +namespace ct { +namespace core { +namespace internal { + +template +StepperEulerCT::StepperEulerCT() +{ +} + +template +StepperEulerCT::~StepperEulerCT() +{ +} + +template +void StepperEulerCT::do_step(const SystemFunction_t& rhs, + MANIFOLD& stateInOut, + const SCALAR time, + const SCALAR dt) +{ + rhs(stateInOut, derivative_, time); + + // TODO: fails if order of multiplication is other way round ... why? Is this intentional? + stateInOut = stateInOut + derivative_ * dt; +} + +} // namespace internal +} // namespace core +} // namespace ct \ No newline at end of file diff --git a/ct_core/include/ct/core/integration/internal/StepperEulerCT.h b/ct_core/include/ct/core/integration/internal/StepperEulerCT.h new file mode 100644 index 000000000..e090f55da --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperEulerCT.h @@ -0,0 +1,39 @@ +#pragma once + +#include "StepperCTBase.h" + +namespace ct { +namespace core { +namespace internal { + +/** + * @brief Custom implementation of the euler stepper + * + * @tparam MATRIX The matrix type + * @tparam SCALAR The scalar type + */ +template +class StepperEulerCT : public StepperCTBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using SCALAR = typename MANIFOLD::Scalar; + using Tangent = typename MANIFOLD::Tangent; + using SystemFunction_t = typename StepperCTBase::SystemFunction_t; + + StepperEulerCT(); + virtual ~StepperEulerCT(); + +private: + virtual void do_step(const SystemFunction_t& rhs, + MANIFOLD& stateInOut, + const SCALAR time, + const SCALAR dt) override; + + Tangent derivative_; +}; + +} // namespace internal +} // namespace core +} // namespace ct \ No newline at end of file diff --git a/ct_core/include/ct/core/integration/internal/StepperODEInt-impl.h b/ct_core/include/ct/core/integration/internal/StepperODEInt-impl.h new file mode 100644 index 000000000..8b2720797 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperODEInt-impl.h @@ -0,0 +1,99 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +namespace ct { +namespace core { +namespace internal { + +template +StepperODEInt::StepperODEInt() +{ +} + +template +StepperODEInt::~StepperODEInt() +{ +} + +template +void StepperODEInt::integrate_n_steps(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) +{ + boost::numeric::odeint::integrate_n_steps(stepper_, rhs, state, startTime, dt, numSteps); +} + +template +void StepperODEInt::integrate_n_steps(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) +{ + using namespace ct::core::internal; + boost::numeric::odeint::integrate_n_steps(stepper_, rhs, state, startTime, dt, numSteps, observer); +} + +template +void StepperODEInt::integrate_const(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dt) +{ + boost::numeric::odeint::integrate_const(stepper_, rhs, state, startTime, finalTime, dt); +} + +template +void StepperODEInt::integrate_const(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dt) +{ + boost::numeric::odeint::integrate_const(stepper_, rhs, state, startTime, finalTime, dt, observer); +} + +template +void StepperODEInt::integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dtInitial) +{ + boost::numeric::odeint::integrate_adaptive(stepper_, rhs, state, startTime, finalTime, dtInitial); +} + +template +void StepperODEInt::integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + const SCALAR dtInitial) +{ + boost::numeric::odeint::integrate_adaptive(stepper_, rhs, state, startTime, finalTime, dtInitial, observer); +} + +template +void StepperODEInt::integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const tpl::TimeArray& timeTrajectory, + SCALAR dtInitial) +{ + boost::numeric::odeint::integrate_times( + stepper_, rhs, state, &timeTrajectory.front(), &timeTrajectory.back() + 1, dtInitial, observer); +} + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperODEInt.h b/ct_core/include/ct/core/integration/internal/StepperODEInt.h new file mode 100644 index 000000000..865084393 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperODEInt.h @@ -0,0 +1,87 @@ +/********************************************************************************************************************** +This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. +Licensed under the BSD-2 license (see LICENSE file in main directory) +**********************************************************************************************************************/ + +#pragma once + +#include +#include "StepperBase.h" + +namespace ct { +namespace core { +namespace internal { + +/** + * @brief The interface to call the integration routines from ODEInt + * + * @tparam STEPPER The ODEInt stepper + * @tparam MATRIX The matrix type + * @tparam SCALAR The scalar type + */ +template +class StepperODEInt : public StepperBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using SCALAR = typename MANIFOLD::Scalar; + using Tangent = typename MANIFOLD::Tangent; + using SystemFunction_t = typename StepperBase::SystemFunction_t; + using ObserverFunction_t = typename StepperBase::ObserverFunction_t; + + StepperODEInt(); + virtual ~StepperODEInt(); + + virtual void integrate_n_steps(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) override; + + virtual void integrate_n_steps(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + size_t numSteps, + SCALAR dt) override; + + virtual void integrate_const(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dt) override; + + virtual void integrate_const(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dt) override; + + virtual void integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dtInitial = SCALAR(0.01)) override; + + virtual void integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + const SCALAR dtInitial = SCALAR(0.01)) override; + + virtual void integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const tpl::TimeArray& timeTrajectory, + SCALAR dtInitial = SCALAR(0.01)) override; + +private: + STEPPER stepper_; +}; + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperODEIntControlled-impl.h b/ct_core/include/ct/core/integration/internal/StepperODEIntControlled-impl.h new file mode 100644 index 000000000..d5e5d9a30 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperODEIntControlled-impl.h @@ -0,0 +1,53 @@ +#pragma once + +namespace ct { +namespace core { +namespace internal { + +template +StepperODEIntControlled::StepperODEIntControlled() +{ + stepperControlled_ = boost::numeric::odeint::make_controlled(this->absErrTol_, this->relErrTol_, stepper_); +} + +template +StepperODEIntControlled::~StepperODEIntControlled() +{ +} + +template +void StepperODEIntControlled::integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dtInitial) +{ + boost::numeric::odeint::integrate_adaptive(stepperControlled_, rhs, state, startTime, finalTime, dtInitial); +} + +template +void StepperODEIntControlled::integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + const SCALAR dtInitial) +{ + boost::numeric::odeint::integrate_adaptive( + stepperControlled_, rhs, state, startTime, finalTime, dtInitial, observer); +} + +template +void StepperODEIntControlled::integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const tpl::TimeArray& timeTrajectory, + SCALAR dtInitial) +{ + boost::numeric::odeint::integrate_times( + stepperControlled_, rhs, state, &timeTrajectory.front(), &timeTrajectory.back() + 1, dtInitial, observer); +} + +} // namespace internal +} // namespace core +} // namespace ct \ No newline at end of file diff --git a/ct_core/include/ct/core/integration/internal/StepperODEIntControlled.h b/ct_core/include/ct/core/integration/internal/StepperODEIntControlled.h new file mode 100644 index 000000000..c4668ac74 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperODEIntControlled.h @@ -0,0 +1,62 @@ +#pragma once + +#include +#include "SteppersODEIntDefinitions.h" +#include "../manifIntegration.h" +#include "../eigenIntegration.h" + +namespace ct { +namespace core { +namespace internal { + + +/** + * @brief The interface to call ODEInt Controlled integration routines + * + * @tparam STEPPER The ODEInt stepper type + * @tparam MATRIX The Matrix Type + * @tparam SCALAR The Scalar + */ +template +class StepperODEIntControlled : public StepperODEInt +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef typename boost::numeric::odeint::result_of::make_controlled::type StepperControlled; + + using SCALAR = typename MANIFOLD::Scalar; + using Tangent = typename MANIFOLD::Tangent; + using SystemFunction_t = typename StepperODEInt::SystemFunction_t; + using ObserverFunction_t = typename StepperODEInt::ObserverFunction_t; + + StepperODEIntControlled(); + virtual ~StepperODEIntControlled(); + + virtual void integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dtInitial = SCALAR(0.01)) override; + + virtual void integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + const SCALAR dtInitial = SCALAR(0.01)) override; + + virtual void integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const tpl::TimeArray& timeTrajectory, + SCALAR dtInitial = SCALAR(0.01)) override; + +private: + STEPPER stepper_; + StepperControlled stepperControlled_; +}; + +} // namespace internal +} // namespace core +} // namespace ct \ No newline at end of file diff --git a/ct_core/include/ct/core/integration/internal/StepperODEIntDenseOutput-impl.h b/ct_core/include/ct/core/integration/internal/StepperODEIntDenseOutput-impl.h new file mode 100644 index 000000000..e71b34425 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperODEIntDenseOutput-impl.h @@ -0,0 +1,55 @@ +#pragma once + +namespace ct { +namespace core { +namespace internal { + +template +StepperODEIntDenseOutput::StepperODEIntDenseOutput() +{ + stepperDense_ = boost::numeric::odeint::make_dense_output(this->absErrTol_, this->relErrTol_, stepper_); +} + +template +StepperODEIntDenseOutput::~StepperODEIntDenseOutput() +{ +} + +template +void StepperODEIntDenseOutput::integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dtInitial) +{ + stepperDense_.initialize(state, startTime, dtInitial); + boost::numeric::odeint::integrate_adaptive(stepperDense_, rhs, state, startTime, finalTime, dtInitial); +} + +template +void StepperODEIntDenseOutput::integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + const SCALAR dtInitial) +{ + stepperDense_.initialize(state, startTime, dtInitial); + boost::numeric::odeint::integrate_adaptive(stepperDense_, rhs, state, startTime, finalTime, dtInitial, observer); +} + +template +void StepperODEIntDenseOutput::integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const tpl::TimeArray& timeTrajectory, + SCALAR dtInitial) +{ + stepperDense_.initialize(state, timeTrajectory.front(), dtInitial); + boost::numeric::odeint::integrate_times( + stepperDense_, rhs, state, &timeTrajectory.front(), &timeTrajectory.back() + 1, dtInitial, observer); +} + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperODEIntDenseOutput.h b/ct_core/include/ct/core/integration/internal/StepperODEIntDenseOutput.h new file mode 100644 index 000000000..42d40a265 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperODEIntDenseOutput.h @@ -0,0 +1,62 @@ +#pragma once + +#include +#include "SteppersODEIntDefinitions.h" +#include "StepperODEInt.h" +#include "../manifIntegration.h" +#include "../eigenIntegration.h" + +namespace ct { +namespace core { +namespace internal { + +/** + * @brief The interface to call ODEInt Dense Output Integration routines + * + * @tparam STEPPER The ODEInt stepper + * @tparam MATRIX The matrix type + * @tparam SCALAR The scalar type + */ +template +class StepperODEIntDenseOutput : public StepperODEInt +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef typename boost::numeric::odeint::result_of::make_dense_output::type StepperDense; + + using SCALAR = typename MANIFOLD::Scalar; + using Tangent = typename MANIFOLD::Tangent; + using SystemFunction_t = typename StepperODEInt::SystemFunction_t; + using ObserverFunction_t = typename StepperODEInt::ObserverFunction_t; + + StepperODEIntDenseOutput(); + virtual ~StepperODEIntDenseOutput(); + + virtual void integrate_adaptive(const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + SCALAR dtInitial = SCALAR(0.01)) override; + + virtual void integrate_adaptive(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const SCALAR& startTime, + const SCALAR& finalTime, + const SCALAR dtInitial = SCALAR(0.01)) override; + + virtual void integrate_times(ObserverFunction_t observer, + const SystemFunction_t& rhs, + MANIFOLD& state, + const tpl::TimeArray& timeTrajectory, + SCALAR dtInitial = SCALAR(0.01)) override; + +private: + STEPPER stepper_; + StepperDense stepperDense_; +}; + +} // namespace internal +} // namespace core +} // namespace ct diff --git a/ct_core/include/ct/core/integration/internal/StepperRK4CT-impl.h b/ct_core/include/ct/core/integration/internal/StepperRK4CT-impl.h new file mode 100644 index 000000000..77890d9c1 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperRK4CT-impl.h @@ -0,0 +1,34 @@ +#pragma once + +namespace ct { +namespace core { +namespace internal { + +template +StepperRK4CT::StepperRK4CT() : oneSixth_(SCALAR(1.0 / 6.0)) +{ +} + +template +StepperRK4CT::~StepperRK4CT() +{ +} + +template +void StepperRK4CT::do_step(const SystemFunction_t& rhs, + MANIFOLD& stateInOut, + const SCALAR time, + const SCALAR dt) +{ + SCALAR halfStep = SCALAR(0.5) * dt; + SCALAR timePlusHalfStep = time + halfStep; + rhs(stateInOut, k1_, time); + rhs(stateInOut + k1_ * halfStep, k2_, timePlusHalfStep); + rhs(stateInOut + k2_ * halfStep, k3_, timePlusHalfStep); + rhs(stateInOut + k3_ * dt, k4_, time + dt); + stateInOut = stateInOut + (k1_ + k2_ * SCALAR(2.0) + k3_ * SCALAR(2.0) + k4_) * oneSixth_ * dt; +} + +} // namespace internal +} // namespace core +} // namespace ct \ No newline at end of file diff --git a/ct_core/include/ct/core/integration/internal/StepperRK4CT.h b/ct_core/include/ct/core/integration/internal/StepperRK4CT.h new file mode 100644 index 000000000..986c1aa26 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/StepperRK4CT.h @@ -0,0 +1,43 @@ +#pragma once + +#include "StepperCTBase.h" + +namespace ct { +namespace core { +namespace internal { + +/** + * @brief Custom implementation of the rk4 integration scheme + * + * @tparam MATRIX The matrix type + * @tparam SCALAR The scalar type + */ +template +class StepperRK4CT : public StepperCTBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using SCALAR = typename MANIFOLD::Scalar; + using Tangent = typename MANIFOLD::Tangent; + using SystemFunction_t = typename StepperCTBase::SystemFunction_t; + + StepperRK4CT(); + virtual ~StepperRK4CT(); + +private: + virtual void do_step(const SystemFunction_t& rhs, + MANIFOLD& stateInOut, + const SCALAR time, + const SCALAR dt) override; + + Tangent k1_; + Tangent k2_; + Tangent k3_; + Tangent k4_; + SCALAR oneSixth_; +}; + +} // namespace internal +} // namespace core +} // namespace ct \ No newline at end of file diff --git a/ct_core/include/ct/core/integration/internal/SteppersCT.h b/ct_core/include/ct/core/integration/internal/SteppersCT.h deleted file mode 100644 index 0a3fb7473..000000000 --- a/ct_core/include/ct/core/integration/internal/SteppersCT.h +++ /dev/null @@ -1,136 +0,0 @@ -/********************************************************************************************************************** -This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. -Licensed under the BSD-2 license (see LICENSE file in main directory) -**********************************************************************************************************************/ - -#pragma once - -#include -#include "StepperBase.h" - -namespace ct { -namespace core { -namespace internal { - - -/** - * @brief The stepper interface for custom steppers - * - * @tparam MATRIX The Matrix type to be integrated - * @tparam SCALAR The scalar type - */ -template -class StepperCTBase : public StepperBase -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - virtual void integrate_n_steps(const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - size_t numSteps, - SCALAR dt) override - { - SCALAR time = startTime; - for (size_t i = 0; i < numSteps; ++i) - { - do_step(rhs, state, time, dt); - time += dt; - } - } - - virtual void integrate_n_steps(std::function observe, - const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - size_t numSteps, - SCALAR dt) override - { - SCALAR time = startTime; - - for (size_t i = 0; i < numSteps; ++i) - { - do_step(rhs, state, time, dt); - time += dt; - observe(state, time); - } - } - - /** - * @brief Implements a single step of the integration scheme - * - * @param[in] rhs The ODE - * @param[in, out] stateInOut The state - * @param[in] time The integration time - * @param[in] dt The integration timestep - */ - virtual void do_step(const std::function& rhs, - MATRIX& stateInOut, - const SCALAR time, - const SCALAR dt) = 0; -}; - - -/** - * @brief Custom implementation of the euler stepper - * - * @tparam MATRIX The matrix type - * @tparam SCALAR The scalar type - */ -template -class StepperEulerCT : public StepperCTBase -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - StepperEulerCT() {} -private: - virtual void do_step(const std::function& rhs, - MATRIX& stateInOut, - const SCALAR time, - const SCALAR dt) override - { - rhs(stateInOut, derivative_, time); - stateInOut += dt * derivative_; - } - - MATRIX derivative_; -}; - -/** - * @brief Custom implementation of the rk4 integration scheme - * - * @tparam MATRIX The matrix type - * @tparam SCALAR The scalar type - */ -template -class StepperRK4CT : public StepperCTBase -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - StepperRK4CT() : oneSixth_(SCALAR(1.0 / 6.0)) {} -private: - virtual void do_step(const std::function& rhs, - MATRIX& stateInOut, - const SCALAR time, - const SCALAR dt) override - { - SCALAR halfStep = SCALAR(0.5) * dt; - SCALAR timePlusHalfStep = time + halfStep; - rhs(stateInOut, k1_, time); - rhs(stateInOut + halfStep * k1_, k2_, timePlusHalfStep); - rhs(stateInOut + halfStep * k2_, k3_, timePlusHalfStep); - rhs(stateInOut + dt * k3_, k4_, time + dt); - stateInOut += oneSixth_ * dt * (k1_ + SCALAR(2.0) * k2_ + SCALAR(2.0) * k3_ + k4_); - } - - MATRIX k1_; - MATRIX k2_; - MATRIX k3_; - MATRIX k4_; - SCALAR oneSixth_; -}; -} -} -} diff --git a/ct_core/include/ct/core/integration/internal/SteppersODEInt.h b/ct_core/include/ct/core/integration/internal/SteppersODEInt.h deleted file mode 100644 index 731baa630..000000000 --- a/ct_core/include/ct/core/integration/internal/SteppersODEInt.h +++ /dev/null @@ -1,214 +0,0 @@ -/********************************************************************************************************************** -This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich. -Licensed under the BSD-2 license (see LICENSE file in main directory) -**********************************************************************************************************************/ - -#pragma once - -#include -#include "SteppersODEIntDefinitions.h" - -namespace ct { -namespace core { -namespace internal { - -/** - * @brief The interface to call the integration routines from ODEInt - * - * @tparam STEPPER The ODEInt stepper - * @tparam MATRIX The matrix type - * @tparam SCALAR The scalar type - */ -template -class StepperODEInt : public StepperBase -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - StepperODEInt() {} - virtual void integrate_n_steps(const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - size_t numSteps, - SCALAR dt) override - { - boost::numeric::odeint::integrate_n_steps(stepper_, rhs, state, startTime, dt, numSteps); - } - - virtual void integrate_n_steps(std::function observer, - const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - size_t numSteps, - SCALAR dt) override - { - boost::numeric::odeint::integrate_n_steps(stepper_, rhs, state, startTime, dt, numSteps, observer); - } - - - virtual void integrate_const(const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - SCALAR dt) override - { - boost::numeric::odeint::integrate_const(stepper_, rhs, state, startTime, finalTime, dt); - } - - virtual void integrate_const(std::function observer, - const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - SCALAR dt) override - { - boost::numeric::odeint::integrate_const(stepper_, rhs, state, startTime, finalTime, dt, observer); - } - - virtual void integrate_adaptive(const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - SCALAR dtInitial = SCALAR(0.01)) override - { - boost::numeric::odeint::integrate_adaptive(stepper_, rhs, state, startTime, finalTime, dtInitial); - } - - virtual void integrate_adaptive(std::function observer, - const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - const SCALAR dtInitial = SCALAR(0.01)) override - { - boost::numeric::odeint::integrate_adaptive(stepper_, rhs, state, startTime, finalTime, dtInitial, observer); - } - - virtual void integrate_times(std::function observer, - const std::function& rhs, - MATRIX& state, - const tpl::TimeArray& timeTrajectory, - SCALAR dtInitial = SCALAR(0.01)) override - { - boost::numeric::odeint::integrate_times( - stepper_, rhs, state, &timeTrajectory.front(), &timeTrajectory.back() + 1, dtInitial, observer); - } - -private: - STEPPER stepper_; -}; - - -/** - * @brief The interface to call ODEInt Dense Output Integration routines - * - * @tparam STEPPER The ODEInt stepper - * @tparam MATRIX The matrix type - * @tparam SCALAR The scalar type - */ -template -class StepperODEIntDenseOutput : public StepperODEInt -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef typename boost::numeric::odeint::result_of::make_dense_output::type StepperDense; - - StepperODEIntDenseOutput() - { - stepperDense_ = boost::numeric::odeint::make_dense_output(this->absErrTol_, this->relErrTol_, stepper_); - } - - virtual void integrate_adaptive(const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - SCALAR dtInitial = SCALAR(0.01)) override - { - stepperDense_.initialize(state, startTime, dtInitial); - boost::numeric::odeint::integrate_adaptive(stepperDense_, rhs, state, startTime, finalTime, dtInitial); - } - - virtual void integrate_adaptive(std::function observer, - const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - const SCALAR dtInitial = SCALAR(0.01)) override - { - stepperDense_.initialize(state, startTime, dtInitial); - boost::numeric::odeint::integrate_adaptive( - stepperDense_, rhs, state, startTime, finalTime, dtInitial, observer); - } - - virtual void integrate_times(std::function observer, - const std::function& rhs, - MATRIX& state, - const tpl::TimeArray& timeTrajectory, - SCALAR dtInitial = SCALAR(0.01)) override - { - stepperDense_.initialize(state, timeTrajectory.front(), dtInitial); - boost::numeric::odeint::integrate_times( - stepperDense_, rhs, state, &timeTrajectory.front(), &timeTrajectory.back() + 1, dtInitial, observer); - } - -private: - STEPPER stepper_; - StepperDense stepperDense_; -}; - -/** - * @brief The interface to call ODEInt Controlled integration routines - * - * @tparam STEPPER The ODEInt stepper type - * @tparam MATRIX The Matrix Type - * @tparam SCALAR The Scalar - */ -template -class StepperODEIntControlled : public StepperODEInt -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef typename boost::numeric::odeint::result_of::make_controlled::type StepperControlled; - - StepperODEIntControlled() - { - stepperControlled_ = boost::numeric::odeint::make_controlled(this->absErrTol_, this->relErrTol_, stepper_); - } - - virtual void integrate_adaptive(const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - SCALAR dtInitial = SCALAR(0.01)) override - { - boost::numeric::odeint::integrate_adaptive(stepperControlled_, rhs, state, startTime, finalTime, dtInitial); - } - - virtual void integrate_adaptive(std::function observer, - const std::function& rhs, - MATRIX& state, - const SCALAR& startTime, - const SCALAR& finalTime, - const SCALAR dtInitial = SCALAR(0.01)) override - { - boost::numeric::odeint::integrate_adaptive( - stepperControlled_, rhs, state, startTime, finalTime, dtInitial, observer); - } - - virtual void integrate_times(std::function observer, - const std::function& rhs, - MATRIX& state, - const tpl::TimeArray& timeTrajectory, - SCALAR dtInitial = SCALAR(0.01)) override - { - boost::numeric::odeint::integrate_times( - stepperControlled_, rhs, state, &timeTrajectory.front(), &timeTrajectory.back() + 1, dtInitial, observer); - } - -private: - STEPPER stepper_; - StepperControlled stepperControlled_; -}; -} -} -} diff --git a/ct_core/include/ct/core/integration/internal/SteppersODEIntDefinitions.h b/ct_core/include/ct/core/integration/internal/SteppersODEIntDefinitions.h index 7713108e4..3a439a077 100644 --- a/ct_core/include/ct/core/integration/internal/SteppersODEIntDefinitions.h +++ b/ct_core/include/ct/core/integration/internal/SteppersODEIntDefinitions.h @@ -5,7 +5,9 @@ Licensed under the BSD-2 license (see LICENSE file in main directory) #pragma once +#include #include +#include "manif_operations.h" namespace ct { namespace core { @@ -13,100 +15,118 @@ namespace internal { /***************************************************************************** * Defining the (explicit) steppers *****************************************************************************/ -//! Simple Euler stepper -template -using euler_t = boost::numeric::odeint::euler, - SCALAR, - Eigen::Matrix, - SCALAR, - boost::numeric::odeint::vector_space_algebra>; + +/** + * @brief this macro selects an "operations-class" for the boost odeint steppers + * Based on the MANIFOLD in use, it either selects the default_operations by boost for the + * Euclidean case, or the custom CT implementation for manif operations. + */ +#define SELECT_OPERATIONS(MANIFOLD) \ + typename std::conditional_t::value, boost::numeric::odeint::default_operations, \ + boost::numeric::odeint::manif_operations> + +//! Simple Euler stepper, uses specialized operations set based on the manifold in use. +template +using euler_t = boost::numeric::odeint::euler; //! Modified Midpoint stepper -template -using modified_midpoint_t = boost::numeric::odeint::modified_midpoint, - SCALAR, - Eigen::Matrix, - SCALAR, - boost::numeric::odeint::vector_space_algebra>; +template +using modified_midpoint_t = boost::numeric::odeint::modified_midpoint; //! Runge-Kutta4 stepper -template -using runge_kutta_4_t = boost::numeric::odeint::runge_kutta4, - SCALAR, - Eigen::Matrix, - SCALAR, - boost::numeric::odeint::vector_space_algebra>; +template +using runge_kutta_4_t = boost::numeric::odeint::runge_kutta4; //! Runge-Kutta Dormand Price 5 stepper -template -using runge_kutta_dopri5_t = boost::numeric::odeint::runge_kutta_dopri5, - SCALAR, - Eigen::Matrix, - SCALAR, - boost::numeric::odeint::vector_space_algebra>; +template +using runge_kutta_dopri5_t = boost::numeric::odeint::runge_kutta_dopri5; //! Runge Kutta Fehlberg 78 stepper -template -using runge_kutta_fehlberg78_t = boost::numeric::odeint::runge_kutta_fehlberg78, - SCALAR, - Eigen::Matrix, +template +using runge_kutta_fehlberg78_t = boost::numeric::odeint::runge_kutta_fehlberg78; + boost::numeric::odeint::vector_space_algebra, + SELECT_OPERATIONS(MANIFOLD)>; //! Bulirsch Stoer stepper -template -using bulirsch_stoer_t = boost::numeric::odeint::bulirsch_stoer, - SCALAR, - Eigen::Matrix, - SCALAR, - boost::numeric::odeint::vector_space_algebra>; +template +using bulirsch_stoer_t = boost::numeric::odeint::bulirsch_stoer; //! Adams Bashforth stepper -template +template using adams_bashforth_uncontrolled_t = boost::numeric::odeint::adams_bashforth, // state - SCALAR, // typename value - Eigen::Matrix, // derivative - SCALAR, // typename time - boost::numeric::odeint::vector_space_algebra>; + MANIFOLD, + typename MANIFOLD::Scalar, + typename MANIFOLD::Tangent, + typename MANIFOLD::Scalar, + boost::numeric::odeint::vector_space_algebra, + SELECT_OPERATIONS(MANIFOLD)>; /***************************************************************************** * Defining the symplectic steppers *****************************************************************************/ -//! A symplictic rk type stepper -template -using symplectic_rk_t = boost::numeric::odeint::symplectic_rkn_sb3a_mclachlan, - Eigen::Matrix, - SCALAR, - Eigen::Matrix, - Eigen::Matrix, - SCALAR, - boost::numeric::odeint::vector_space_algebra>; +//! A symplectic rk type stepper +template +using symplectic_rk_t = boost::numeric::odeint::symplectic_rkn_sb3a_mclachlan; // Symplectic euler stepper -template -using symplectic_euler_t = boost::numeric::odeint::symplectic_euler, - Eigen::Matrix, - SCALAR, - Eigen::Matrix, - Eigen::Matrix, - SCALAR, - boost::numeric::odeint::vector_space_algebra>; +template +using symplectic_euler_t = boost::numeric::odeint::symplectic_euler; -/***************************************************************************** - * Defining the (implicit) steppers - *****************************************************************************/ -// // works only for boost 1.56 or higher -//template -//using adams_bashforth_moulton_uncontrolled_t = -// boost::numeric::odeint::adams_bashforth_moulton< -// STEPS, -// Eigen::Matrix, // state -// double, // typename value -// Eigen::Matrix, // derivative -// double, // typename time -// boost::numeric::odeint::vector_space_algebra> ; +/**************************************************************************** + Defining the (implicit) steppers +****************************************************************************/ +// works only for boost 1.56 or higher +template +using adams_bashforth_moulton_uncontrolled_t = boost::numeric::odeint::adams_bashforth_moulton; } // namespace internal } // namespace core diff --git a/ct_core/include/ct/core/integration/internal/manif_operations.h b/ct_core/include/ct/core/integration/internal/manif_operations.h new file mode 100644 index 000000000..45f6e1c13 --- /dev/null +++ b/ct_core/include/ct/core/integration/internal/manif_operations.h @@ -0,0 +1,813 @@ +// clang-format off + +#pragma once + +#include + +#include +#include + +#include + +namespace boost { +namespace numeric { +namespace odeint { + + +/** + * @brief custom CT "operations" set for boost odeint, required to make + * manif-types compatible with boost integration + * + * The boost "default_operations" classes, wich are used by the different + * stepper methods, are implemented such that + * they are not naturally compatible with manifold operations (e.g. scaling, + * addition, etc.) + * + * Here, we provide our own operations set, which attempts to respect the + * constraints of manifold operations where possible. + * When an operation required by boost is not compatible with manif, an + * exception is thrown. + * + * @note: not all operations can be ported to manifolds + */ +struct manif_operations { + /* + template< class Fac1 = double > + struct scale + { + const Fac1 m_alpha1; + + scale( Fac1 alpha1 ) : m_alpha1( alpha1 ) { } + + template< class T1 > + void operator()( T1 &t1 ) const + { + t1 *= m_alpha1; + } + + typedef void result_type; + }; + + template< class Fac1 = double > + struct scale_sum1 + { + const Fac1 m_alpha1; + + scale_sum1( Fac1 alpha1 ) : m_alpha1( alpha1 ) { } + + template< class T1 , class T2 > + void operator()( T1 &t1 , const T2 &t2 ) const + { + t1 = m_alpha1 * t2; + } + + typedef void result_type; + }; + */ + + template + struct scale_sum2 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum2(Fac1 alpha1, Fac2 alpha2) : m_alpha1(alpha1), m_alpha2(alpha2) {} + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum2 operator lost validity."); +#endif + // removing scaling by alpha, since that step would break our lifted + // manifold setup + t1 = /* m_alpha1 * */ t2 + t3 * m_alpha2; + } + + typedef void result_type; + }; + + template + struct scale_sum3 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + + scale_sum3(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3) + : m_alpha1(alpha1), m_alpha2(alpha2), m_alpha3(alpha3) {} + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum3 operator lost validity."); +#endif + t1 = /*m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4; + } + + typedef void result_type; + }; + + template + struct scale_sum4 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + + scale_sum4(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum4 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5; + } + + typedef void result_type; + }; + + template + struct scale_sum5 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + + scale_sum5(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum5 operator lost validity."); +#endif + t1 = /*m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6; + } + + typedef void result_type; + }; + + template + struct scale_sum6 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + + scale_sum6(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum6 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7; + } + + typedef void result_type; + }; + + template + struct scale_sum7 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + + scale_sum7(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, + const T8 &t8) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum7 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8; + } + + typedef void result_type; + }; + + template + struct scale_sum8 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + + scale_sum8(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7, Fac8 alpha8) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7), + m_alpha8(alpha8) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, const T8 &t8, + const T9 &t9) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum8 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9; + } + + typedef void result_type; + }; + + template + struct scale_sum9 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + + scale_sum9(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7, Fac8 alpha8, Fac9 alpha9) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7), + m_alpha8(alpha8), + m_alpha9(alpha9) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, const T8 &t8, + const T9 &t9, const T10 &t10) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum9 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + + m_alpha9 * t10; + } + + typedef void result_type; + }; + + template + struct scale_sum10 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + + scale_sum10(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7, Fac8 alpha8, Fac9 alpha9, + Fac10 alpha10) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7), + m_alpha8(alpha8), + m_alpha9(alpha9), + m_alpha10(alpha10) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, const T8 &t8, + const T9 &t9, const T10 &t10, const T11 &t11) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum10 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + + m_alpha9 * t10 + m_alpha10 * t11; + } + + typedef void result_type; + }; + + template + struct scale_sum11 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + + scale_sum11(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7, Fac8 alpha8, Fac9 alpha9, + Fac10 alpha10, Fac11 alpha11) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7), + m_alpha8(alpha8), + m_alpha9(alpha9), + m_alpha10(alpha10), + m_alpha11(alpha11) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, const T8 &t8, + const T9 &t9, const T10 &t10, const T11 &t11, + const T12 &t12) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum11 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12; + } + + typedef void result_type; + }; + + template + struct scale_sum12 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + const Fac12 m_alpha12; + + scale_sum12(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7, Fac8 alpha8, Fac9 alpha9, + Fac10 alpha10, Fac11 alpha11, Fac12 alpha12) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7), + m_alpha8(alpha8), + m_alpha9(alpha9), + m_alpha10(alpha10), + m_alpha11(alpha11), + m_alpha12(alpha12) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, const T8 &t8, + const T9 &t9, const T10 &t10, const T11 &t11, + const T12 &t12, const T13 &t13) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum12 operator lost validity."); +#endif + t1 = /*m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13; + } + + typedef void result_type; + }; + + template + struct scale_sum13 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + const Fac12 m_alpha12; + const Fac13 m_alpha13; + + scale_sum13(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7, Fac8 alpha8, Fac9 alpha9, + Fac10 alpha10, Fac11 alpha11, Fac12 alpha12, Fac13 alpha13) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7), + m_alpha8(alpha8), + m_alpha9(alpha9), + m_alpha10(alpha10), + m_alpha11(alpha11), + m_alpha12(alpha12), + m_alpha13(alpha13) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, const T8 &t8, + const T9 &t9, const T10 &t10, const T11 &t11, + const T12 &t12, const T13 &t13, const T14 &t14) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum13 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + + m_alpha12 * t13 + m_alpha13 * t14; + } + + typedef void result_type; + }; + + template + struct scale_sum14 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + const Fac3 m_alpha3; + const Fac4 m_alpha4; + const Fac5 m_alpha5; + const Fac6 m_alpha6; + const Fac7 m_alpha7; + const Fac8 m_alpha8; + const Fac9 m_alpha9; + const Fac10 m_alpha10; + const Fac11 m_alpha11; + const Fac12 m_alpha12; + const Fac13 m_alpha13; + const Fac14 m_alpha14; + + scale_sum14(Fac1 alpha1, Fac2 alpha2, Fac3 alpha3, Fac4 alpha4, Fac5 alpha5, + Fac6 alpha6, Fac7 alpha7, Fac8 alpha8, Fac9 alpha9, + Fac10 alpha10, Fac11 alpha11, Fac12 alpha12, Fac13 alpha13, + Fac14 alpha14) + : m_alpha1(alpha1), + m_alpha2(alpha2), + m_alpha3(alpha3), + m_alpha4(alpha4), + m_alpha5(alpha5), + m_alpha6(alpha6), + m_alpha7(alpha7), + m_alpha8(alpha8), + m_alpha9(alpha9), + m_alpha10(alpha10), + m_alpha11(alpha11), + m_alpha12(alpha12), + m_alpha13(alpha13), + m_alpha14(alpha14) {} + + template + void operator()(T1 &t1, const T2 &t2, const T3 &t3, const T4 &t4, + const T5 &t5, const T6 &t6, const T7 &t7, const T8 &t8, + const T9 &t9, const T10 &t10, const T11 &t11, + const T12 &t12, const T13 &t13, const T14 &t14, + const T15 &t15) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum14 operator lost validity."); +#endif + t1 = /* m_alpha1 * */ t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + + m_alpha12 * t13 + m_alpha13 * t14 + m_alpha14 * t15; + } + + typedef void result_type; + }; + + template + struct scale_sum_swap2 { + const Fac1 m_alpha1; + const Fac2 m_alpha2; + + scale_sum_swap2(Fac1 alpha1, Fac2 alpha2) + : m_alpha1(alpha1), m_alpha2(alpha2) {} + template + void operator()(T1 &t1, T2 &t2, const T3 &t3) const { +#ifndef CT_DISABLE_NUMERIC_MANIFOLD_CHECKS // set this definition for disabling checks + if (m_alpha1 != 1.0) + throw std::runtime_error( + "manif_operations: m_alpha1 is not equal to " + "1.0 ... scale_sum_swap2 operator lost " + "validity."); +#endif + const T1 tmp(t1); + t1 = /*m_alpha1 * */ t2 + m_alpha2 * t3; + t2 = tmp; + } + + typedef void result_type; + }; + + /* + * for usage in for_each2 + * + * Works with boost::units by eliminating the unit + */ + /* + template< class Fac1 = double > + struct rel_error + { + const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; + + rel_error( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) + : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , + m_a_dxdt( a_dxdt ) { } + + template< class T1 , class T2 , class T3 > + void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const + { + using std::abs; + set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + + m_eps_rel * ( m_a_x * abs( get_unit_value( t1 ) ) + m_a_dxdt * abs( + get_unit_value( t2 ) ) ) ) ); + } + + typedef void result_type; + }; */ + + // /* + // * for usage in for_each3 + // * + // * used in the controller for the rosenbrock4 method + // * + // * Works with boost::units by eliminating the unit + // */ + // template< class Fac1 = double > + // struct default_rel_error + // { + // const Fac1 m_eps_abs , m_eps_rel ; + + // default_rel_error( Fac1 eps_abs , Fac1 eps_rel ) + // : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) { } + + // /* + // * xerr = xerr / ( eps_abs + eps_rel * max( x , x_old ) ) + // */ + // template< class T1 , class T2 , class T3 > + // void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const + // { + // BOOST_USING_STD_MAX(); + // using std::abs; + // Fac1 x1 = abs( get_unit_value( t1 ) ) , x2 = abs( get_unit_value( + // t2 ) ); + // set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + + // m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( x1 , x2 ) ) ); + // } + + // typedef void result_type; + // }; + + // /* + // * for usage in reduce + // */ + + // template< class Value > + // struct maximum + // { + // template< class Fac1 , class Fac2 > + // Value operator()( Fac1 t1 , const Fac2 t2 ) const + // { + // using std::abs; + // Value a1 = abs( get_unit_value( t1 ) ) , a2 = abs( get_unit_value( + // t2 ) ); + // return ( a1 < a2 ) ? a2 : a1 ; + // } + + // typedef Value result_type; + // }; + + // template< class Fac1 = double > + // struct rel_error_max + // { + // const Fac1 m_eps_abs , m_eps_rel; + + // rel_error_max( Fac1 eps_abs , Fac1 eps_rel ) + // : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) + // { } + + // template< class Res , class T1 , class T2 , class T3 > + // Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err + // ) + // { + // BOOST_USING_STD_MAX(); + // using std::abs; + // Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel + // * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) + // ); + // return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp ); + // } + // }; + + // template< class Fac1 = double > + // struct rel_error_max2 + // { + // const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; + + // rel_error_max2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) + // : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , + // m_a_dxdt( a_dxdt ) + // { } + + // template< class Res , class T1 , class T2 , class T3 , class T4 > + // Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 + // &dxdt_old , const T4 &x_err ) + // { + // BOOST_USING_STD_MAX(); + // using std::abs; + // Res tmp = abs( get_unit_value( x_err ) ) / + // ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( + // x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) + // ); + // return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp ); + // } + // }; + + // template< class Fac1 = double > + // struct rel_error_l2 + // { + // const Fac1 m_eps_abs , m_eps_rel; + + // rel_error_l2( Fac1 eps_abs , Fac1 eps_rel ) + // : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) + // { } + + // template< class Res , class T1 , class T2 , class T3 > + // Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err + // ) + // { + // BOOST_USING_STD_MAX(); + // using std::abs; + // Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel + // * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) + // ); + // return r + tmp * tmp; + // } + // }; + + // template< class Fac1 = double > + // struct rel_error_l2_2 + // { + // const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt; + + // rel_error_l2_2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt ) + // : m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , + // m_a_dxdt( a_dxdt ) + // { } + + // template< class Res , class T1 , class T2 , class T3 , class T4 > + // Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 + // &dxdt_old , const T4 &x_err ) + // { + // using std::abs; + // Res tmp = abs( get_unit_value( x_err ) ) / + // ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( + // x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) + // ); + // return r + tmp * tmp; + // } + // }; +}; + +} // odeint +} // numeric +} // boost + + +// clang-format on \ No newline at end of file diff --git a/ct_core/include/ct/core/integration/manifIntegration.h b/ct_core/include/ct/core/integration/manifIntegration.h new file mode 100644 index 000000000..e9564755f --- /dev/null +++ b/ct_core/include/ct/core/integration/manifIntegration.h @@ -0,0 +1,91 @@ + +#pragma once + +#ifdef CT_USE_MANIF + +#include +#include +#include + +/** + * @brief Necessary routines for manif matrices to work with vector_space_algebra from boost ODEINT + * + * Lets boost ODEINT steppers treat the manif operations correctly, knowing how to add, multiply, etc. + * These operations will be used in the CT steppers, as well as in the manif_operations.h header + */ +namespace manif { + +// operator overload for adding manifolds +template