├── LICENSE ├── README.md ├── hunter_base ├── CMakeLists.txt ├── ascent │ ├── CMakeLists.txt │ ├── Doxyfile │ ├── LICENSE │ ├── README.md │ ├── catch2 │ │ └── catch.hpp │ ├── cmake │ │ └── ascentConfig.cmake.in │ ├── examples │ │ ├── CMakeLists.txt │ │ ├── airy │ │ │ ├── CMakeLists.txt │ │ │ └── airy.cpp │ │ ├── lorenz │ │ │ ├── CMakeLists.txt │ │ │ └── lorenz.cpp │ │ ├── modular-spring-damper │ │ │ ├── Body.h │ │ │ ├── CMakeLists.txt │ │ │ ├── Damper.h │ │ │ ├── Main.cpp │ │ │ └── Spring.h │ │ ├── pliny-fountain │ │ │ ├── CMakeLists.txt │ │ │ └── fountain.cpp │ │ └── sampling │ │ │ ├── CMakeLists.txt │ │ │ └── sampling.cpp │ ├── include │ │ ├── CMakeLists.txt │ │ └── ascent │ │ │ ├── Ascent.h │ │ │ ├── ChaiEngine.h │ │ │ ├── Param.h │ │ │ ├── ParamV.h │ │ │ ├── Recorder.h │ │ │ ├── System.h │ │ │ ├── Utility.h │ │ │ ├── Vector.h │ │ │ ├── algorithms │ │ │ └── Derivative.h │ │ │ ├── containers │ │ │ └── stack.h │ │ │ ├── direct │ │ │ └── State.h │ │ │ ├── integrators │ │ │ ├── DOPRI45.h │ │ │ ├── Euler.h │ │ │ ├── Midpoint.h │ │ │ ├── PC233.h │ │ │ ├── RK2.h │ │ │ ├── RK4.h │ │ │ ├── RKMM.h │ │ │ └── RTAM4.h │ │ │ ├── integrators_direct │ │ │ ├── Euler.h │ │ │ └── RK4.h │ │ │ ├── integrators_modular │ │ │ ├── DOPRI45.h │ │ │ ├── Euler.h │ │ │ ├── Heun.h │ │ │ ├── Midpoint.h │ │ │ ├── ModularIntegrators.h │ │ │ ├── NCRK4.h │ │ │ ├── PC233.h │ │ │ ├── RK2.h │ │ │ ├── RK3.h │ │ │ ├── RK4.h │ │ │ ├── RTAM2.h │ │ │ ├── RTAM3.h │ │ │ └── RTAM4.h │ │ │ ├── modular │ │ │ ├── Link.h │ │ │ └── Module.h │ │ │ ├── threading │ │ │ ├── Pool.h │ │ │ └── Queue.h │ │ │ └── timing │ │ │ ├── Sampler.h │ │ │ ├── TimeAdvanced.h │ │ │ └── Timing.h │ └── unit_tests │ │ ├── CMakeLists.txt │ │ └── src │ │ └── main.cpp ├── include │ └── hunter_base │ │ ├── bicycle_model.hpp │ │ ├── hunter_messenger.hpp │ │ └── hunter_params.hpp ├── launch │ ├── hunter_base.launch │ └── hunter_base_sim.launch ├── package.xml └── src │ ├── bicycle_model.cpp │ ├── hunter_base_node.cpp │ ├── hunter_base_sim_node.cpp │ ├── hunter_messenger.cpp │ └── hunter_status_node.cpp ├── hunter_bringup ├── CMakeLists.txt ├── launch │ ├── hunter_robot_base.launch │ ├── hunter_teleop_keyboard.launch │ └── includes │ │ └── nodelet.launch.xml ├── package.xml └── scripts │ ├── bringup_can2usb.bash │ └── setup_can2usb.bash └── hunter_msgs ├── CMakeLists.txt ├── msg ├── HunterBmsStatus.msg ├── HunterDriverState.msg ├── HunterMotorState.msg └── HunterStatus.msg └── package.xml /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, WestonRobot 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS Packages for Hunter Mobile Base 2 | 3 | ## Packages 4 | 5 | * hunter_base: a ROS wrapper around Hunter SDK to monitor and control the robot 6 | * hunter_bringup: launch and configuration files to start ROS nodes 7 | * hunter_msgs: hunter related message definitions 8 | 9 | 10 | ## Communication interface setup 11 | 12 | Please refer to the [README](https://github.com/agilexrobotics/ugv_sdk#hardware-interface) of "ugv_sdk" package for setup of communication interfaces. 13 | 14 | #### Note on CAN interface on Nvidia Jetson Platforms 15 | 16 | Nvidia Jeston TX2/Xavier/XavierNX have CAN controller(s) integrated in the main SOC. If you're using a dev kit, you need to add a CAN transceiver for proper CAN communication. 17 | 18 | ## Basic usage of the ROS package 19 | 20 | 1. Install dependent packages 21 | 22 | ``` 23 | $ sudo apt install -y ros-$ROS_DISTRO-teleop-twist-keyboard 24 | ``` 25 | 26 | 2. Clone the packages into your catkin workspace and compile 27 | 28 | (the following instructions assume your catkin workspace is at: ~/catkin_ws/src) 29 | 30 | ``` 31 | $ cd ~/catkin_ws/src 32 | $ git clone https://github.com/agilexrobotics/ugv_sdk.git 33 | $ git clone https://github.com/agilexrobotics/hunter_ros.git 34 | $ cd .. 35 | $ catkin_make 36 | ``` 37 | 38 | 3. Setup CAN-To-USB adapter 39 | 40 | * Enable gs_usb kernel module(If you have already added this module, you do not need to add it) 41 | ``` 42 | $ sudo modprobe gs_usb 43 | ``` 44 | 45 | * first time use hunter-ros package 46 | ``` 47 | $ rosrun hunter_bringup setup_can2usb.bash 48 | ``` 49 | 50 | * if not the first time use hunter-ros package(Run this command every time you turn off the power) 51 | ``` 52 | $ rosrun hunter_bringup bringup_can2usb.bash 53 | ``` 54 | 55 | * Testing command 56 | ``` 57 | # receiving data from can0 58 | $ candump can0 59 | ``` 60 | 61 | 4. Launch ROS nodes 62 | 63 | * Start the base node for the real robot 64 | 65 | ``` 66 | $ roslaunch hunter_bringup hunter_robot_base.launch 67 | ``` 68 | * Start the keyboard node to control the real robot 69 | 70 | ``` 71 | $ roslaunch hunter_bringup hunter_teleop_keyboard.launch 72 | ``` 73 | 74 | **SAFETY PRECAUSION**: 75 | 76 | Always have your remote controller ready to take over the control whenever necessary. 77 | -------------------------------------------------------------------------------- /hunter_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hunter_base) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++17) 6 | ## Set compiler to use c++ 17 features 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | 10 | ## Find catkin macros and libraries 11 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 12 | ## is used, also find other catkin packages 13 | find_package(catkin REQUIRED COMPONENTS 14 | hunter_msgs 15 | roscpp 16 | sensor_msgs 17 | std_msgs 18 | tf2 19 | tf2_ros 20 | ugv_sdk 21 | ) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | # find_package(Boost REQUIRED COMPONENTS system) 25 | add_subdirectory(ascent) 26 | 27 | ## Uncomment this if the package has a setup.py. This macro ensures 28 | ## modules and global scripts declared therein get installed 29 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 30 | # catkin_python_setup() 31 | 32 | ################################################ 33 | ## Declare ROS messages, services and actions ## 34 | ################################################ 35 | 36 | ## To declare and build messages, services or actions from within this 37 | ## package, follow these steps: 38 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 39 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 40 | ## * In the file package.xml: 41 | ## * add a build_depend tag for "message_generation" 42 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 43 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 44 | ## but can be declared for certainty nonetheless: 45 | ## * add a exec_depend tag for "message_runtime" 46 | ## * In this file (CMakeLists.txt): 47 | ## * add "message_generation" and every package in MSG_DEP_SET to 48 | ## find_package(catkin REQUIRED COMPONENTS ...) 49 | ## * add "message_runtime" and every package in MSG_DEP_SET to 50 | ## catkin_package(CATKIN_DEPENDS ...) 51 | ## * uncomment the add_*_files sections below as needed 52 | ## and list every .msg/.srv/.action file to be processed 53 | ## * uncomment the generate_messages entry below 54 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 55 | 56 | ## Generate messages in the 'msg' folder 57 | # add_message_files( 58 | # FILES 59 | # Message1.msg 60 | # Message2.msg 61 | # ) 62 | 63 | ## Generate services in the 'srv' folder 64 | # add_service_files( 65 | # FILES 66 | # Service1.srv 67 | # Service2.srv 68 | # ) 69 | 70 | ## Generate actions in the 'action' folder 71 | # add_action_files( 72 | # FILES 73 | # Action1.action 74 | # Action2.action 75 | # ) 76 | 77 | ## Generate added messages and services with any dependencies listed here 78 | # generate_messages( 79 | # DEPENDENCIES 80 | # hunter_msgs# sensor_msgs# std_msgs 81 | # ) 82 | 83 | ################################################ 84 | ## Declare ROS dynamic reconfigure parameters ## 85 | ################################################ 86 | 87 | ## To declare and build dynamic reconfigure parameters within this 88 | ## package, follow these steps: 89 | ## * In the file package.xml: 90 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 91 | ## * In this file (CMakeLists.txt): 92 | ## * add "dynamic_reconfigure" to 93 | ## find_package(catkin REQUIRED COMPONENTS ...) 94 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 95 | ## and list every .cfg file to be processed 96 | 97 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 98 | # generate_dynamic_reconfigure_options( 99 | # cfg/DynReconf1.cfg 100 | # cfg/DynReconf2.cfg 101 | # ) 102 | 103 | ################################### 104 | ## catkin specific configuration ## 105 | ################################### 106 | ## The catkin_package macro generates cmake config files for your package 107 | ## Declare things to be passed to dependent projects 108 | ## INCLUDE_DIRS: uncomment this if your package contains header files 109 | ## LIBRARIES: libraries you create in this project that dependent projects also need 110 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 111 | ## DEPENDS: system dependencies of this project that dependent projects also need 112 | catkin_package( 113 | INCLUDE_DIRS include 114 | LIBRARIES hunter_messenger 115 | # CATKIN_DEPENDS hunter_msgs wrp_sdk roscpp sensor_msgs std_msgs tf2 tf2_ros 116 | # DEPENDS system_lib 117 | ) 118 | 119 | ########### 120 | ## Build ## 121 | ########### 122 | 123 | ## Specify additional locations of header files 124 | ## Your package locations should be listed before other locations 125 | include_directories( 126 | include 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | # add_library(${PROJECT_NAME} 132 | # src/${PROJECT_NAME}/hunter_base.cpp 133 | # ) 134 | 135 | add_library(hunter_messenger STATIC 136 | src/hunter_messenger.cpp 137 | src/bicycle_model.cpp) 138 | target_link_libraries(hunter_messenger ${catkin_LIBRARIES} ascent) 139 | 140 | add_executable(hunter_base_node src/hunter_base_node.cpp) 141 | target_link_libraries(hunter_base_node hunter_messenger ${catkin_LIBRARIES}) 142 | 143 | #add_executable(hunter_base_sim_node src/hunter_base_sim_node.cpp) 144 | #target_link_libraries(hunter_base_sim_node hunter_messenger ${catkin_LIBRARIES}) 145 | 146 | #add_executable(hunter_status_node src/hunter_status_node.cpp) 147 | #target_link_libraries(hunter_status_node hunter_messenger ${catkin_LIBRARIES}) 148 | 149 | ## Add cmake target dependencies of the library 150 | ## as an example, code may need to be generated before libraries 151 | ## either from message generation or dynamic reconfigure 152 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 153 | 154 | ## Declare a C++ executable 155 | ## With catkin_make all packages are built within a single CMake context 156 | ## The recommended prefix ensures that target names across packages don't collide 157 | # add_executable(${PROJECT_NAME}_node src/hunter_base_node.cpp) 158 | 159 | ## Rename C++ executable without prefix 160 | ## The above recommended prefix causes long target names, the following renames the 161 | ## target back to the shorter version for ease of user use 162 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 163 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 164 | 165 | ## Add cmake target dependencies of the executable 166 | ## same as for the library above 167 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 168 | 169 | ## Specify libraries to link a library or executable target against 170 | # target_link_libraries(${PROJECT_NAME}_node 171 | # ${catkin_LIBRARIES} 172 | # ) 173 | 174 | ############# 175 | ## Install ## 176 | ############# 177 | 178 | # all install targets should use catkin DESTINATION variables 179 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 180 | 181 | ## Mark executable scripts (Python etc.) for installation 182 | ## in contrast to setup.py, you can choose the destination 183 | # catkin_install_python(PROGRAMS 184 | # scripts/my_python_script 185 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 186 | # ) 187 | 188 | ## Mark executables for installation 189 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 190 | # install(TARGETS ${PROJECT_NAME}_node 191 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 192 | # ) 193 | 194 | ## Mark libraries for installation 195 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 196 | # install(TARGETS ${PROJECT_NAME} 197 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 198 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 199 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 200 | # ) 201 | 202 | ## Mark cpp header files for installation 203 | # install(DIRECTORY include/${PROJECT_NAME}/ 204 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 205 | # FILES_MATCHING PATTERN "*.h" 206 | # PATTERN ".svn" EXCLUDE 207 | # ) 208 | 209 | ## Mark other files for installation (e.g. launch and bag files, etc.) 210 | # install(FILES 211 | # # myfile1 212 | # # myfile2 213 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 214 | # ) 215 | 216 | ############# 217 | ## Testing ## 218 | ############# 219 | 220 | ## Add gtest based cpp test target and link libraries 221 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_hunter_base.cpp) 222 | # if(TARGET ${PROJECT_NAME}-test) 223 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 224 | # endif() 225 | 226 | ## Add folders to be run by python nosetests 227 | # catkin_add_nosetests(test) 228 | -------------------------------------------------------------------------------- /hunter_base/ascent/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project("ascent" 3 | VERSION 0.5.0 4 | DESCRIPTION "An extremely fast and flexible C++ simulation engine and differential equation solver." 5 | # HOMEPAGE_URL "https://github.com/AnyarInc/Ascent" 6 | LANGUAGES CXX) 7 | 8 | include(GNUInstallDirs) 9 | include(CMakePackageConfigHelpers) 10 | include(CPack) 11 | if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME) 12 | include(CTest) 13 | endif() 14 | 15 | add_library(${PROJECT_NAME} INTERFACE) 16 | add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) 17 | 18 | target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) 19 | if (MSVC) 20 | target_compile_options(${PROJECT_NAME} INTERFACE "/bigobj") # for ChaiScript 21 | endif() 22 | 23 | target_include_directories( 24 | ${PROJECT_NAME} 25 | INTERFACE $ 26 | $ 27 | $) 28 | 29 | install(TARGETS ${PROJECT_NAME} 30 | EXPORT ${PROJECT_NAME}_Targets 31 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 32 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 33 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 34 | 35 | write_basic_package_version_file("${PROJECT_NAME}ConfigVersion.cmake" 36 | VERSION ${PROJECT_VERSION} 37 | COMPATIBILITY SameMajorVersion) 38 | configure_package_config_file( 39 | "${PROJECT_SOURCE_DIR}/cmake/${PROJECT_NAME}Config.cmake.in" 40 | "${PROJECT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" 41 | INSTALL_DESTINATION 42 | ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake) 43 | 44 | install(EXPORT ${PROJECT_NAME}_Targets 45 | FILE ${PROJECT_NAME}Targets.cmake 46 | NAMESPACE ${PROJECT_NAME}:: 47 | DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake) 48 | 49 | install(FILES "${PROJECT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" 50 | "${PROJECT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" 51 | DESTINATION ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}/cmake) 52 | 53 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME} 54 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 55 | 56 | set(CPACK_RESOURCE_FILE_LICENSE "${PROJECT_SOURCE_DIR}/LICENSE") 57 | 58 | if((CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME OR MODERN_CMAKE_BUILD_TESTING) AND BUILD_TESTING) 59 | add_subdirectory(unit_tests) 60 | endif() 61 | 62 | option(BUILD_EXAMPLES "Build example projects" TRUE) 63 | if((CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME) AND BUILD_EXAMPLES) 64 | add_subdirectory(examples) 65 | endif() -------------------------------------------------------------------------------- /hunter_base/ascent/README.md: -------------------------------------------------------------------------------- 1 | # Ascent 2 | 3 | An extremely fast and flexible C++ simulation engine and differential equation solver. 4 | 5 | Built from the ground up as a major advancement of [Ascent-beta](https://github.com/AnyarInc/ascent-beta) 6 | 7 | See the [Ascent Wiki](https://github.com/AnyarInc/Ascent/wiki) for more examples and help 8 | 9 | Study the [modular-spring-damper](https://github.com/AnyarInc/Ascent/wiki/modular-spring-damper) example to learn the basics of solving matrix free systems of ordinary differential equations in an object-oriented manner 10 | 11 | ## Blazingly Fast 12 | 13 | Ascent's integration algorithms are designed for speed, and outperform boost's [odeint](http://headmyshoulder.github.io/odeint-v2/index.html) in both Debug and Release 14 | 15 | 16 | 17 | Integration algorithms are automatically vectorized when using std::vector 18 | 19 | ## Extremely Flexible 20 | 21 | Ascent solvers conform to the odeint system syntax, letting you run odeint system without changes (and with faster results!) 22 | 23 | odeint solvers can also be used to run Ascent simulations, providing all flexibility of the odeint engine with Ascent's simulation framework 24 | 25 | Ascent can solve complex, dynamic systems of differential equations in a modular, object-oriented manner 26 | 27 | Easily multi-thread systems and change integrators on the fly 28 | 29 | ## Highlights 30 | - Header Only 31 | - Automatic Vectorization: Ascent conforms to vectorization standards (such as Intel's) 32 | - Free for open source and commercial applications (Apache License) 33 | - Modular (Optional): solve systems in an object-oriented manner 34 | - Variable Tracking: Optimized recording of variable time history 35 | - Asynchronous Sampling and Event Scheduling 36 | - Multiple Integration Algorithms (In Progress): adaptive steppers, predictor-correctors, etc. 37 | - Use boost's odeint library as the numerical integration engine 38 | - Scripting Interface: Optional ChaiScript interface to easily script simulations 39 | 40 | ## Applications 41 | - Aerospace, multi-body physics, chemical reactions, economics, circuits, and much more 42 | - As a game engine for synchronization and physics 43 | - Agent-based simulations 44 | - Complex systems of differential equations 45 | - State-space modeling 46 | - Control algorithms (e.g. robotics) 47 | 48 | *** 49 | ## Requirements 50 | - C++17 compliant compiler 51 | 52 | ## Scripting Requirements 53 | - [ChaiScript](http://chaiscript.com/) Embedded Scripting Language 54 | 55 | *** 56 | ## Why A New Version of Ascent? 57 | [Ascent-beta](https://github.com/AnyarInc/ascent-beta) still remains a state of the art simulation engine, far surpassing the capabilities of many modern engines/ode solvers. 58 | #### However, this new framework is... 59 | - Header only 60 | - Much, much faster. With a simple spring-mass-damper simulation, the current version is twelve-times faster than the beta version. 61 | - Cleaner code: Ascent doesn't require as many specialized containers, avoids pointers, and takes advantage of more core C++. This makes it faster to write simulations, the code is more comprehensible, and it is easier to debug. 62 | - State-space modeling: Ascent allows state space modeling and allows it to be integrated with modular design. 63 | - Module/Simulation abstraction: Modules are only simulation specific if they directly handle integration states. This means modules can easily be used across simulations even while running. 64 | - Easier, faster, and more powerful scripting: Simulation loops can now be scripted in Ascent. Scripts are also easier to move to C++ if the user wants to compile simulation designs. 65 | - More straightforward multi-threading 66 | - This new version completely separates concepts of simulators, integrators, systems, modules, recorders, and more. 67 | - Integration algorithms have reduced memory footprints (RK4 uses less than half the memory as before) 68 | 69 | 70 | ### About [odeint](https://github.com/boostorg/odeint) 71 | odeint is a C++ ordinary differential equation solver that is part of the boost library. 72 | Ascent was partly inspired by the design of odeint, but Ascent offers better performance where comparisons can be made, this is especially true for solving object-oriented systems. 73 | odeint offers various state types and solvers that Ascent integration algorithms currently do not support, so the odeint solvers are a viable option as the integration algorithm beneath an Ascent simulation. 74 | -------------------------------------------------------------------------------- /hunter_base/ascent/cmake/ascentConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") 4 | check_required_components("@PROJECT_NAME@") -------------------------------------------------------------------------------- /hunter_base/ascent/examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(airy) 2 | add_subdirectory(lorenz) 3 | add_subdirectory(modular-spring-damper) 4 | add_subdirectory(pliny-fountain) 5 | add_subdirectory(sampling) -------------------------------------------------------------------------------- /hunter_base/ascent/examples/airy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(airy airy.cpp) 2 | target_link_libraries(airy ascent) -------------------------------------------------------------------------------- /hunter_base/ascent/examples/airy/airy.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "ascent/Ascent.h" 16 | 17 | using namespace asc; 18 | 19 | struct Airy 20 | { 21 | void operator()(const state_t& x, state_t& xd, const double t) 22 | { 23 | xd[0] = x[1]; 24 | xd[1] = -t*x[0]; 25 | } 26 | }; 27 | 28 | int main() 29 | { 30 | state_t x = { 1.0, 0.0 }; 31 | double t = 0.0; 32 | double dt = 0.1; 33 | double t_end = 10.0; 34 | 35 | RK4 integrator; 36 | Airy system; 37 | 38 | Recorder recorder; 39 | 40 | while (t < t_end) 41 | { 42 | recorder({ t, x[0], x[1] }); 43 | integrator(system, x, t, dt); 44 | } 45 | 46 | recorder.csv("airy", { "t", "x0", "x1" }); // generate a file of comma separated values 47 | 48 | return 0; 49 | } -------------------------------------------------------------------------------- /hunter_base/ascent/examples/lorenz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(lorenz lorenz.cpp) 2 | target_link_libraries(lorenz ascent) -------------------------------------------------------------------------------- /hunter_base/ascent/examples/lorenz/lorenz.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "ascent/Ascent.h" 16 | 17 | using namespace asc; 18 | 19 | struct Lorenz 20 | { 21 | void operator()(const state_t& x, state_t& xd, const double) 22 | { 23 | static constexpr double sigma = 10.0; 24 | static constexpr double R = 28.0; 25 | static constexpr double b = 8.0 / 3.0; 26 | 27 | xd[0] = sigma * (x[1] - x[0]); 28 | xd[1] = R * x[0] - x[1] - x[0] * x[2]; 29 | xd[2] = -b * x[2] + x[0] * x[1]; 30 | } 31 | }; 32 | 33 | int main() 34 | { 35 | state_t x = { 10.0, 1.0, 1.0 }; 36 | double t = 0.0; 37 | double dt = 0.01; 38 | double t_end = 10.0; 39 | 40 | RK4 integrator; 41 | Lorenz system; 42 | 43 | Recorder recorder; 44 | 45 | while (t < t_end) 46 | { 47 | recorder({ t, x[0], x[1], x[2] }); 48 | integrator(system, x, t, dt); 49 | } 50 | 51 | recorder.csv("lorenz", { "t", "x0", "x1", "x2" }); 52 | 53 | return 0; 54 | } -------------------------------------------------------------------------------- /hunter_base/ascent/examples/modular-spring-damper/Body.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Ascent.h" 18 | 19 | struct Body 20 | { 21 | Body(asc::state_t& state) : s(state), v(state) {} 22 | 23 | asc::Param s; // position 24 | asc::Param v; // velocity 25 | double m{}; // mass 26 | double f{}; // force 27 | 28 | void operator()(const asc::state_t&, asc::state_t& D, const double) 29 | { 30 | s(D) = v; 31 | 32 | if (m > 0.0) 33 | v(D) = f / m; 34 | else 35 | v(D) = 0.0; 36 | 37 | f = 0.0; 38 | } 39 | }; -------------------------------------------------------------------------------- /hunter_base/ascent/examples/modular-spring-damper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(modular-spring-damper Main.cpp) 2 | target_link_libraries(modular-spring-damper ascent) -------------------------------------------------------------------------------- /hunter_base/ascent/examples/modular-spring-damper/Damper.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "Body.h" 18 | 19 | struct Damper 20 | { 21 | Damper(Body& b0, Body& b1) : b0(b0), b1(b1) {} 22 | 23 | Body& b0; 24 | Body& b1; 25 | 26 | double dv{}; // velocity difference 27 | double c{}; // damping coefficient 28 | double f{}; // force 29 | 30 | void operator()(const asc::state_t&, asc::state_t&, const double) 31 | { 32 | dv = b0.v - b1.v; 33 | f = c*dv; 34 | 35 | b0.f -= f; 36 | b1.f += f; 37 | } 38 | }; -------------------------------------------------------------------------------- /hunter_base/ascent/examples/modular-spring-damper/Main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "Damper.h" 16 | #include "Spring.h" 17 | 18 | using namespace asc; 19 | 20 | int main() 21 | { 22 | state_t x; 23 | x.reserve(100); // We reserve more space than necessary, but Ascent will only allocate what is needed 24 | double t = 0.0; 25 | double dt = 0.01; 26 | double t_end = 1.5; 27 | 28 | Body b0(x); 29 | Body b1(x); 30 | b1.m = 1.0; 31 | b1.s = 1.0; 32 | b1.v = 40.0; 33 | 34 | Spring spring(b0, b1); 35 | spring.k = 2000.0; 36 | 37 | Damper damper(b0, b1); 38 | damper.c = 5.0; 39 | 40 | RK4 integrator; 41 | Recorder recorder; 42 | 43 | auto system = [&](const asc::state_t& x, asc::state_t& D, const double t) 44 | { 45 | // We must run the spring and damper before the body in order to accumulate forces 46 | spring(x, D, t); 47 | damper(x, D, t); 48 | b1(x, D, t); 49 | }; 50 | 51 | while (t < t_end) 52 | { 53 | recorder({ t, b1.s }); 54 | integrator(system, x, t, dt); 55 | } 56 | 57 | recorder.csv("spring-damper", { "t", "b1 position" }); 58 | 59 | return 0; 60 | } -------------------------------------------------------------------------------- /hunter_base/ascent/examples/modular-spring-damper/Spring.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "Body.h" 18 | 19 | struct Spring 20 | { 21 | Spring(Body& b0, Body& b1) : b0(b0), b1(b1) 22 | { 23 | l0 = b1.s - b0.s; 24 | } 25 | 26 | Body& b0; 27 | Body& b1; 28 | 29 | double l0{}; // initial spring length (distance between masses) 30 | double ds{}; // spring compression/extension 31 | double k{}; // spring coefficient 32 | double f{}; // force 33 | 34 | void operator()(const asc::state_t&, asc::state_t&, const double) 35 | { 36 | ds = l0 + b0.s - b1.s; 37 | f = k*ds; 38 | 39 | b0.f -= f; 40 | b1.f += f; 41 | } 42 | }; -------------------------------------------------------------------------------- /hunter_base/ascent/examples/pliny-fountain/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(pliny-fountain fountain.cpp) 2 | target_link_libraries(pliny-fountain ascent) -------------------------------------------------------------------------------- /hunter_base/ascent/examples/pliny-fountain/fountain.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "ascent/Ascent.h" 16 | 17 | using namespace asc; 18 | 19 | // Pliny's Fountain example from: 20 | // Applied Numerical Methods with MATLAB 21 | // for Engineers and Scientists 22 | // Steven C. Chapra 23 | // p.608-609 24 | 25 | struct Fountain 26 | { 27 | double siphon; 28 | static constexpr double Rt = 0.05; 29 | static constexpr double r = 0.007; 30 | static constexpr double yhi = 0.1; 31 | static constexpr double ylo = 0.025; 32 | static constexpr double C = 0.6; 33 | static constexpr double g = 9.81; 34 | static constexpr double Qin = 0.00005; 35 | 36 | static constexpr double pi = 3.14159265359; 37 | 38 | void operator()(const state_t& x, state_t& xd, const double) 39 | { 40 | if (x[0] <= ylo) 41 | siphon = 0.0; 42 | else if (x[0] >= yhi) 43 | siphon = 1.0; 44 | 45 | const double Qout = siphon * C * sqrt(2 * g * x[0]) * pi * r * r; 46 | xd[0] = (Qin - Qout) / (pi * Rt * Rt); 47 | } 48 | }; 49 | 50 | int main() 51 | { 52 | state_t x = { 0.0 }; 53 | double t = 0.0; 54 | double dt = 1.0; 55 | double t_end = 100.0; 56 | 57 | RK4 integrator; 58 | Fountain system; 59 | 60 | Recorder recorder; 61 | 62 | while (t < t_end) 63 | { 64 | recorder({ t, x[0] }); 65 | integrator(system, x, t, dt); 66 | } 67 | 68 | recorder.csv("results", { "t", "x[0]" }); // generate a file of comma separated values 69 | 70 | return 0; 71 | } -------------------------------------------------------------------------------- /hunter_base/ascent/examples/sampling/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(sampling sampling.cpp) 2 | target_link_libraries(sampling ascent) -------------------------------------------------------------------------------- /hunter_base/ascent/examples/sampling/sampling.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "ascent/Ascent.h" 16 | 17 | using namespace asc; 18 | 19 | struct ODE 20 | { 21 | void operator()(const state_t& x, state_t& xd, const double t) 22 | { 23 | xd[0] = cos(t); 24 | } 25 | }; 26 | 27 | int main() 28 | { 29 | state_t x = { 0.0 }; 30 | double t = 0.0; 31 | double dt = 0.1; 32 | double t_end = 10.0; 33 | 34 | RK4 integrator; 35 | ODE system; 36 | 37 | Recorder recorder; 38 | 39 | while (t < t_end) 40 | { 41 | Sampler sampler(t, dt); 42 | 43 | // We force the system to be evaluated at all increments of 0.33 and 0.41, as well as trigger a single event evaluation at 0.617 44 | if (sampler(0.33) || sampler(0.41) || sampler.event(0.617)) 45 | recorder({ t, x[0] }); 46 | 47 | integrator(system, x, t, dt); 48 | } 49 | 50 | recorder.csv("sampling", { "t", "x0" }); // generate a file of comma separated values 51 | 52 | return 0; 53 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ascent library 2 | add_library(ascent INTERFACE) 3 | target_include_directories(ascent INTERFACE 4 | $ 5 | $) -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/Ascent.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Recorder.h" 18 | #include "ascent/Param.h" 19 | 20 | // Timing 21 | #include "ascent/timing/Sampler.h" 22 | 23 | // Integrators 24 | #include "ascent/integrators/Euler.h" 25 | #include "ascent/integrators/Midpoint.h" 26 | #include "ascent/integrators/RK2.h" 27 | #include "ascent/integrators/RK4.h" 28 | #include "ascent/integrators/DOPRI45.h" 29 | #include "ascent/integrators/RTAM4.h" 30 | #include "ascent/integrators/PC233.h" 31 | 32 | // Linear Algebra 33 | #include "ascent/ParamV.h" 34 | 35 | #include "ascent/System.h" 36 | 37 | #include 38 | #include 39 | 40 | // Type definitions for cleaner code 41 | 42 | namespace asc 43 | { 44 | // Edits To The Following Types Are Not Reccommended 45 | // ------------------------------------------------ 46 | using system_t = std::function; 47 | 48 | using System = SystemT; 49 | 50 | using Recorder = RecorderT; 51 | using RecorderString = RecorderT; 52 | using Sampler = SamplerT; 53 | using Param = ParamT; 54 | 55 | // Integrators 56 | using Euler = EulerT; 57 | using Midpoint = MidpointT; 58 | using RK2 = RK2T; 59 | using RK4 = RK4T; 60 | using DOPRI45 = DOPRI45T; 61 | using PC233 = PC233T; 62 | 63 | // Linear Algebra 64 | using ParamV = ParamVT; 65 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/ChaiEngine.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2018 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Ascent.h" 18 | #include "ascent/threading/Pool.h" 19 | #include "ascent/threading/Queue.h" 20 | #include "chaiscript/chaiscript.hpp" 21 | #include "chaiscript/chaiscript_stdlib.hpp" 22 | 23 | namespace asc 24 | { 25 | struct ChaiEngine : public chaiscript::ChaiScript 26 | { 27 | // Captures the systyem object by reference. 28 | template 29 | void addSystem() 30 | { 31 | add(chaiscript::type_conversion([](T& system) -> system_t { return [&](const state_t& x, state_t& xd, const value_t t) { return system(x, xd, t); }; })); 32 | add(chaiscript::type_conversion, system_t>([](std::shared_ptr system) -> system_t { return [=](const state_t& x, state_t& xd, const value_t t) { return (*system)(x, xd, t); }; })); 33 | } 34 | 35 | // This code must exist here and not within the Recorder class for compatibilty with compilers that inspect templates (i.e. Xcode's LLVM) 36 | template 37 | void scriptRecorder(ChaiScript& c, const std::string& name) 38 | { 39 | using namespace chaiscript; 40 | using R = RecorderT; 41 | c.add(user_type(), name); 42 | c.add(constructor(), name); 43 | 44 | c.add(fun(&R::titles), "titles"); 45 | c.add(fun(&R::precision), "precision"); 46 | c.add(fun(&R::update), "update"); 47 | c.add(fun([](R& rec, const T& x) { rec.add(x); }), "add"); 48 | c.add(fun([](R& rec, const std::vector& data) { rec.add(data); }), "add"); 49 | c.add(fun([](R& rec, const std::string& title) { rec.add_title(title); }), "add_title"); 50 | c.add(fun([](R& rec, const std::vector& titles) { rec.add_titles(titles); }), "add_titles"); 51 | c.add(fun([](R& rec, T& x) { rec.record(x); }), "record"); 52 | c.add(fun([](R& rec, T& x, const std::string& title) { rec.record(x, title); }), "record"); 53 | c.add(fun([](R& rec, std::vector& v) { rec.record(v); }), "record"); 54 | c.add(fun([](R& rec, std::vector& v, const std::vector& title) { rec.record(v, title); }), "record"); 55 | c.add(fun([](R& rec, const std::vector& data) { rec.push_back(data); }), "push_back"); 56 | c.add(fun([](R& rec, const std::string& file_name) { rec.csv(file_name); }), "csv"); 57 | c.add(fun([](R& rec, const std::string& file_name, const std::vector& names) { rec.csv(file_name, names); }), "csv"); 58 | } 59 | 60 | ChaiEngine() 61 | { 62 | using namespace chaiscript; 63 | 64 | add(vector_conversion()); 65 | add(vector_conversion>()); 66 | add(bootstrap::standard_library::vector_type("state_t")); 67 | 68 | scriptRecorder(*this, "Recorder"); 69 | add(fun([](RecorderT& rec, int& x) { rec.record(x); }), "record"); 70 | add(fun([](RecorderT& rec, size_t& x) { rec.record(x); }), "record"); 71 | add(fun([](RecorderT& rec, int& x, const std::string& title) { rec.record(x, title); }), "record"); 72 | add(fun([](RecorderT& rec, size_t& x, const std::string& title) { rec.record(x, title); }), "record"); 73 | 74 | // This allows for more generic mixing of various data types, all they need is to be converted to a string prior to being passed into the recorder 75 | // The RecorderString is primarily for data that is going to be output to a file and not operated on during the simulation 76 | scriptRecorder(*this, "RecorderString"); 77 | 78 | add(user_type(), "Param"); 79 | add(constructor(), "Param"); 80 | add(constructor(), "Param"); 81 | add(fun([](asc::Param& param, const value_t y) { return param = y; }), "="); 82 | add(fun([](asc::Param& param) { std::cout << param << '\n'; }), "print"); 83 | add(fun([](asc::Param& param) { return static_cast(param); }), "value"); 84 | 85 | add(user_type(), "ParamV"); 86 | add(constructor(), "ParamV"); 87 | add(fun([](ParamV& lhs, const std::vector& rhs) { return lhs = rhs; }), "="); 88 | add(fun([](ParamV& v) { v.zero(); }), "zero"); 89 | 90 | add(user_type(), "Sampler"); 91 | add(constructor(), "Sampler"); 92 | add(fun(&Sampler::operator()), "eval"); 93 | add(fun(&Sampler::event), "event"); 94 | add(fun(&Sampler::reset), "reset"); 95 | 96 | // System 97 | add(user_type(), "System"); 98 | add(constructor(), "System"); 99 | add(fun([](System& sys, const asc::state_t& x, asc::state_t& D, const asc::value_t t) { sys(x, D, t); }), "eval"); 100 | add(type_conversion()); 101 | add(fun([](System& sys, const system_t& func) { sys.push_back(func); }), "push_back"); 102 | 103 | // Integrators 104 | integrator("ascEuler"); 105 | integrator("RK2"); 106 | integrator("RK4"); 107 | 108 | // threading 109 | Queue::script(*this, "Queue"); 110 | Pool::script(*this, "Pool"); 111 | add(fun([] { return std::thread::hardware_concurrency(); }), "hardware_concurrency"); 112 | add(fun([](asc::Recorder& rec, const int sig_digits) { rec.precision = sig_digits; }), "precision"); 113 | } 114 | ChaiEngine(const ChaiEngine&) = default; 115 | ChaiEngine(ChaiEngine&&) = default; 116 | ChaiEngine& operator=(const ChaiEngine&) = default; 117 | ChaiEngine& operator=(ChaiEngine&&) = default; 118 | 119 | private: 120 | template 121 | void integrator(const std::string& name) 122 | { 123 | add(chaiscript::user_type(), name); 124 | add(chaiscript::constructor(), name); 125 | add(chaiscript::fun([](T& integrator, system_t& system, state_t& state, value_t& t, const value_t dt) { integrator(system, state, t, dt); }), "step"); 126 | } 127 | 128 | template 129 | void adaptiveIntegrator(const std::string& name) 130 | { 131 | add(chaiscript::user_type(), name); 132 | add(chaiscript::constructor(), name); 133 | add(chaiscript::fun([](T& integrator, system_t& system, state_t& state, value_t& t, value_t& dt) { integrator(system, state, t, dt); }), "step"); 134 | } 135 | }; 136 | } 137 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/Param.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | // A Param is a reference to a memory location within the system's state 18 | 19 | namespace asc 20 | { 21 | namespace core 22 | { 23 | template 24 | inline typename C::value_type& emplace_back_ref(C& c, const typename C::value_type x0) 25 | { 26 | c.emplace_back(x0); 27 | return c.back(); 28 | } 29 | } 30 | 31 | template 32 | struct ParamT 33 | { 34 | template 35 | ParamT(C& c, const T x0 = T()) : index(c.size()), x(core::emplace_back_ref(c, x0)) {} 36 | 37 | ParamT(const ParamT&) = default; 38 | ParamT(ParamT&&) = default; 39 | ParamT& operator=(const ParamT&) = default; 40 | ParamT& operator=(ParamT&&) = default; 41 | 42 | template 43 | T& operator()(C& xd) const noexcept { return xd[index]; } 44 | 45 | // Returns true if this State is within a given state array 46 | template 47 | bool within(const C& vec) const noexcept 48 | { 49 | if (&x == &vec[index]) 50 | return true; 51 | return false; 52 | } 53 | 54 | operator T&() const noexcept { return x; } 55 | 56 | T& operator=(const T y) noexcept { x = y; return x; } 57 | 58 | private: 59 | const size_t index; 60 | T& x; 61 | }; 62 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/ParamV.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "Param.h" 18 | #include 19 | #include 20 | 21 | // The ParamV behaves like a std::vector of Params 22 | 23 | namespace asc 24 | { 25 | template 26 | struct ParamVT 27 | { 28 | using iterator = typename std::vector::iterator; 29 | 30 | ParamVT(ParamVT&& other) : i0(std::move(other.i0)), n(std::move(other.n)), data_ptr(std::move(other.data_ptr)) {} 31 | ParamVT(const ParamVT& other) : i0(other.i0), n(other.n), data_ptr(other.data_ptr) {} 32 | 33 | ParamVT& operator=(const ParamVT& v) 34 | { 35 | for (size_t i = 0; i < n; ++i) 36 | this->operator[](i) = v[i]; 37 | return *this; 38 | } 39 | 40 | template 41 | ParamVT& operator=(const C& c) 42 | { 43 | for (size_t i = 0; i < n; ++i) 44 | this->operator[](i) = c[i]; 45 | return *this; 46 | } 47 | 48 | template 49 | ParamVT(C& c, const size_t n) : n(n) 50 | { 51 | ParamT(c, T()); 52 | data_ptr = c.data() + c.size() - 1; 53 | i0 = (--c.end()) - c.begin(); 54 | 55 | for (size_t i = 1; i < n; ++i) 56 | ParamT(c, T()); 57 | } 58 | 59 | template 60 | ParamVT(C& c, std::initializer_list&& list) : n(list.size()) 61 | { 62 | bool first_element{ true }; 63 | for (T x : list) 64 | { 65 | ParamT(c, x); 66 | 67 | if (first_element) 68 | { 69 | first_element = false; 70 | data_ptr = c.data() + c.size() - 1; 71 | i0 = (--c.end()) - c.begin(); 72 | } 73 | } 74 | } 75 | 76 | // Constructor for selecting a specific section of allocated memory 77 | ParamVT(const size_t i0, const size_t n, T* data) : i0(i0), n(n), data_ptr(data) {} 78 | 79 | template 80 | ParamVT operator()(C& xd) const 81 | { 82 | return ParamVT(i0, n, xd.data() + i0); 83 | } 84 | 85 | const T* begin() const noexcept { return data_ptr; } 86 | const T* end() const noexcept { return data_ptr + n; } 87 | 88 | T& operator[](const size_t i) const noexcept { return *(data_ptr + i); } 89 | 90 | size_t size() const noexcept { return n; } 91 | 92 | T* data() const noexcept { return data_ptr; } 93 | 94 | void zero() noexcept 95 | { 96 | for (size_t i = 0; i < n; ++i) 97 | this->operator[](i) = T(); 98 | } 99 | 100 | protected: 101 | size_t i0{}; // starting index 102 | const size_t n; 103 | T* data_ptr; 104 | }; 105 | } 106 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/System.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace asc 21 | { 22 | template 23 | struct SystemT 24 | { 25 | void push_back(const system_t& func) 26 | { 27 | functions.push_back(func); 28 | } 29 | 30 | void operator()(const state_t& x, state_t& xd, const double t) 31 | { 32 | for (auto& f : functions) 33 | f(x, xd, t); 34 | } 35 | 36 | std::vector functions; 37 | }; 38 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/Utility.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace asc 20 | { 21 | // User type definitions for ease of use 22 | using value_t = double; // float, double, etc. 23 | using state_t = std::vector; // std::vector, std::deque, etc. 24 | 25 | constexpr const value_t cx(long double v) { return static_cast(v); } 26 | constexpr const value_t operator"" _v(long double v) { return static_cast(v); } 27 | 28 | struct AdaptiveIntegrator 29 | { 30 | AdaptiveIntegrator() = default; 31 | AdaptiveIntegrator(const AdaptiveIntegrator&) = default; 32 | AdaptiveIntegrator(AdaptiveIntegrator&&) = default; 33 | AdaptiveIntegrator& operator=(const AdaptiveIntegrator&) = default; 34 | AdaptiveIntegrator& operator=(AdaptiveIntegrator&&) = default; 35 | virtual ~AdaptiveIntegrator() = default; 36 | }; 37 | 38 | template 39 | struct AdaptiveT 40 | { 41 | T abs_tol = static_cast(1.0); // absolute tolerance 42 | T rel_tol = static_cast(1.0); // relative tolerance 43 | T safety_factor = static_cast(0.9); // value < 1.0 reduces time step change aggressiveness 44 | }; 45 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/Vector.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2018 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/ParamV.h" 18 | 19 | namespace asc 20 | { 21 | template 22 | struct Vector : asc::ParamVT 23 | { 24 | Vector(std::vector& c) : asc::ParamVT(c, N) {} 25 | Vector(Vector&& other) : asc::ParamVT(std::move(other)) {} 26 | Vector(const value_t& other) : asc::ParamVT(other) {} 27 | 28 | using value_type = value_t; 29 | 30 | template 31 | Vector(C& c, std::initializer_list&& list) : asc::ParamVT(c, list) {} 32 | 33 | Vector(const size_t i0, const size_t n, value_t* data) : asc::ParamVT(i0, n, data) {} 34 | 35 | Vector& operator=(const Vector& v) 36 | { 37 | for (size_t i = 0; i < n; ++i) 38 | this->operator[](i) = v[i]; 39 | return *this; 40 | } 41 | 42 | Vector& operator=(const std::vector& v) 43 | { 44 | for (size_t i = 0; i < n; ++i) 45 | this->operator[](i) = v[i]; 46 | return *this; 47 | } 48 | 49 | using asc::ParamVT::operator[]; 50 | 51 | double norm() const 52 | { 53 | const size_t n = size(); 54 | value_t sum{}; 55 | for (size_t i = 0; i < n; ++i) 56 | sum += operator[](i) * operator[](i); 57 | return sqrt(sum); 58 | } 59 | 60 | std::vector normalized() const 61 | { 62 | std::vector ret; 63 | ret.reserve(N); 64 | const value_t magnitude = norm(); 65 | for (size_t i = 0; i < N; ++i) 66 | { 67 | ret.emplace_back(operator[](i) / magnitude); 68 | } 69 | return ret; 70 | } 71 | }; 72 | 73 | inline auto operator-(const Vector<3>& lhs, const Vector<3>& rhs) -> std::vector::value_type> 74 | { 75 | const size_t n = lhs.size(); 76 | std::vector::value_type> ret(n); 77 | for (size_t i = 0; i < n; ++i) 78 | ret[i] = lhs[i] - rhs[i]; 79 | return ret; 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/algorithms/Derivative.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #ifdef min 20 | #undef min 21 | #endif 22 | 23 | namespace asc 24 | { 25 | // x is the independant variable, y is dependent. Returns derivative for unequally spaced points. xest is the x value at which to evaluate the derivative. 26 | // x and y vectors don't need to be the same length if they have three elements or more each 27 | template 28 | inline auto derivative(const T0& x, const T1& y, const typename T0::value_type xest) 29 | { 30 | const size_t nx = x.size(); 31 | const size_t ny = y.size(); 32 | if (x.size() < 2 || y.size() < 2) 33 | return typename T0::value_type(); 34 | else if (nx == 2 || ny == 2) 35 | return (y.back() - y.front()) / (x.back() - x.front()); 36 | else 37 | { 38 | const typename T0::value_type x2 = x[nx - 1]; 39 | const typename T0::value_type x1 = x[nx - 2]; 40 | const typename T0::value_type x0 = x[nx - 3]; 41 | 42 | const typename T1::value_type y2 = y[ny - 1]; 43 | const typename T1::value_type y1 = y[ny - 2]; 44 | const typename T1::value_type y0 = y[ny - 3]; 45 | 46 | const typename T0::value_type dydx = y0*(2.0 * xest - x1 - x2) / ((x0 - x1)*(x0 - x2)) + y1*(2.0 * xest - x0 - x2) / ((x1 - x0)*(x1 - x2)) + y2*(2.0 * xest - x0 - x1) / ((x2 - x0)*(x2 - x1)); 47 | 48 | return dydx; 49 | } 50 | } 51 | 52 | // x is the independant variable, y is dependent. Returns derivative for unequally spaced points. 53 | // Uses the last x value for the prediction point. 54 | template 55 | inline auto derivative(const T0& x, const T1& y) 56 | { 57 | return derivative(x, y, x.back()); 58 | } 59 | 60 | // supports n dimensional containers of vectors 61 | template 62 | inline std::vector derivative_vector(const T0& t, const T1& v) 63 | { 64 | const size_t n = std::min(t.size(), v.size()); 65 | const size_t dimensions = v.front().size(); 66 | if (n < 2) 67 | return std::vector(dimensions); 68 | else if (n == 2) 69 | { 70 | //return (v[1] - v[0]) / (t[1] - t[0]); // Changed this so its compatible with std::array 71 | std::vector ret; 72 | ret.resize(dimensions); 73 | for (size_t i = 0; i < dimensions; ++i) 74 | ret[i] = (v[1][i] - v[0][i]) / (t[1] - t[0]); 75 | return ret; 76 | } 77 | 78 | std::vector> dimensional_history(dimensions); // each vector is for a dimension, such as x, y, z, . . . (can have more dimensions than 3), saving three time steps of history 79 | for (size_t i = 0; i < dimensions; ++i) 80 | { 81 | for (size_t j = 3; j > 0; --j) // iterate over last three states, from oldest to newest 82 | dimensional_history[i].push_back(v[n - j][i]); // this will push back the ith states (i.e. if i = 0, x dimension) from the last three vx states 83 | } 84 | 85 | std::vector deriv(dimensions); 86 | for (size_t i = 0; i < dimensions; ++i) 87 | deriv[i] = asc::derivative(t, dimensional_history[i]); // t can contain more than 3 steps 88 | 89 | return deriv; 90 | } 91 | 92 | namespace eigen 93 | { 94 | // E is intended to be an Eigen::Vector, such as Eigen::Vector3d, supports n dimensional Eigen vectors 95 | template 96 | inline E derivative_vector(const T0 &t, const T1 &v) 97 | { 98 | const auto n = std::min(t.size(), v.size()); 99 | if (n < 2) 100 | { 101 | return E::Zero(); 102 | } 103 | 104 | return (v[1] - v[0]) / (t[1] - t[0]); 105 | } 106 | 107 | // takes the derivative over three points of history 108 | template 109 | struct derivative_vector_3 110 | { 111 | std::vector> dimensional_history; // each vector is for a dimension, such as x, y, z, . . . (can have more dimensions than 3), saving three time steps of history 112 | 113 | // E is intended to be an Eigen::Vector, such as Eigen::Vector3d, supports n dimensional Eigen vectors 114 | template 115 | inline E operator()(const T0 &t, const T1 &v) 116 | { 117 | const auto n = std::min(t.size(), v.size()); 118 | if (n < 2) 119 | { 120 | return E::Zero(); 121 | } 122 | else if (n == 2) 123 | { 124 | return (v[1] - v[0]) / (t[1] - t[0]); 125 | } 126 | 127 | const auto dimensions = v[0].rows(); 128 | dimensional_history.resize(dimensions); 129 | for (auto& h : dimensional_history) 130 | { 131 | h.resize(3); 132 | } 133 | for (auto i = 0; i < dimensions; ++i) 134 | { 135 | for (auto j = 0; j < 3; ++j) // iterate over last three states, from oldest to newest 136 | { 137 | dimensional_history[i][j] = v[n - 3 + j][i]; // this will push back the ith states (i.e. if i = 0, x dimension) from the last three Eigen::Vector states 138 | } 139 | } 140 | 141 | E deriv; 142 | for (auto i = 0; i < dimensions; ++i) 143 | { 144 | deriv[i] = derivative(t, dimensional_history[i]); // t can contain more than 3 steps 145 | } 146 | 147 | return deriv; 148 | } 149 | }; 150 | } 151 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/containers/stack.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2018 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | // An optimized stack container for Recorders 18 | 19 | #include 20 | #include 21 | 22 | namespace asc 23 | { 24 | template 25 | struct stack_iterator 26 | { 27 | stack_iterator(const size_t i, stack_t& stack) noexcept : index(i % stack_t::block_size), slice(i / stack_t::block_size), stack(stack), ptr(stack.data(i)) {} 28 | 29 | stack_iterator(const stack_iterator&) noexcept = default; 30 | stack_iterator(stack_iterator&&) noexcept = default; 31 | stack_iterator& operator=(const stack_iterator&) noexcept = default; 32 | stack_iterator& operator=(stack_iterator&&) noexcept = default; 33 | 34 | using value_type = T; 35 | using pointer = T*; 36 | using reference = T&; 37 | using iterator_category = std::forward_iterator_tag; 38 | 39 | T& operator*() 40 | { 41 | return *ptr; 42 | } 43 | 44 | const T& operator*() const 45 | { 46 | return *ptr; 47 | } 48 | 49 | pointer operator->() noexcept 50 | { 51 | return ptr; 52 | } 53 | 54 | const pointer operator->() const noexcept 55 | { 56 | return ptr; 57 | } 58 | 59 | stack_iterator& operator++() noexcept 60 | { 61 | ++index; 62 | ++ptr; 63 | if (index == stack_t::block_size) 64 | { 65 | index = 0; 66 | ++slice; 67 | ptr = stack.data_slice(slice); 68 | } 69 | return *this; 70 | } 71 | 72 | bool operator!=(const stack_iterator& rhs) const noexcept 73 | { 74 | if (index != rhs.index || slice != rhs.slice) 75 | return true; 76 | return false; 77 | } 78 | 79 | bool operator==(const stack_iterator& rhs) const noexcept 80 | { 81 | if (index == rhs.index && slice == rhs.slice) 82 | return true; 83 | return false; 84 | } 85 | 86 | private: 87 | size_t index{}; 88 | size_t slice{}; 89 | stack_t& stack; 90 | T* ptr; 91 | }; 92 | 93 | template 94 | struct const_stack_iterator 95 | { 96 | const_stack_iterator(const size_t i, stack_t& stack) noexcept : index(i % stack_t::block_size), slice(i / stack_t::block_size), stack(stack), ptr(stack.data(i)) {} 97 | 98 | const_stack_iterator(const const_stack_iterator&) noexcept = default; 99 | const_stack_iterator(const_stack_iterator&&) noexcept = default; 100 | const_stack_iterator& operator=(const const_stack_iterator&) noexcept = default; 101 | const_stack_iterator& operator=(const_stack_iterator&&) noexcept = default; 102 | 103 | using value_type = T; 104 | using pointer = T*; 105 | using reference = T&; 106 | using iterator_category = std::forward_iterator_tag; 107 | 108 | const T& operator*() const 109 | { 110 | return *ptr; 111 | } 112 | 113 | const pointer operator->() const 114 | { 115 | return ptr; 116 | } 117 | 118 | const_stack_iterator& operator++() noexcept 119 | { 120 | ++index; 121 | ++ptr; 122 | if (index == stack_t::block_size) 123 | { 124 | index = 0; 125 | ++slice; 126 | ptr = stack.data_slice(slice); 127 | } 128 | return *this; 129 | } 130 | 131 | bool operator!=(const const_stack_iterator& rhs) const noexcept 132 | { 133 | if (index != rhs.index || slice != rhs.slice) 134 | return true; 135 | return false; 136 | } 137 | 138 | bool operator==(const const_stack_iterator& rhs) const noexcept 139 | { 140 | if (index == rhs.index && slice == rhs.slice) 141 | return true; 142 | return false; 143 | } 144 | 145 | private: 146 | size_t index{}; 147 | size_t slice{}; 148 | stack_t& stack; 149 | T* ptr; 150 | }; 151 | 152 | template 153 | struct stack final 154 | { 155 | stack() noexcept 156 | { 157 | deq.emplace_back(); 158 | deq.back().reserve(block); 159 | } 160 | 161 | stack(const stack& other) 162 | { 163 | deq = other.deq; 164 | deq.back().reserve(block); // is not by default copied to the same size 165 | slice = other.slice; 166 | } 167 | stack(stack&&) = default; 168 | stack& operator=(const stack& other) 169 | { 170 | deq = other.deq; 171 | deq.back().reserve(block); // capacity is not by default copied to the same size 172 | slice = other.slice; 173 | return *this; 174 | } 175 | stack& operator=(stack&&) = default; 176 | 177 | static constexpr size_t block_size = block; 178 | 179 | using iterator = stack_iterator>; 180 | using const_iterator = const_stack_iterator>; 181 | 182 | iterator begin() 183 | { 184 | return{ 0, *this }; 185 | } 186 | 187 | iterator end() 188 | { 189 | return{ size(), *this }; 190 | } 191 | 192 | const_iterator cbegin() 193 | { 194 | return{ size(), *this }; 195 | } 196 | 197 | const_iterator cend() 198 | { 199 | return{ size(), *this }; 200 | } 201 | 202 | void emplace_back() noexcept 203 | { 204 | emplace_back(T()); 205 | } 206 | 207 | template 208 | void emplace_back(Arg&& x) noexcept 209 | { 210 | auto& s = deq[slice]; 211 | if (s.size() == block) 212 | { 213 | deq.emplace_back(); 214 | auto& v_back = deq.back(); 215 | v_back.reserve(block); 216 | v_back.emplace_back(std::forward(x)); 217 | ++slice; 218 | } 219 | else 220 | { 221 | s.emplace_back(std::forward(x)); 222 | } 223 | } 224 | 225 | T& operator[](const size_t i) noexcept 226 | { 227 | return deq[i / block][i % block]; 228 | } 229 | 230 | const T& operator[](const size_t i) const noexcept 231 | { 232 | return deq[i / block][i % block]; 233 | } 234 | 235 | T& operator()(const size_t slice_index, const size_t index) noexcept 236 | { 237 | return deq[slice_index][index]; 238 | } 239 | 240 | const T& operator()(const size_t slice_index, const size_t index) const noexcept 241 | { 242 | return deq[slice_index][index]; 243 | } 244 | 245 | T& front() 246 | { 247 | return deq.front().front(); 248 | } 249 | 250 | const T& front() const 251 | { 252 | return deq.front().front(); 253 | } 254 | 255 | T& back() 256 | { 257 | return deq.back().back(); 258 | } 259 | 260 | const T& back() const 261 | { 262 | return deq.back().back(); 263 | } 264 | 265 | size_t size() const noexcept 266 | { 267 | return (deq.size() - 1) * block + deq.back().size(); 268 | } 269 | 270 | T* data_slice(const size_t s) 271 | { 272 | if (s >= deq.size()) 273 | return nullptr; 274 | return deq[s].data(); 275 | } 276 | 277 | const T* data_slice(const size_t s) const 278 | { 279 | if (s >= deq.size()) 280 | return nullptr; 281 | return deq[s].data(); 282 | } 283 | 284 | T* data(const size_t i) 285 | { 286 | if (i / block >= deq.size()) 287 | return nullptr; 288 | return deq[i / block].data() + i % block; 289 | } 290 | 291 | const T* data(const size_t i) const 292 | { 293 | if (i / block >= deq.size()) 294 | return nullptr; 295 | return deq[i / block].data() + i % block; 296 | } 297 | 298 | void clear() 299 | { 300 | deq.resize(1); 301 | deq.front().resize(0); // we don't want to remove the capacity on the vector by calling clear 302 | } 303 | 304 | private: 305 | std::deque> deq; 306 | size_t slice{}; 307 | }; 308 | } 309 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/direct/State.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace asc 20 | { 21 | // pointers, rather than references are used for the possibility of developing states with shared_ptrs for x and xd 22 | struct State 23 | { 24 | State(double& x, double& xd) noexcept : x(&x), xd(&xd) {} 25 | State(const State&) = default; 26 | State(State&&) = default; 27 | State& operator=(const State&) = default; 28 | State& operator=(State&&) = default; 29 | 30 | double* x{}; 31 | double* xd{}; 32 | 33 | std::vector memory; 34 | }; 35 | 36 | template 37 | inline void make_states(states_t& states, x_t& x, xd_t& xd) 38 | { 39 | const size_t n = x.size(); 40 | for (size_t i = 0; i < n; ++i) 41 | { 42 | states.emplace_back(x[i], xd[i]); 43 | } 44 | } 45 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/DOPRI45.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Utility.h" 18 | 19 | // Runge Kutta Dormand Prince 45 20 | 21 | namespace asc 22 | { 23 | template 24 | struct DOPRI45T 25 | { 26 | using value_t = typename state_t::value_type; 27 | 28 | template 29 | void operator()(System&& system, state_t& x, value_t& t, const value_t dt) 30 | { 31 | const auto t0 = t; 32 | const auto dt_5 = 0.2_v * dt; 33 | 34 | const auto n = x.size(); 35 | if (xd0.size() < n) 36 | { 37 | xd0.resize(n); 38 | xd_temp.resize(n); 39 | xd2.resize(n); 40 | xd3.resize(n); 41 | xd4.resize(n); 42 | } 43 | 44 | x0 = x; 45 | 46 | if (!fsal_computed) // if an adaptive stepper hasn't computed the first same as last state, we must compute the step here 47 | { 48 | system(x0, xd0, t); 49 | fsal_computed = false; 50 | } 51 | 52 | size_t i{}; 53 | for (; i < n; ++i) 54 | x[i] = x0[i] + dt_5 * xd0[i]; 55 | t += dt_5; 56 | 57 | system(x, xd_temp, t); 58 | for (i = 0; i < n; ++i) 59 | x[i] = x0[i] + dt * (c10 * xd0[i] + c11 * xd_temp[i]); 60 | t = t0 + cx(3.0 / 10.0) * dt; 61 | 62 | system(x, xd2, t); 63 | for (i = 0; i < n; ++i) 64 | x[i] = x0[i] + dt * (c20 * xd0[i] + c21 * xd_temp[i] + c22 * xd2[i]); 65 | t = t0 + cx(4.0 / 5.0) * dt; 66 | 67 | system(x, xd3, t); 68 | for (i = 0; i < n; ++i) 69 | x[i] = x0[i] + dt * (c30 * xd0[i] + c31 * xd_temp[i] + c32 * xd2[i] + c33 * xd3[i]); 70 | t = t0 + cx(8.0 / 9.0) * dt; 71 | 72 | system(x, xd4, t); 73 | for (i = 0; i < n; ++i) 74 | x[i] = x0[i] + dt * (c40 * xd0[i] + c41 * xd_temp[i] + c42 * xd2[i] + c43 * xd3[i] + c44 * xd4[i]); 75 | t = t0 + dt; 76 | 77 | system(x, xd_temp, t); 78 | for (i = 0; i < n; ++i) 79 | x[i] = x0[i] + dt * (c50 * xd0[i] + c52 * xd2[i] + c53 * xd3[i] + c54 * xd4[i] + c55 * xd_temp[i]); 80 | } 81 | 82 | template 83 | void operator()(System&& system, state_t& x, value_t& t, value_t& dt, const AdaptiveT& settings) 84 | { 85 | const value_t abs_tol = settings.abs_tol; 86 | const value_t rel_tol = settings.rel_tol; 87 | const value_t safety_factor = settings.safety_factor; 88 | 89 | const value_t t0 = t; 90 | const size_t n = x.size(); 91 | 92 | xd6.resize(n); 93 | 94 | start_adaptive: 95 | operator()(system, x, t, dt); 96 | 97 | system(x, xd6, t); // xd6 is xd0, because first same as last (FSAL) 98 | 99 | // overwrite xd2 as the error estimate, this lets us vectorize the calculation of errors and saves memory 100 | for (size_t i = 0; i < n; ++i) 101 | { 102 | xd2[i] = abs(x0[i] + dt * (e0 * xd0[i] + e2 * xd2[i] + e3 * xd3[i] + e4 * xd4[i] + e5 * xd_temp[i] + e6 * xd6[i]) - x[i]); // absolute error estimate (x4th - x5th) 103 | } 104 | 105 | value_t e, e_max{}; 106 | for (size_t i = 0; i < n; ++i) 107 | { 108 | //e = xd2[i] / (abs_tol + rel_tol * (1.0 * abs(x0[i]) + 0.01 * abs(xd0[i]))); 109 | e = xd2[i] / (abs_tol + rel_tol * (abs(x0[i]) + 0.01 * abs(xd0[i]))); 110 | 111 | if (e > e_max) 112 | e_max = e; 113 | } 114 | 115 | if (e_max > 1.0_v) 116 | { 117 | dt *= std::max(0.9_v * pow(e_max, -cx(1.0 / 3.0)), 0.2_v); 118 | 119 | t = t0; 120 | x = x0; 121 | 122 | goto start_adaptive; // recompute the solution recursively 123 | } 124 | 125 | if (e_max < 0.5_v) 126 | { 127 | e_max = std::max(3.2e-4_v, e_max); // 3.2e-4 = pow(5, -5) 128 | dt *= 0.9_v * pow(e_max, -0.2_v); 129 | } 130 | 131 | xd0 = xd6; 132 | fsal_computed = true; 133 | } 134 | 135 | private: 136 | bool fsal_computed = false; 137 | 138 | static constexpr auto c10 = cx(3.0 / 40.0); 139 | static constexpr auto c11 = cx(9.0 / 40.0); 140 | 141 | static constexpr auto c20 = cx(44.0 / 45.0); 142 | static constexpr auto c21 = cx(-56.0 / 15.0); 143 | static constexpr auto c22 = cx(32.0 / 9.0); 144 | 145 | static constexpr auto c30 = cx(19372.0 / 6561.0); 146 | static constexpr auto c31 = cx(-25360.0 / 2187.0); 147 | static constexpr auto c32 = cx(64448.0 / 6561.0); 148 | static constexpr auto c33 = cx(-212.0 / 729.0); 149 | 150 | static constexpr auto c40 = cx(9017.0 / 3168.0); 151 | static constexpr auto c41 = cx(-355.0 / 33.0); 152 | static constexpr auto c42 = cx(46732.0 / 5247.0); 153 | static constexpr auto c43 = cx(49.0 / 176.0); 154 | static constexpr auto c44 = cx(-5103.0 / 18656.0); 155 | 156 | static constexpr auto c50 = cx(35.0 / 384.0); 157 | // c51 is 0 158 | static constexpr auto c52 = cx(500.0 / 1113.0); 159 | static constexpr auto c53 = cx(125.0 / 192.0); 160 | static constexpr auto c54 = cx(-2187.0 / 6784.0); 161 | static constexpr auto c55 = cx(11.0 / 84.0); 162 | 163 | static constexpr auto e0 = cx(5179.0 / 57600.0); 164 | // e1 is 0 165 | static constexpr auto e2 = cx(7571.0 / 16695.0); 166 | static constexpr auto e3 = cx(393.0 / 640.0); 167 | static constexpr auto e4 = cx(-92097.0 / 339200.0); 168 | static constexpr auto e5 = cx(187.0 / 2100.0); 169 | static constexpr auto e6 = cx(1.0 / 40.0); 170 | 171 | state_t x0, xd0, xd_temp, xd2, xd3, xd4, xd6; // xd_temp is used for xd1 and xd5 172 | }; 173 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/Euler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | // Simple Euler integration. 18 | 19 | namespace asc 20 | { 21 | template 22 | struct EulerT 23 | { 24 | using value_t = typename state_t::value_type; 25 | 26 | template 27 | void operator()(System&& system, state_t& x, value_t& t, const value_t dt) 28 | { 29 | const size_t n = x.size(); 30 | if (xd.size() < n) 31 | xd.resize(n); 32 | 33 | system(x, xd, t); 34 | for (size_t i = 0; i < n; ++i) 35 | { 36 | x[i] += dt * xd[i]; 37 | } 38 | t += dt; 39 | } 40 | 41 | private: 42 | state_t xd; 43 | }; 44 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/Midpoint.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2018 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | // Modified Euler Midpoint integration 18 | 19 | namespace asc 20 | { 21 | template 22 | struct MidpointT 23 | { 24 | using value_t = typename state_t::value_type; 25 | 26 | template 27 | void operator()(System&& system, state_t& x, value_t& t, const value_t dt) 28 | { 29 | const size_t n = x.size(); 30 | if (xd.size() < n) 31 | { 32 | xd.resize(n); 33 | xd_new.resize(n); 34 | } 35 | 36 | system(x, xd_new, t); 37 | for (size_t i = 0; i < n; ++i) 38 | { 39 | x[i] += 0.5 * dt * (xd_new[i] + xd[i]); 40 | } 41 | xd = xd_new; 42 | t += dt; 43 | } 44 | 45 | private: 46 | state_t xd, xd_new; 47 | }; 48 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/PC233.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Utility.h" 18 | #include "ascent/integrators/RK2.h" 19 | 20 | // P-2/PC-3/C-3 algorithm, which has the same error coefficient and order as the P-3/PC-3/C-3 predictor-corrector, but is more stable 21 | 22 | // Real Time (RT) Adam's Moulton predictor-corrector integration 23 | // Source: R.M. Howe. A new family of real-time predictor-corrector integration algorithms. The University of Michigan. September 1991. 24 | 25 | namespace asc 26 | { 27 | template > 28 | struct PC233T 29 | { 30 | using value_t = typename state_t::value_type; 31 | 32 | template 33 | void operator()(System&& system, state_t& x, value_t& t, const value_t dt) 34 | { 35 | if (!initialized) 36 | { 37 | initializer(system, x, t, dt); 38 | xd_1 = initializer.xd; 39 | initialized = true; 40 | return; 41 | } 42 | 43 | const value_t t0 = t; 44 | const value_t dt_3 = cx(1.0 / 3.0)*dt; 45 | 46 | const size_t n = x.size(); 47 | if (xd0.size() < n) 48 | { 49 | xd0.resize(n); 50 | xd_temp.resize(n); 51 | } 52 | 53 | x0 = x; 54 | system(x0, xd0, t); 55 | size_t i{}; 56 | for (; i < n; ++i) 57 | x[i] = x0[i] + c0 * dt * (7.0*xd0[i] - xd_1[i]); // X(n + 1/3), third step computation 58 | t += dt_3; 59 | 60 | system(x, xd_temp, t); 61 | for (i = 0; i < n; ++i) 62 | x[i] = x0[i] + c1 * dt * (39.0*xd_temp[i] - 4.0*xd0[i] + xd_1[i]); // X(n + 2/3), two thirds step computation 63 | t += dt_3; 64 | 65 | system(x, xd_temp, t); 66 | for (i = 0; i < n; ++i) 67 | x[i] = x0[i] + c2 * dt * (xd0[i] + 3.0*xd_temp[i]); 68 | xd_1 = xd0; 69 | t = t0 + dt; 70 | } 71 | 72 | private: 73 | bool initialized{}; 74 | init_integrator initializer; 75 | state_t x0, xd0, xd_temp; 76 | state_t xd_1; // -1, previous time step derivative 77 | 78 | static constexpr auto c0 = cx(1.0 / 18.0); 79 | static constexpr auto c1 = cx(1.0 / 54.0); 80 | static constexpr auto c2 = cx(1.0 / 4.0); 81 | }; 82 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/RK2.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Utility.h" 18 | 19 | // Second order, two pass Runge Kutta. 20 | 21 | namespace asc 22 | { 23 | template 24 | struct RK2T 25 | { 26 | using value_t = typename state_t::value_type; 27 | 28 | template 29 | void operator()(System&& system, state_t& x, value_t& t, const value_t dt) 30 | { 31 | const value_t t0 = t; 32 | const value_t dt_2 = 0.5_v*dt; 33 | 34 | const size_t n = x.size(); 35 | if (xd.size() < n) 36 | xd.resize(n); 37 | 38 | x0 = x; 39 | system(x0, xd, t); 40 | size_t i{}; 41 | for (; i < n; ++i) 42 | x[i] = dt_2 * xd[i] + x0[i]; 43 | t += dt_2; 44 | 45 | system(x, xd, t); 46 | for (i = 0; i < n; ++i) 47 | x[i] = dt * xd[i] + x0[i]; 48 | t = t0 + dt; 49 | } 50 | 51 | state_t xd; 52 | 53 | private: 54 | state_t x0; 55 | }; 56 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/RK4.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Utility.h" 18 | 19 | namespace asc 20 | { 21 | /// Fourth order, four pass Runge Kutta integrator. 22 | /// 23 | /// Designed for optimum speed and minimal memory load. We use one additional multiplication to reduce memory cost by 25%. 24 | /// \tparam state_t The state type of the system to be integrated. I.e. a std::vector or std::deque. 25 | template 26 | struct RK4T 27 | { 28 | using value_t = typename state_t::value_type; 29 | 30 | /// \brief Integration step operation 31 | /// 32 | /// Steps the system a single time step (dt), internally advances time (t) 33 | /// 34 | /// \tparam System The system object or function type. The system to be stepped must have a function syntax that takes \c (state_t, state_t, value_t) \c (x, xd, t). 35 | /// The system can be a function, functor, or lambda expression. Taken as an rvalue reference. 36 | /// \param[in, out] system The \c System instance. 37 | /// \param[in, out] x The system's state vector. 38 | /// \param[in, out] t The current time, which will be advanced by c\ dt. 39 | /// \param[in] dt The time step for the input system to be advanced. 40 | template 41 | void operator()(System&& system, state_t& x, value_t& t, const value_t dt) 42 | { 43 | const value_t t0 = t; 44 | const value_t dt_2 = 0.5_v*dt; 45 | const value_t dt_6 = cx(1.0 / 6.0)*dt; 46 | 47 | const size_t n = x.size(); 48 | if (xd.size() < n) 49 | { 50 | xd.resize(n); 51 | xd_temp.resize(n); 52 | } 53 | 54 | x0 = x; 55 | system(x0, xd, t); 56 | size_t i{}; 57 | for (; i < n; ++i) 58 | x[i] = dt_2 * xd[i] + x0[i]; 59 | t += dt_2; 60 | 61 | system(x, xd_temp, t); 62 | for (i = 0; i < n; ++i) 63 | { 64 | xd[i] += 2 * xd_temp[i]; 65 | x[i] = dt_2 * xd_temp[i] + x0[i]; 66 | } 67 | 68 | system(x, xd_temp, t); 69 | for (i = 0; i < n; ++i) 70 | { 71 | xd[i] += 2 * xd_temp[i]; 72 | x[i] = dt * xd_temp[i] + x0[i]; 73 | } 74 | t = t0 + dt; 75 | 76 | system(x, xd_temp, t); 77 | for (i = 0; i < n; ++i) 78 | x[i] = dt_6 * (xd[i] + xd_temp[i]) + x0[i]; 79 | } 80 | 81 | state_t xd; 82 | 83 | private: 84 | state_t x0, xd_temp; 85 | }; 86 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/RKMM.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Utility.h" 18 | 19 | // Five pass Runge Kutta Merson's Method. With adaptive time stepping. 20 | 21 | namespace asc 22 | { 23 | template 24 | struct RKMMT 25 | { 26 | using value_t = typename state_t::value_type; 27 | 28 | // epsilon is the error tolerance 29 | RKMMT(const value_t epsilon) : epsilon(epsilon), epsilon_64(epsilon / static_cast(64.0)) {} 30 | 31 | template 32 | void operator()(System&& system, state_t& x, value_t& t, value_t& dt) 33 | { 34 | const value_t t0 = t; 35 | const value_t dt_2 = 0.5_v*dt; 36 | const value_t dt_3 = cx(1.0 / 3.0)*dt; 37 | const value_t dt_6 = cx(1.0 / 6.0)*dt; 38 | const value_t dt_8 = cx(1.0 / 8.0)*dt; 39 | 40 | const size_t n = x.size(); 41 | if (xd0.size() < n) 42 | { 43 | xd0.resize(n); 44 | xd2_temp.resize(n); 45 | xd3_temp.resize(n); 46 | xd_temp.resize(n); 47 | x3_temp.resize(n); 48 | } 49 | 50 | x0 = x; 51 | system(x0, xd0, t); 52 | size_t i{}; 53 | for (; i < n; ++i) 54 | x[i] = dt_3 * xd0[i] + x0[i]; 55 | t += dt_3; 56 | 57 | system(x, xd_temp, t); 58 | for (i = 0; i < n; ++i) 59 | x[i] = dt_6 * (xd0[i] + xd_temp[i]) + x0[i]; 60 | 61 | system(x, xd2_temp, t); 62 | for (i = 0; i < n; ++i) 63 | { 64 | xd2_temp[i] *= 3; // all uses of xd2 are 3*xd2, so we perform this multiplication only once 65 | x[i] = dt_8 * (xd0[i] + xd2_temp[i]) + x0[i]; 66 | } 67 | t = t0 + dt_2; 68 | 69 | system(x, xd3_temp, t); 70 | for (i = 0; i < n; ++i) 71 | { 72 | xd3_temp[i] *= 4; // all uses of xd3 are 4*xd3, so we perform this multiplication only once 73 | x[i] = dt_2 * (xd0[i] - xd2_temp[i] + xd3_temp[i]) + x0[i]; 74 | } 75 | x3_temp = x; // used for error estimation 76 | t = t0 + dt; 77 | 78 | system(x, xd_temp, t); 79 | for (i = 0; i < n; ++i) 80 | x[i] = dt_6 * (xd0[i] + xd3_temp[i] + xd_temp[i]) + x0[i]; 81 | 82 | // Adaptive time stepping would probably be best handled by a constexpr if, because we want the handling to be determined at compile time, perhaps by a templated boolean 83 | // https://www.encyclopediaofmath.org/index.php/Kutta-Merson_method#Eq-2 84 | 85 | value_t abs_diff; 86 | value_t max_abs_diff{}; 87 | for (i = 0; i < n; ++i) 88 | { 89 | abs_diff = abs(x3_temp[i] - x[i]); // absolute error estimate 90 | 91 | if (abs_diff > max_abs_diff) 92 | max_abs_diff = abs_diff; 93 | } 94 | 95 | const value_t R = fifth * max_abs_diff; 96 | 97 | if (R > epsilon) 98 | { 99 | dt *= half; // reduce the time step 100 | t = t0; 101 | x = x0; 102 | operator()(system, x, t, dt); // recompute the solution recursively 103 | } 104 | else if (R <= epsilon_64) 105 | dt *= 2; // increase the time step for future steps 106 | } 107 | 108 | private: 109 | state_t x0, xd0, xd2_temp, xd3_temp, xd_temp; 110 | state_t x3_temp; 111 | const value_t epsilon, epsilon_64; 112 | }; 113 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators/RTAM4.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | // UNDER CONSTRUCTION 18 | 19 | #include "ascent/integrators/RK4.h" 20 | 21 | namespace asc 22 | { 23 | template 24 | struct RTAM4T 25 | { 26 | using value_t = typename state_t::value_type; 27 | 28 | template 29 | void operator()(System&& system, state_t& x, value_t& t, const value_t dt) 30 | { 31 | if (!initialized) 32 | { 33 | initializer(system, x, t, dt); 34 | } 35 | 36 | const value_t t0 = t; 37 | 38 | const size_t n = x.size(); 39 | if (xd.size() < n) 40 | { 41 | xd.resize(n); 42 | xd0.resize(n); 43 | xd_1.resize(n); 44 | xd_2.resize(n); 45 | xd_3.resize(n); 46 | } 47 | 48 | x0 = x; 49 | system(x0, xd, t); 50 | xd0 = xd; 51 | size_t i; 52 | for (i = 0; i < n; ++i) 53 | x[i] = x0[i] + dt * (c0*xd[i] + c1*xd_1[i] + c2*xd_2[i] + c3*xd_3[i]); 54 | t += 0.5_v * dt; 55 | 56 | system(x, xd, t); 57 | for (i = 0; i < n; ++i) 58 | x[i] = x0[i] + dt * (c4*xd[i] + c5*xd0[i] + c6*xd_1[i] + c7*xd_2[i]); 59 | t = t0 + dt; 60 | 61 | xd_3 = xd_2; 62 | xd_2 = xd_1; 63 | xd_1 = xd0; 64 | } 65 | 66 | private: 67 | bool initialized{}; 68 | 69 | RK4T initializer; 70 | 71 | static constexpr auto c0 = cx(297.0 / 384.0); 72 | static constexpr auto c1 = cx(-187.0 / 384.0); 73 | static constexpr auto c2 = cx(107.0 / 384.0); 74 | static constexpr auto c3 = cx(-25.0 / 384.0); 75 | 76 | static constexpr auto c4 = cx(36.0 / 30.0); 77 | static constexpr auto c5 = cx(-10.0 / 30.0); 78 | static constexpr auto c6 = cx(5.0 / 30.0); 79 | static constexpr auto c7 = cx(-1.0 / 30.0); 80 | 81 | state_t x0, xd, xd0, xd_1, xd_2, xd_3; 82 | }; 83 | } 84 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_direct/Euler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | // Simple Euler integration. 18 | 19 | namespace asc 20 | { 21 | namespace direct 22 | { 23 | template 24 | struct Euler 25 | { 26 | template 27 | void operator()(system_t&& system, states_t& states, value_t& t, const value_t dt) 28 | { 29 | const size_t n = states.size(); 30 | 31 | system(); 32 | for (size_t i = 0; i < n; ++i) 33 | { 34 | *states[i] += dt * *states[i].xd; 35 | } 36 | t += dt; 37 | } 38 | }; 39 | } 40 | 41 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_direct/RK4.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Utility.h" 18 | 19 | namespace asc 20 | { 21 | namespace direct 22 | { 23 | /// Fourth order, four pass Runge Kutta integrator. 24 | template 25 | struct RK4 26 | { 27 | template 28 | void operator()(system_t&& system, states_t& states, value_t& t, const value_t dt) 29 | { 30 | const auto t0 = t; 31 | const auto dt_2 = 0.5_v*dt; 32 | const auto dt_6 = cx(1.0 / 6.0)*dt; 33 | 34 | const size_t n = states.size(); 35 | if (x0.size() < n) 36 | { 37 | x0.resize(n); 38 | xd.resize(n); 39 | } 40 | 41 | size_t i{}; 42 | for (; i < n; ++i) 43 | x0[i] = *states[i].x; 44 | 45 | system(); 46 | for (i = 0; i < n; ++i) 47 | { 48 | xd[i] = *states[i].xd; 49 | *states[i].x = dt_2 * xd[i] + x0[i]; 50 | } 51 | t += dt_2; 52 | 53 | system(); 54 | for (i = 0; i < n; ++i) 55 | { 56 | const auto& xdi = *states[i].xd; 57 | xd[i] += 2 * xdi; 58 | *states[i].x = dt_2 * xdi + x0[i]; 59 | } 60 | 61 | system(); 62 | for (i = 0; i < n; ++i) 63 | { 64 | const auto& xdi = *states[i].xd; 65 | xd[i] += 2 * xdi; 66 | *states[i].x = dt * xdi + x0[i]; 67 | } 68 | t = t0 + dt; 69 | 70 | system(); 71 | for (i = 0; i < n; ++i) 72 | { 73 | *states[i].x = dt_6 * (xd[i] + *states[i].xd) + x0[i]; 74 | } 75 | } 76 | 77 | std::vector x0, xd; 78 | }; 79 | } 80 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/Euler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | // Simple Euler integration. 18 | 19 | #include "ascent/integrators_modular/ModularIntegrators.h" 20 | 21 | namespace asc 22 | { 23 | namespace modular 24 | { 25 | template 26 | struct EulerProp : public Propagator 27 | { 28 | void operator()(State& state, const value_t dt) override 29 | { 30 | *state.x += dt * *state.xd; 31 | } 32 | }; 33 | 34 | template 35 | struct Euler 36 | { 37 | asc::Module* run_first{}; 38 | 39 | EulerProp propagator; 40 | 41 | template 42 | void operator()(modules_t& blocks, value_t& t, const value_t dt) 43 | { 44 | update(blocks, run_first); 45 | propagate(blocks, propagator, dt); 46 | t += dt; 47 | postprop(blocks); 48 | } 49 | }; 50 | } 51 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/Heun.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | // Heun's Method 21 | 22 | namespace asc 23 | { 24 | namespace modular 25 | { 26 | template 27 | struct Heunprop : public Propagator 28 | { 29 | void operator()(State& state, const value_t dt) override 30 | { 31 | auto& x = *state.x; 32 | auto& xd = *state.xd; 33 | state.memory.resize(2); 34 | auto& x0 = state.memory[0]; 35 | auto& xd0 = state.memory[1]; 36 | 37 | switch (Propagator::pass) 38 | { 39 | case 0: 40 | x0 = x; 41 | xd0 = xd; //k1 42 | x = x0 + dt * xd; 43 | break; 44 | case 1: 45 | x = x0 + dt * 0.5 * ( xd0 + xd ); 46 | break; 47 | } 48 | } 49 | }; 50 | 51 | template 52 | struct Heunstepper : public TimeStepper 53 | { 54 | value_t t0{}; 55 | 56 | void operator()(const size_t pass, value_t& t, const value_t dt) override 57 | { 58 | switch (pass) 59 | { 60 | case 0: 61 | t0 = t; 62 | t += dt; 63 | break; 64 | case 1: 65 | t = t0 + dt; 66 | break; 67 | default: 68 | break; 69 | } 70 | } 71 | }; 72 | 73 | template 74 | struct Heun 75 | { 76 | asc::Timing* timing{}; 77 | 78 | Heunprop propagator; 79 | Heunstepper stepper; 80 | 81 | template 82 | void operator()(modules_t& modules, value_t& t, const value_t dt) 83 | { 84 | auto& pass = propagator.pass; 85 | pass = 0; 86 | 87 | update(modules); 88 | propagate(modules, dt); 89 | stepper(pass, t, dt); 90 | ++pass; 91 | 92 | update(modules); 93 | propagate(modules, dt); 94 | stepper(pass, t, dt); 95 | } 96 | 97 | template 98 | void update(modules_t& modules) 99 | { 100 | if (timing) 101 | { 102 | (*timing)(); 103 | } 104 | for (auto& module : modules) 105 | { 106 | (*module)(); 107 | } 108 | } 109 | 110 | template 111 | void propagate(modules_t& modules, const value_t dt) 112 | { 113 | for (auto& module : modules) 114 | { 115 | module->propagate(propagator, dt); 116 | } 117 | } 118 | }; 119 | } 120 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/Midpoint.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | namespace asc 21 | { 22 | namespace modular 23 | { 24 | template 25 | struct MidpointProp : public Propagator 26 | { 27 | void operator()(State& state, const value_t dt) override 28 | { 29 | if (state.memory.empty()) 30 | { 31 | state.memory.resize(1); 32 | state.memory[0] = 0.0; 33 | } 34 | auto& xd0 = state.memory[0]; 35 | 36 | auto& xd = *state.xd; 37 | *state.x += 0.5 * dt * (xd0 + xd); 38 | xd0 = xd; 39 | } 40 | }; 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/ModularIntegrators.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace asc 18 | { 19 | namespace modular 20 | { 21 | template 22 | struct TimeStepper 23 | { 24 | TimeStepper() = default; 25 | TimeStepper(const TimeStepper&) = default; 26 | TimeStepper(TimeStepper&&) = default; 27 | TimeStepper& operator=(const TimeStepper&) = default; 28 | TimeStepper& operator=(TimeStepper&&) = default; 29 | virtual ~TimeStepper() {} 30 | 31 | virtual void operator()(const size_t, value_t&, const value_t) = 0; // inputs: pass, t (time), dt (time step) 32 | }; 33 | } 34 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/NCRK4.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | // Non-confluent RK4 method ( "3/8th's Rule" ) 21 | 22 | namespace asc 23 | { 24 | namespace modular 25 | { 26 | template 27 | struct NCRK4prop : public Propagator 28 | { 29 | void operator()(State& state, const value_t dt) override 30 | { 31 | auto& x = *state.x; 32 | auto& xd = *state.xd; 33 | state.memory.resize(5); 34 | auto& x0 = state.memory[0]; 35 | auto& xd0 = state.memory[1]; 36 | auto& xd1 = state.memory[2]; 37 | auto& xd2 = state.memory[3]; 38 | auto& xd3 = state.memory[4]; 39 | 40 | switch (Propagator::pass) 41 | { 42 | case 0: 43 | x0 = x; 44 | xd0 = xd; // k1 45 | x = x0 + (1.0/3.0) * dt * xd0; 46 | break; 47 | case 1: 48 | xd1 = xd; // k2 49 | x = x0 + dt * ( (-1.0/3.0) * xd0 + xd1) ; 50 | break; 51 | case 2: 52 | xd2 = xd; // k3 53 | x = x0 + dt * ( xd0 - xd1 + xd2); 54 | break; 55 | case 3: 56 | xd3 = xd; // k4 57 | x = x0 + dt / 8.0 * (xd0 + 3.0 * xd1 + 3.0 * xd2 + xd3); 58 | break; 59 | } 60 | } 61 | }; 62 | 63 | template 64 | struct NCRK4stepper : public TimeStepper 65 | { 66 | value_t t0{}; 67 | 68 | void operator()(const size_t pass, value_t& t, const value_t dt) override 69 | { 70 | switch (pass) 71 | { 72 | case 0: 73 | t0 = t; 74 | t += 1.0/3.0 * dt; 75 | break; 76 | case 1: 77 | t = t0 + 2.0/3.0 * dt; 78 | break; 79 | case 2: 80 | t = t0 + dt; 81 | break; 82 | default: 83 | break; 84 | } 85 | } 86 | }; 87 | 88 | template 89 | struct NCRK4 90 | { 91 | NCRK4prop propagator; 92 | NCRK4stepper stepper; 93 | 94 | template 95 | void operator()(modules_t& modules, value_t& t, const value_t dt) 96 | { 97 | auto& pass = propagator.pass; 98 | pass = 0; 99 | 100 | update(modules); 101 | propagate(modules, dt); 102 | stepper(pass, t, dt); 103 | ++pass; 104 | 105 | update(modules); 106 | propagate(modules, dt); 107 | ++pass; 108 | 109 | update(modules); 110 | propagate(modules, dt); 111 | stepper(pass, t, dt); 112 | ++pass; 113 | 114 | update(modules); 115 | propagate(modules, dt); 116 | } 117 | 118 | template 119 | void update(modules_t& modules) 120 | { 121 | for (auto& module : modules) 122 | { 123 | (*module)(); 124 | } 125 | } 126 | 127 | template 128 | void propagate(modules_t& modules, const value_t dt) 129 | { 130 | for (auto& module : modules) 131 | { 132 | module->propagate(propagator, dt); 133 | } 134 | } 135 | }; 136 | } 137 | } 138 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/PC233.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/Utility.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | #include "ascent/integrators_modular/RK4.h" 20 | 21 | // P-2/PC-3/C-3 algorithm, which has the same error coefficient and order as the P-3/PC-3/C-3 predictor-corrector, but is more stable 22 | 23 | // Real Time (RT) Adam's Moulton predictor-corrector integration 24 | // Source: R.M. Howe. A new family of real-time predictor-corrector integration algorithms. The University of Michigan. September 1991. 25 | 26 | namespace asc 27 | { 28 | namespace modular 29 | { 30 | template 31 | struct PC233prop : public Propagator 32 | { 33 | void operator()(State& state, const value_t dt) override 34 | { 35 | auto& x = *state.x; 36 | auto& xd = *state.xd; 37 | state.memory.resize(5); 38 | auto& x0 = state.memory[0]; 39 | auto& xd0 = state.memory[1]; 40 | auto& xd1 = state.memory[2]; 41 | auto& xd2 = state.memory[3]; 42 | auto& xd_1 = state.memory[4]; 43 | 44 | switch (Propagator::pass) 45 | { 46 | case 0: 47 | x0 = x; 48 | xd0 = xd; 49 | x = x0 + c0 * dt * (7 * xd0 - xd_1); // X(n + 1/3), third step computation 50 | break; 51 | case 1: 52 | xd1 = xd; 53 | x = x0 + c1 * dt * (39 * xd1 - 4 * xd0 + xd_1); // X(n + 2/3), two thirds step computation 54 | break; 55 | case 2: 56 | xd2 = xd; 57 | x = x0 + c2 * dt * (xd0 + 3 * xd2); 58 | xd_1 = xd0; 59 | break; 60 | } 61 | } 62 | 63 | private: 64 | static constexpr auto c0 = cx(1.0 / 18.0); 65 | static constexpr auto c1 = cx(1.0 / 54.0); 66 | static constexpr auto c2 = cx(1.0 / 4.0); 67 | }; 68 | 69 | template 70 | struct PC233stepper : public TimeStepper 71 | { 72 | value_t t0{}; 73 | 74 | void operator()(const size_t pass, value_t& t, const value_t dt) override 75 | { 76 | switch (pass) 77 | { 78 | case 0: 79 | t0 = t; 80 | t += (1.0 / 3.0) * dt; 81 | break; 82 | case 1: 83 | t += (1.0 / 3.0) * dt; 84 | break; 85 | case 2: 86 | t = t0 + dt; 87 | break; 88 | } 89 | } 90 | }; 91 | 92 | template > 93 | struct PC233 94 | { 95 | asc::Module* run_first{}; 96 | 97 | PC233prop propagator; 98 | PC233stepper stepper; 99 | 100 | template 101 | void operator()(modules_t& blocks, value_t& t, const value_t dt) 102 | { 103 | if (!initialized) 104 | { 105 | if (run_first) 106 | { 107 | initializer.run_first = run_first; 108 | } 109 | // Run initializer integrator 110 | initializer(blocks, t, dt); 111 | 112 | if constexpr (is_pair_v::value_type>) 113 | { 114 | // Assign previous time step's derivative 115 | for (auto& block : blocks) 116 | { 117 | for (auto& state : block.second->states) 118 | { 119 | state.memory.resize(5); 120 | auto& xd_1 = state.memory[4]; 121 | xd_1 = *state.xd; 122 | } 123 | } 124 | } 125 | else 126 | { 127 | // Assign previous time step's derivative 128 | for (auto& block : blocks) 129 | { 130 | for (auto& state : block->states) 131 | { 132 | state.memory.resize(5); 133 | auto& xd_1 = state.memory[4]; 134 | xd_1 = *state.xd; 135 | } 136 | } 137 | } 138 | 139 | initialized = true; 140 | return; 141 | } 142 | auto& pass = propagator.pass; 143 | pass = 0; 144 | 145 | update(blocks); 146 | propagate(blocks, propagator, dt); 147 | stepper(pass, t, dt); 148 | postprop(blocks); 149 | ++pass; 150 | 151 | update(blocks); 152 | propagate(blocks, propagator, dt); 153 | stepper(pass, t, dt); 154 | postprop(blocks); 155 | ++pass; 156 | 157 | update(blocks); 158 | propagate(blocks, propagator, dt); 159 | stepper(pass, t, dt); 160 | postprop(blocks); 161 | } 162 | 163 | private: 164 | bool initialized{}; 165 | init_integrator initializer; 166 | }; 167 | } 168 | 169 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/RK2.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | //#include "ascent/Utility.h" 18 | #include "ascent/modular/Module.h" 19 | #include "ascent/integrators_modular/ModularIntegrators.h" 20 | 21 | // Second order, two pass Runge Kutta. 22 | 23 | namespace asc 24 | { 25 | namespace modular 26 | { 27 | template 28 | struct RK2prop : public Propagator 29 | { 30 | void operator()(State& state, const value_t dt) override 31 | { 32 | auto& x = *state.x; 33 | auto& xd = *state.xd; 34 | if (state.memory.size() < 1) 35 | { 36 | state.memory.resize(1); 37 | } 38 | auto& x0 = state.memory[0]; 39 | 40 | switch (Propagator::pass) 41 | { 42 | case 0: 43 | x0 = x; 44 | x = x0 + 0.5 * dt * xd; 45 | break; 46 | case 1: 47 | x = x0 + dt * xd; 48 | break; 49 | default: 50 | break; 51 | } 52 | } 53 | }; 54 | 55 | template 56 | struct RK2stepper : public TimeStepper 57 | { 58 | value_t t0{}; 59 | 60 | void operator()(const size_t pass, value_t& t, const value_t dt) override 61 | { 62 | switch (pass) 63 | { 64 | case 0: 65 | t0 = t; 66 | t += 0.5 * dt; 67 | break; 68 | case 1: 69 | t = t0 + dt; 70 | break; 71 | default: 72 | break; 73 | } 74 | } 75 | }; 76 | 77 | template 78 | struct RK2 79 | { 80 | asc::Module* run_first{}; 81 | 82 | RK2prop propagator; 83 | RK2stepper stepper; 84 | 85 | template 86 | void operator()(modules_t& blocks, value_t& t, const value_t dt) 87 | { 88 | auto& pass = propagator.pass; 89 | pass = 0; 90 | 91 | update(blocks); 92 | propagate(blocks, propagator, dt); 93 | stepper(pass, t, dt); 94 | postprop(blocks); 95 | ++pass; 96 | 97 | update(blocks); 98 | propagate(blocks, propagator, dt); 99 | stepper(pass, t, dt); 100 | postprop(blocks); 101 | } 102 | }; 103 | } 104 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/RK3.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | // Third order, Three stage Runge Kutta (Heun's 3rd Order). 21 | 22 | namespace asc 23 | { 24 | namespace modular 25 | { 26 | template 27 | struct RK3prop : public Propagator 28 | { 29 | void operator()(State& state, const value_t dt) override 30 | { 31 | auto& x = *state.x; 32 | auto& xd = *state.xd; 33 | state.memory.resize(4); 34 | auto& x0 = state.memory[0]; 35 | auto& xd0 = state.memory[1]; 36 | auto& xd1 = state.memory[2]; 37 | auto& xd2 = state.memory[3]; 38 | 39 | switch (Propagator::pass) 40 | { 41 | case 0: 42 | x0 = x; 43 | xd0 = xd; // k1 44 | x = x0 + dt * (1.0/3.0) * xd0; 45 | break; 46 | case 1: 47 | xd1 = xd; // k2 48 | x = x0 + dt * (2.0/3.0) * xd1; 49 | break; 50 | case 2: 51 | xd2 = xd; // k3 52 | x = x0 + dt * ( (1.0/4.0)*xd0 + (3.0/4.0)*xd2 ); 53 | break; 54 | } 55 | } 56 | }; 57 | 58 | template 59 | struct RK3stepper : public TimeStepper 60 | { 61 | value_t t0{}; 62 | 63 | void operator()(const size_t pass, value_t& t, const value_t dt) override 64 | { 65 | switch (pass) 66 | { 67 | case 0: 68 | t0 = t; 69 | t += (1.0/3.0) * dt; 70 | break; 71 | case 1: 72 | t = t0 + (2.0/3.0) * dt; 73 | break; 74 | case 2: 75 | t = t0 + dt; 76 | break; 77 | default: 78 | break; 79 | } 80 | } 81 | }; 82 | 83 | template 84 | struct RK3 85 | { 86 | asc::Timing* timing{}; 87 | 88 | RK3prop propagator; 89 | RK3stepper stepper; 90 | 91 | template 92 | void operator()(modules_t& modules, value_t& t, const value_t dt) 93 | { 94 | auto& pass = propagator.pass; 95 | pass = 0; 96 | 97 | update(modules); 98 | propagate(modules, dt); 99 | stepper(pass, t, dt); 100 | ++pass; 101 | 102 | update(modules); 103 | propagate(modules, dt); 104 | stepper(pass, t, dt); 105 | ++pass; 106 | 107 | update(modules); 108 | propagate(modules, dt); 109 | stepper(pass, t, dt); 110 | } 111 | 112 | template 113 | void update(modules_t& modules) 114 | { 115 | if (timing) 116 | { 117 | (*timing)(); 118 | } 119 | for (auto& module : modules) 120 | { 121 | (*module)(); 122 | } 123 | } 124 | 125 | template 126 | void propagate(modules_t& modules, const value_t dt) 127 | { 128 | for (auto& module : modules) 129 | { 130 | module->propagate(propagator, dt); 131 | } 132 | } 133 | }; 134 | } 135 | } 136 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/RK4.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | namespace asc 21 | { 22 | namespace modular 23 | { 24 | template 25 | struct RK4prop : public Propagator 26 | { 27 | void operator()(State& state, const value_t dt) override 28 | { 29 | auto& x = *state.x; 30 | auto& xd = *state.xd; 31 | if (state.memory.size() < 5) 32 | { 33 | state.memory.resize(5); 34 | } 35 | auto& x0 = state.memory[0]; 36 | auto& xd0 = state.memory[1]; 37 | auto& xd1 = state.memory[2]; 38 | auto& xd2 = state.memory[3]; 39 | auto& xd3 = state.memory[4]; 40 | 41 | switch (Propagator::pass) 42 | { 43 | case 0: 44 | x0 = x; 45 | xd0 = xd; 46 | x = x0 + 0.5 * dt * xd0; 47 | break; 48 | case 1: 49 | xd1 = xd; 50 | x = x0 + 0.5 * dt * xd1; 51 | break; 52 | case 2: 53 | xd2 = xd; 54 | x = x0 + dt * xd2; 55 | break; 56 | case 3: 57 | xd3 = xd; 58 | x = x0 + dt / 6.0 * (xd0 + 2 * xd1 + 2 * xd2 + xd3); 59 | break; 60 | } 61 | } 62 | }; 63 | 64 | template 65 | struct RK4stepper : public TimeStepper 66 | { 67 | value_t t0{}; 68 | 69 | void operator()(const size_t pass, value_t& t, const value_t dt) override 70 | { 71 | switch (pass) 72 | { 73 | case 0: 74 | t0 = t; 75 | t += 0.5 * dt; 76 | break; 77 | case 2: 78 | t = t0 + dt; 79 | break; 80 | default: 81 | break; 82 | } 83 | } 84 | }; 85 | 86 | template 87 | struct RK4 88 | { 89 | asc::Module* run_first{}; 90 | 91 | RK4prop propagator; 92 | RK4stepper stepper; 93 | 94 | template 95 | void operator()(modules_t& blocks, value_t& t, const value_t dt) 96 | { 97 | auto& pass = propagator.pass; 98 | pass = 0; 99 | 100 | update(blocks, run_first); 101 | propagate(blocks, propagator, dt); 102 | stepper(pass, t, dt); 103 | postprop(blocks); 104 | ++pass; 105 | 106 | update(blocks, run_first); 107 | propagate(blocks, propagator, dt); 108 | postprop(blocks); 109 | ++pass; 110 | 111 | update(blocks, run_first); 112 | propagate(blocks, propagator, dt); 113 | stepper(pass, t, dt); 114 | postprop(blocks); 115 | ++pass; 116 | 117 | update(blocks, run_first); 118 | propagate(blocks, propagator, dt); 119 | postprop(blocks); 120 | } 121 | }; 122 | } 123 | } 124 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/RTAM2.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | // Second order, two pass Real Time (RT) Adams Moulton predictor-corrector integrator 21 | // A new family of real-time predictor-corrector integration algorithms 22 | // R.M. Howe. A new family of real-time predictor-corrector integration algorithms. The University of Michigan. September 1991. 23 | 24 | namespace asc 25 | { 26 | namespace modular 27 | { 28 | template 29 | struct RTAM2prop : public Propagator 30 | { 31 | void operator()(State& state, const value_t dt) override 32 | { 33 | auto& x = *state.x; 34 | auto& xd = *state.xd; 35 | if (state.memory.size() < 2) 36 | { 37 | state.memory.resize(2); 38 | } 39 | auto& x0 = state.memory[0]; 40 | auto& xd1 = state.memory[1]; 41 | 42 | switch (Propagator::pass) 43 | { 44 | case 0: 45 | x0 = x; 46 | x = x0 + dt / 8 * (5 * xd - xd1); 47 | xd1 = xd; 48 | break; 49 | case 1: 50 | x = x0 + dt * xd; // where xd is the evaluated derivative at n + 1/2 (half a step) 51 | break; 52 | } 53 | } 54 | }; 55 | 56 | template 57 | struct RTAM2stepper : public TimeStepper 58 | { 59 | value_t t0{}; 60 | 61 | void operator()(const size_t pass, value_t& t, const value_t dt) override 62 | { 63 | switch (pass) 64 | { 65 | case 0: 66 | t0 = t; 67 | t += 0.5 * dt; 68 | break; 69 | case 1: 70 | t = t0 + dt; 71 | break; 72 | default: 73 | break; 74 | } 75 | } 76 | }; 77 | 78 | template 79 | struct RTAM2 80 | { 81 | asc::Timing* timing{}; 82 | 83 | RTAM2prop propagator; 84 | RTAM2stepper stepper; 85 | 86 | template 87 | void operator()(modules_t& modules, value_t& t, const value_t dt) 88 | { 89 | auto& pass = propagator.pass; 90 | pass = 0; 91 | 92 | update(modules); 93 | propagate(modules, dt); 94 | stepper(pass, t, dt); 95 | ++pass; 96 | 97 | update(modules); 98 | propagate(modules, dt); 99 | stepper(pass, t, dt); 100 | } 101 | 102 | template 103 | void update(modules_t& modules) 104 | { 105 | if (timing) 106 | { 107 | (*timimg)(); 108 | } 109 | for (auto& module : modules) 110 | { 111 | (*module)(); 112 | } 113 | } 114 | 115 | template 116 | void propagate(modules_t& modules, const value_t dt) 117 | { 118 | for (auto& module : modules) 119 | { 120 | module->propagate(propagator, dt); 121 | } 122 | } 123 | }; 124 | } 125 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/RTAM3.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | // Third order, two pass Real Time (RT) Adams Moulton predictor-corrector integrator 21 | // A new family of real-time predictor-corrector integration algorithms 22 | // R.M. Howe. A new family of real-time predictor-corrector integration algorithms. The University of Michigan. September 1991. 23 | 24 | namespace asc 25 | { 26 | namespace modular 27 | { 28 | template 29 | struct RTAM3prop : public Propagator 30 | { 31 | void operator()(State& state, const value_t dt) override 32 | { 33 | auto& x = *state.x; 34 | auto& xd = *state.xd; 35 | if (state.memory.size() < 4) 36 | { 37 | state.memory.resize(4); 38 | } 39 | auto& x0 = state.memory[0]; 40 | auto& xd0 = state.memory[1]; 41 | auto& xd1 = state.memory[2]; 42 | auto& xd2 = state.memory[3]; 43 | 44 | switch (Propagator::pass) 45 | { 46 | case 0: 47 | x0 = x; 48 | xd0 = xd; 49 | x = x0 + dt / 24 * (17 * xd - 7 * xd1 + 2 * xd2); 50 | break; 51 | case 1: 52 | x = x0 + dt / 18 * (20 * xd - 3 * xd0 + xd1); 53 | xd2 = xd1; 54 | xd1 = xd0; 55 | break; 56 | } 57 | } 58 | }; 59 | 60 | template 61 | struct RTAM3stepper : public TimeStepper 62 | { 63 | value_t t0{}; 64 | 65 | void operator()(const size_t pass, value_t& t, const value_t dt) override 66 | { 67 | switch (pass) 68 | { 69 | case 0: 70 | t0 = t; 71 | t += 0.5 * dt; 72 | break; 73 | case 1: 74 | t = t0 + dt; 75 | break; 76 | default: 77 | break; 78 | } 79 | } 80 | }; 81 | 82 | template 83 | struct RTAM3 84 | { 85 | asc::Timing* timing{}; 86 | 87 | RTAM3prop propagator; 88 | RTAM3stepper stepper; 89 | 90 | template 91 | void operator()(modules_t& modules, value_t& t, const value_t dt) 92 | { 93 | auto& pass = propagator.pass; 94 | pass = 0; 95 | 96 | update(modules); 97 | propagate(modules, dt); 98 | stepper(pass, t, dt); 99 | ++pass; 100 | 101 | update(modules); 102 | propagate(modules, dt); 103 | stepper(pass, t, dt); 104 | } 105 | 106 | template 107 | void update(modules_t& modules) 108 | { 109 | if (timing) 110 | { 111 | (*timing)(); 112 | } 113 | for (auto& module : modules) 114 | { 115 | (*module)(); 116 | } 117 | } 118 | 119 | template 120 | void propagate(modules_t& modules, const value_t dt) 121 | { 122 | for (auto& module : modules) 123 | { 124 | module->propagate(propagator, dt); 125 | } 126 | } 127 | }; 128 | } 129 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/integrators_modular/RTAM4.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | #include "ascent/integrators_modular/ModularIntegrators.h" 19 | 20 | // Fourth order, two pass Real Time (RT) Adams Moulton predictor-corrector integrator 21 | // A new family of real-time predictor-corrector integration algorithms 22 | // R.M. Howe. A new family of real-time predictor-corrector integration algorithms. The University of Michigan. September 1991. 23 | 24 | namespace asc 25 | { 26 | namespace modular 27 | { 28 | template 29 | struct RTAM4prop : public Propagator 30 | { 31 | void operator()(State& state, const value_t dt) override 32 | { 33 | auto& x = *state.x; 34 | auto& xd = *state.xd; 35 | if (state.memory.size() < 5) 36 | { 37 | state.memory.resize(5); 38 | } 39 | auto& x0 = state.memory[0]; 40 | auto& xd0 = state.memory[1]; 41 | auto& xd1 = state.memory[2]; 42 | auto& xd2 = state.memory[3]; 43 | auto& xd3 = state.memory[4]; 44 | 45 | switch (Propagator::pass) 46 | { 47 | case 0: 48 | x0 = x; 49 | xd0 = xd; 50 | x = x0 + dt / 384 * (297 * xd - 187 * xd1 + 107 * xd2 - 25 * xd3); 51 | break; 52 | case 1: 53 | x = x0 + dt / 30 * (36 * xd - 10 * xd0 + 5 * xd1 - xd2); 54 | xd3 = xd2; 55 | xd2 = xd1; 56 | xd1 = xd0; 57 | break; 58 | } 59 | } 60 | }; 61 | 62 | template 63 | struct RTAM4stepper : public TimeStepper 64 | { 65 | value_t t0{}; 66 | 67 | void operator()(const size_t pass, value_t& t, const value_t dt) override 68 | { 69 | switch (pass) 70 | { 71 | case 0: 72 | t0 = t; 73 | t += 0.5 * dt; 74 | break; 75 | case 1: 76 | t = t0 + dt; 77 | break; 78 | default: 79 | break; 80 | } 81 | } 82 | }; 83 | 84 | template 85 | struct RTAM4 86 | { 87 | asc::Timing* timing{}; 88 | 89 | RTAM4prop propagator; 90 | RTAM4stepper stepper; 91 | 92 | template 93 | void operator()(modules_t& modules, value_t& t, const value_t dt) 94 | { 95 | auto& pass = propagator.pass; 96 | pass = 0; 97 | 98 | update(modules); 99 | propagate(modules, dt); 100 | stepper(pass, t, dt); 101 | ++pass; 102 | 103 | update(modules); 104 | propagate(modules, dt); 105 | stepper(pass, t, dt); 106 | } 107 | 108 | template 109 | void update(modules_t& modules) 110 | { 111 | if (timing) 112 | { 113 | (*timing)(); 114 | } 115 | for (auto& module : modules) 116 | { 117 | (*module)(); 118 | } 119 | } 120 | 121 | template 122 | void propagate(modules_t& modules, const value_t dt) 123 | { 124 | for (auto& module : modules) 125 | { 126 | module->propagate(propagator, dt); 127 | } 128 | } 129 | }; 130 | } 131 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/modular/Link.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/modular/Module.h" 18 | 19 | #include 20 | 21 | namespace asc 22 | { 23 | template 24 | struct Link 25 | { 26 | std::remove_cv_t>* module{}; // Non-const qualified module for doing low level, potentially dangerous simulation stuff. 27 | // The pointer will be returned with T qualifiers, so that the module can be const qualified. 28 | 29 | Link& operator=(const T* ptr) noexcept 30 | { 31 | module = ptr; 32 | return *this; 33 | } 34 | 35 | T& operator*() 36 | { 37 | check(); 38 | return *module; 39 | } 40 | 41 | T* operator->() 42 | { 43 | check(); 44 | return module; 45 | } 46 | 47 | explicit operator bool() const noexcept { return (module != 0); } 48 | 49 | std::string to_string() const { return "Link<" + static_cast(typeid(T).name()) + '>'; } 50 | 51 | private: 52 | void check_init() 53 | { 54 | if (!module->init_run) 55 | { 56 | if (module->init_called) 57 | { 58 | throw std::runtime_error("Circular dependency for init(): " + to_string()); 59 | } 60 | else 61 | { 62 | module->init_called = true; 63 | module->init(); 64 | } 65 | module->init_run = true; 66 | } 67 | } 68 | 69 | void check() 70 | { 71 | if (module) 72 | { 73 | assert(module->phase); 74 | // The Link phase and Postprop phase do not check initialization. 75 | // Linking may occur prior to initialization and is not ordered. 76 | // Postprop is not sequenced, as calculations may only be performed on propagated states 77 | switch (*module->phase) 78 | { 79 | case Phase::Init: 80 | check_init(); 81 | return; 82 | case Phase::Update: 83 | check_init(); 84 | if (!module->update_run) 85 | { 86 | if (module->update_called) 87 | { 88 | throw std::runtime_error("Circular dependency for update operator(): " + to_string()); 89 | } 90 | else 91 | { 92 | module->update_called = true; 93 | module->operator()(); 94 | } 95 | module->update_run = true; 96 | } 97 | return; 98 | case Phase::Postcalc: 99 | check_init(); 100 | return; 101 | case default: 102 | return; 103 | } 104 | } 105 | else 106 | { 107 | throw std::runtime_error(("nullptr access ->: " + to_string()).c_str()); 108 | } 109 | } 110 | }; 111 | 112 | template bool operator==(const Link& link, std::nullptr_t ptr) noexcept { return link.module == ptr; } 113 | template bool operator==(std::nullptr_t ptr, const Link& link) noexcept { return link.module == ptr; } 114 | 115 | template bool operator!=(const Link& link, std::nullptr_t ptr) noexcept { return link.module != ptr; } 116 | template bool operator!=(std::nullptr_t ptr, const Link& link) noexcept { return link.module != ptr; } 117 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/modular/Module.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "ascent/direct/State.h" 18 | 19 | namespace asc 20 | { 21 | template 22 | struct is_pair : std::false_type { }; 23 | 24 | template 25 | struct is_pair> : std::true_type { }; 26 | 27 | template 28 | constexpr bool is_pair_v = is_pair::value; 29 | 30 | template 31 | struct Propagator 32 | { 33 | Propagator() = default; 34 | Propagator(const Propagator&) = default; 35 | Propagator(Propagator&&) = default; 36 | Propagator& operator=(const Propagator&) = default; 37 | Propagator& operator=(Propagator&&) = default; 38 | virtual ~Propagator() {} 39 | 40 | virtual void operator()(State&, const double) = 0; // inputs: state, dt (time step) 41 | 42 | size_t pass{}; 43 | }; 44 | 45 | enum struct Phase 46 | { 47 | Link, 48 | Init, 49 | Update, 50 | Postprop, 51 | Postcalc 52 | }; 53 | 54 | struct Module 55 | { 56 | Module() = default; 57 | Module(const Module&) = default; 58 | Module(Module&&) = default; 59 | Module& operator=(const Module&) = default; 60 | Module& operator=(Module&&) = default; 61 | virtual ~Module() = default; 62 | 63 | std::vector states; 64 | 65 | template 66 | void make_state(x_t& x, xd_t& xd) 67 | { 68 | states.emplace_back(x, xd); 69 | } 70 | 71 | template 72 | void make_states(x_t& x, xd_t& xd) 73 | { 74 | const size_t n = x.size(); 75 | for (size_t i = 0; i < n; ++i) 76 | { 77 | states.emplace_back(x[i], xd[i]); 78 | } 79 | } 80 | 81 | template 82 | void make_states(data_t* x, data_t* xd, const size_t n) 83 | { 84 | for (size_t i = 0; i < n; ++i) 85 | { 86 | states.emplace_back(x[i], xd[i]); 87 | } 88 | } 89 | 90 | template 91 | void add_states(states_t& ext_states) 92 | { 93 | for (auto& state : states) 94 | { 95 | ext_states.emplace_back(state); 96 | } 97 | } 98 | 99 | virtual void link() {} // linking modules 100 | virtual void init() {} // initialization 101 | virtual void operator()() {} // derivative updates 102 | virtual void propagate(Propagator& propagator, const double dt) 103 | { 104 | for (auto& state : states) 105 | { 106 | propagator(state, dt); 107 | } 108 | } 109 | virtual void postprop() {} // post propagation calculations (every substep) 110 | virtual void postcalc() {} // post integration calculations (every full step) 111 | 112 | bool init_called = false; 113 | }; 114 | 115 | template 116 | inline void init(modules_t& blocks) 117 | { 118 | for (auto& block : blocks) 119 | { 120 | if (!block->init_called) 121 | { 122 | block->init(); 123 | block->init_called = true; 124 | } 125 | } 126 | } 127 | 128 | template 129 | void update(modules_t& blocks) 130 | { 131 | if constexpr (is_pair_v::value_type>) 132 | { 133 | for (auto& block : blocks) 134 | { 135 | block.second->operator()(); 136 | } 137 | } 138 | else 139 | { 140 | for (auto& block : blocks) 141 | { 142 | (*block)(); 143 | } 144 | } 145 | } 146 | 147 | template 148 | void update(modules_t& blocks, asc::Module* run_first) 149 | { 150 | if (run_first) 151 | { 152 | (*run_first)(); 153 | } 154 | update(blocks); 155 | } 156 | 157 | template 158 | void propagate(modules_t& blocks, propagator_t& propagator, const value_t dt) 159 | { 160 | if constexpr (is_pair_v::value_type>) 161 | { 162 | for (auto& block : blocks) 163 | { 164 | block.second->propagate(propagator, dt); 165 | } 166 | } 167 | else 168 | { 169 | for (auto& block : blocks) 170 | { 171 | block->propagate(propagator, dt); 172 | } 173 | } 174 | } 175 | 176 | template 177 | void postprop(modules_t& blocks) 178 | { 179 | if constexpr (is_pair_v::value_type>) 180 | { 181 | for (auto& block : blocks) 182 | { 183 | block.second->postprop(); 184 | } 185 | } 186 | else 187 | { 188 | for (auto& block : blocks) 189 | { 190 | block->postprop(); 191 | } 192 | } 193 | } 194 | 195 | template 196 | inline void postcalc(modules_t& blocks) 197 | { 198 | if constexpr (is_pair_v::value_type>) 199 | { 200 | for (auto& block : blocks) 201 | { 202 | block.second->postcalc(); 203 | } 204 | } 205 | else 206 | { 207 | for (auto& block : blocks) 208 | { 209 | block->postcalc(); 210 | } 211 | } 212 | } 213 | 214 | template 215 | inline void add_states(states_t& states, std::vector& blocks) 216 | { 217 | for (auto& block : blocks) 218 | { 219 | auto& m_states = block->states; 220 | for (auto& state : m_states) 221 | { 222 | states.emplace_back(state); 223 | } 224 | } 225 | } 226 | 227 | template 228 | [[deprecated("use add_states with pointer types instead: Module*")]] inline void add_states(states_t& states, Module& block) 229 | { 230 | for (auto& state : block.states) 231 | { 232 | states.emplace_back(state); 233 | } 234 | } 235 | 236 | template 237 | inline void add_states(states_t& states, ptr_t& block) 238 | { 239 | for (auto& state : block->states) 240 | { 241 | states.emplace_back(state); 242 | } 243 | } 244 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/threading/Pool.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2018 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | #pragma once 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace asc { 24 | // A simple threadpool 25 | class Pool 26 | { 27 | public: 28 | Pool(const unsigned int n = std::thread::hardware_concurrency()) 29 | { 30 | n_threads(n); 31 | } 32 | void n_threads(const unsigned int n) 33 | { 34 | for (size_t i = threads.size(); i < n; ++i) 35 | threads.emplace_back(std::thread(&Pool::worker, this)); 36 | } 37 | void emplace_back(std::function&& task) 38 | { 39 | std::lock_guard lock(m); 40 | queue.emplace_back(std::forward>(task)); 41 | work_cv.notify_one(); 42 | } 43 | bool computing() const 44 | { 45 | return (working != 0); 46 | } 47 | void wait() { 48 | std::unique_lock lock(m); 49 | if (queue.empty() && (working == 0)) 50 | return; 51 | done_cv.wait(lock, [&]() { return queue.empty() && (working == 0); }); 52 | } 53 | size_t size() const 54 | { 55 | return threads.size(); 56 | } 57 | ~Pool() 58 | { 59 | //Close the queue and finish all the remaining work 60 | std::unique_lock lock(m); 61 | closed = true; 62 | work_cv.notify_all(); 63 | lock.unlock(); 64 | 65 | for (auto& t : threads) 66 | if (t.joinable()) 67 | t.join(); 68 | } 69 | 70 | template 71 | static void script(ChaiScript& c, const std::string& name) 72 | { 73 | using namespace chaiscript; 74 | using T = Pool; 75 | c.add(constructor(), name); 76 | c.add(fun(&T::computing), "computing"); 77 | c.add(fun(&T::n_threads), "n_threads"); 78 | c.add(fun(&T::wait), "wait"); 79 | c.add(fun(&T::emplace_back), "emplace_back"); 80 | c.add(fun(&T::size), "size"); 81 | } 82 | 83 | private: 84 | std::vector< std::thread > threads; 85 | std::deque< std::function > queue; 86 | std::atomic working = 0; 87 | bool closed = false; 88 | std::mutex m; 89 | std::condition_variable work_cv; 90 | std::condition_variable done_cv; 91 | 92 | void worker() 93 | { 94 | while (true) 95 | { 96 | //Wait for work 97 | std::unique_lock lock(m); 98 | work_cv.wait(lock, [this]() { return closed || !queue.empty(); }); 99 | if (queue.empty()) { 100 | if (closed) { 101 | return; 102 | } 103 | continue; 104 | } 105 | 106 | //Grab work 107 | ++working; 108 | auto work = queue.front(); 109 | queue.pop_front(); 110 | lock.unlock(); 111 | 112 | work(); 113 | 114 | //Notify that work is finished 115 | lock.lock(); 116 | --working; 117 | done_cv.notify_all(); 118 | } 119 | } 120 | }; 121 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/threading/Queue.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2018 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // A threaded worker queue 29 | 30 | namespace asc 31 | { 32 | struct Queue final 33 | { 34 | Queue() = default; 35 | Queue(const Queue&) = default; 36 | Queue(Queue&&) = default; 37 | Queue& operator=(const Queue&) = default; 38 | Queue& operator=(Queue&&) = default; 39 | ~Queue() 40 | { 41 | run = false; 42 | if (computing()) 43 | { 44 | std::cerr << "Queue destroyed while work was processing\n"; 45 | } 46 | } 47 | 48 | bool computing() const noexcept 49 | { 50 | return running_jobs; 51 | } 52 | 53 | template 54 | void emplace_back(Args&&... args) 55 | { 56 | adding = true; 57 | jobs.emplace_back(std::forward(args)...); 58 | adding = false; 59 | if (!started) 60 | { 61 | start(); 62 | started = true; 63 | } 64 | cv.notify_one(); 65 | } 66 | 67 | size_t size() const 68 | { 69 | return jobs.size(); 70 | } 71 | 72 | template 73 | static void script(ChaiScript& c, const std::string& name) 74 | { 75 | using namespace chaiscript; 76 | using T = Queue; 77 | c.add(constructor(), name); 78 | 79 | c.add(fun(&T::wait), "wait"); 80 | c.add(fun(&T::computing), "computing"); 81 | c.add(fun(&T::emplace_back>), "emplace_back"); 82 | c.add(fun(&T::size), "size"); 83 | } 84 | 85 | void wait() 86 | { 87 | std::unique_lock lock(mtx_wait); 88 | work_done.wait(lock); 89 | } 90 | 91 | std::deque> jobs; 92 | std::atomic adding{}; 93 | 94 | private: 95 | void start() 96 | { 97 | run = true; 98 | 99 | auto runner = [&] 100 | { 101 | while (run) 102 | { 103 | running_jobs = true; 104 | while (jobs.size()) 105 | { 106 | jobs.front()(); 107 | 108 | while (adding) 109 | { 110 | std::this_thread::yield(); 111 | } 112 | jobs.pop_front(); 113 | } 114 | running_jobs = false; 115 | 116 | if (jobs.empty()) 117 | { 118 | work_done.notify_one(); 119 | std::unique_lock lock(mtx_work); 120 | cv.wait(lock); 121 | } 122 | } 123 | }; 124 | 125 | std::thread(runner).detach(); 126 | } 127 | 128 | std::mutex mtx_work, mtx_wait; 129 | bool started{}; 130 | std::atomic run{}, running_jobs{}; 131 | std::condition_variable cv, work_done; 132 | }; 133 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/timing/Sampler.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace asc 20 | { 21 | // A Sampler resets the base time step when it goes out of scope. In this manner it can be used as a short-lived object within the simulation loop. 22 | // If the Sampler is to exist long term, then the reset() must be called after time is incremented within the simulation loop. 23 | // Note that keeping the Sampler around longer maintains a consistent base time step. 24 | template 25 | struct SamplerT 26 | { 27 | SamplerT(T& t, T& dt) noexcept : t(t), dt(dt), dt_base(dt) {} 28 | SamplerT(const SamplerT& other) : t(other.t), dt(other.dt), dt_base(other.dt_base) {} 29 | SamplerT(SamplerT&&) = default; 30 | SamplerT& operator=(const SamplerT& other) 31 | { 32 | t = other.t; 33 | dt = other.dt; 34 | dt_base = other.dt_base; 35 | return *this; 36 | } 37 | SamplerT& operator=(SamplerT&&) = default; 38 | ~SamplerT() noexcept { dt = dt_base; } 39 | 40 | bool operator()(const T sample_rate) const noexcept 41 | { 42 | const size_t n = static_cast((t + eps) / sample_rate); // the number of sample time steps that have occured 43 | const T sample_time = (n + 1) * sample_rate; // the next sample time 44 | if (sample_time < t + dt - eps) 45 | dt = sample_time - t; 46 | 47 | if (t - sample_time + sample_rate < eps) 48 | return true; 49 | 50 | return false; 51 | } 52 | 53 | bool event(const T event_time) noexcept 54 | { 55 | if (event_time < t + dt - eps && event_time >= t + eps) 56 | dt = event_time - t; 57 | 58 | if (std::abs(event_time - t) < eps) 59 | return true; 60 | 61 | return false; 62 | } 63 | 64 | void reset() noexcept 65 | { 66 | dt = dt_base; 67 | } 68 | 69 | T base_time_step() const noexcept 70 | { 71 | return dt_base; 72 | } 73 | 74 | void base_time_step(const double dt_new) noexcept 75 | { 76 | dt = dt_base = dt_new; 77 | } 78 | 79 | private: 80 | static constexpr T eps = static_cast(1.0e-8); 81 | T& t; 82 | T& dt; 83 | T dt_base; 84 | }; 85 | } 86 | -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/timing/TimeAdvanced.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2017 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace asc 18 | { 19 | // TimeAdvanced must be called at the end of the update sequence, otherwise there will be no difference between the current time and the previous time 20 | template 21 | struct TimeAdvancedT 22 | { 23 | value_t t_previous{}; 24 | value_t eps = static_cast(1.0e-8); 25 | 26 | bool operator()(const value_t t) const noexcept 27 | { 28 | if (t > t_previous + eps) 29 | return true; 30 | return false; 31 | } 32 | 33 | double delta_t(const value_t t) const noexcept 34 | { 35 | return t - t_previous; 36 | } 37 | 38 | void update(const value_t t) noexcept 39 | { 40 | if (operator()(t)) 41 | t_previous = t; 42 | } 43 | 44 | template 45 | void operator()(state_t&, state_t&, const value_t t) noexcept 46 | { 47 | if (operator()(t)) 48 | t_previous = t; 49 | } 50 | }; 51 | } -------------------------------------------------------------------------------- /hunter_base/ascent/include/ascent/timing/Timing.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2019 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include "ascent/timing/Sampler.h" 19 | #include "ascent/modular/Module.h" 20 | 21 | namespace asc 22 | { 23 | template 24 | struct Timing : Module 25 | { 26 | value_t t{}; 27 | value_t dt{ 0.01 }; 28 | value_t t_end = std::numeric_limits::max(); 29 | value_t t_delta{}; 30 | 31 | value_t t_previous{}; 32 | value_t eps = static_cast(1.0e-8); 33 | bool time_advanced = false; 34 | 35 | SamplerT sampler{ t, dt }; 36 | 37 | bool sample(const value_t sample_rate) noexcept 38 | { 39 | return sampler(sample_rate); 40 | } 41 | 42 | bool event(const value_t event_time) noexcept 43 | { 44 | return sampler.event(event_time); 45 | } 46 | 47 | void base_time_step(const value_t base_dt) 48 | { 49 | sampler.base_time_step(base_dt); 50 | } 51 | 52 | value_t base_time_step() const noexcept 53 | { 54 | return sampler.base_time_step(); 55 | } 56 | 57 | value_t delta_t() const noexcept 58 | { 59 | return t_delta; 60 | } 61 | 62 | void reset() noexcept 63 | { 64 | sampler.reset(); 65 | } 66 | 67 | void init() override 68 | { 69 | t_previous = t; 70 | } 71 | 72 | void operator()() override 73 | { 74 | if (t > t_previous + eps) 75 | { 76 | time_advanced = true; 77 | } 78 | else 79 | { 80 | time_advanced = false; 81 | } 82 | t_delta = t - t_previous; 83 | t_previous = t; 84 | } 85 | }; 86 | } -------------------------------------------------------------------------------- /hunter_base/ascent/unit_tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(${PROJECT_NAME}_test src/main.cpp) 2 | target_link_libraries(${PROJECT_NAME}_test ascent) 3 | add_test(NAME ${PROJECT_NAME}_test COMMAND ${PROJECT_NAME}_test) -------------------------------------------------------------------------------- /hunter_base/ascent/unit_tests/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016-2020 Anyar, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "ascent/Ascent.h" 16 | 17 | #include "ascent/integrators_modular/RK2.h" 18 | #include "ascent/integrators_modular/RK4.h" 19 | #include "ascent/integrators_modular/PC233.h" 20 | #include "ascent/timing/Timing.h" 21 | 22 | using namespace asc; 23 | 24 | struct Airy 25 | { 26 | void operator()(const state_t& x, state_t& xd, const double t) 27 | { 28 | xd[0] = x[1]; 29 | xd[1] = -t*x[0]; 30 | } 31 | }; 32 | 33 | struct AiryMod : asc::Module 34 | { 35 | double a{}; 36 | double a_d{}; 37 | double b{}; 38 | double b_d{}; 39 | std::shared_ptr> sim{}; 40 | 41 | void init() 42 | { 43 | make_state(a, a_d); 44 | make_state(b, b_d); 45 | } 46 | 47 | void operator()() 48 | { 49 | a_d = b; 50 | b_d = -sim->t*a; 51 | } 52 | }; 53 | 54 | struct Exponential 55 | { 56 | void operator()(const state_t& x, state_t& xd, const double t) 57 | { 58 | xd[0] = x[0]; 59 | } 60 | }; 61 | struct ExponentialMod : asc::Module 62 | { 63 | double value{}; 64 | double deriv{}; 65 | 66 | void init() 67 | { 68 | make_state(value, deriv); 69 | } 70 | void operator()() 71 | { 72 | deriv = value; 73 | } 74 | }; 75 | 76 | template 77 | state_t airy_test(const double dt) 78 | { 79 | state_t x = { 1.0, 0.0 }; 80 | double t = 0.0; 81 | double t_end = 10.0; 82 | 83 | Integrator integrator; 84 | Airy system; 85 | 86 | while (t < t_end) 87 | { 88 | integrator(system, x, t, dt); 89 | } 90 | 91 | return x; 92 | } 93 | 94 | template 95 | std::vector airy_test_mod(const double dt) 96 | { 97 | Integrator integrator; 98 | auto system = std::make_shared(); 99 | system->sim = std::make_shared>(); 100 | system->a = 1.0; 101 | system->b = 0.0; 102 | system->sim->t = 0.0; 103 | system->sim->t_end = 10.0; 104 | std::vector blocks{}; 105 | blocks.emplace_back(system.get()); 106 | 107 | system->init(); 108 | 109 | while (system->sim->t < system->sim->t_end) 110 | { 111 | integrator(blocks, system->sim->t, dt); 112 | } 113 | 114 | return{ system->a, system->b }; 115 | } 116 | 117 | 118 | template 119 | std::pair exponential_test(const double dt) 120 | { 121 | state_t x = { 1.0 }; 122 | double t = 0.0; 123 | double t_end = 10.0; 124 | 125 | Integrator integrator; 126 | Exponential system; 127 | 128 | while (t < t_end) 129 | { 130 | integrator(system, x, t, dt); 131 | } 132 | 133 | return{ x, t }; 134 | } 135 | 136 | template 137 | std::pair exponential_test_mod(const double dt) 138 | { 139 | Integrator integrator; 140 | auto system = std::make_shared(); 141 | auto sim = std::make_shared>(); 142 | system->value = 1.0; 143 | sim->t = 0.0; 144 | sim->t_end = 10.0; 145 | std::vector blocks{}; 146 | blocks.emplace_back(system.get()); 147 | 148 | system->init(); 149 | 150 | while (sim->t < sim->t_end) 151 | { 152 | integrator(blocks, sim->t, dt); 153 | } 154 | 155 | return{ system->value, sim->t }; 156 | } 157 | 158 | #define CATCH_CONFIG_MAIN // This tells Catch to provide a main() - only do this in one cpp file 159 | #include "catch.hpp" 160 | 161 | // bool test(const std::string& title, const double x, const double target, const double eps) 162 | 163 | TEST_CASE("Airy System RK4", "[airy]") { 164 | auto x = airy_test(0.001); 165 | REQUIRE(x[0] == Approx(-0.200693641142).epsilon(1.0e-8)); 166 | REQUIRE(x[1] == Approx(-1.49817601143).epsilon(1.0e-8)); 167 | } 168 | 169 | TEST_CASE("Airy System RK2", "[airy]") { 170 | auto x = airy_test(0.001); 171 | REQUIRE(x[0] == Approx(-0.200703911717).epsilon(1.0e-8)); 172 | REQUIRE(x[1] == Approx(-1.49816435475).epsilon(1.0e-8)); 173 | 174 | x = airy_test(0.001); 175 | REQUIRE(x[0] == Approx(-0.200693641142).epsilon(1.0e-8)); 176 | REQUIRE(x[1] == Approx(-1.49817601143).epsilon(1.0e-8)); 177 | } 178 | 179 | TEST_CASE("Airy System DOPRI45", "[airy]") { 180 | auto x = airy_test(0.001); 181 | REQUIRE(x[0] == Approx(-0.200693641142).epsilon(1.0e-8)); 182 | REQUIRE(x[1] == Approx(-1.49817601143).epsilon(1.0e-8)); 183 | } 184 | 185 | TEST_CASE("Modular Airy System RK4", "[airy][modular]") { 186 | auto x = airy_test_mod>(0.001); 187 | REQUIRE(x[0] == Approx(-0.200693641142).epsilon(1.0e-8)); 188 | REQUIRE(x[1] == Approx(-1.49817601143).epsilon(1.0e-8)); 189 | } 190 | 191 | TEST_CASE("Modular Airy System RK2", "[airy][modular]") { 192 | auto x = airy_test_mod>(0.001); 193 | REQUIRE(x[0] == Approx(-0.200703911717).epsilon(1.0e-8)); 194 | REQUIRE(x[1] == Approx(-1.49816435475).epsilon(1.0e-8)); 195 | } 196 | 197 | TEST_CASE("Modular Airy System PC233", "[airy][modular]") { 198 | auto x = airy_test_mod>(0.001); 199 | REQUIRE(x[0] == Approx(-0.200693641142).epsilon(1.0e-8)); 200 | REQUIRE(x[1] == Approx(-1.49817601143).epsilon(1.0e-8)); 201 | } 202 | 203 | TEST_CASE("Exponential RK4", "[exponential]") { 204 | auto result = exponential_test(0.001); 205 | REQUIRE(result.first[0] == Approx(exp(result.second)).epsilon(1.0e-8)); 206 | } 207 | 208 | TEST_CASE("Exponential RK2", "[exponential]") { 209 | auto result = exponential_test(0.001); 210 | REQUIRE(result.first[0] == Approx(exp(result.second)).epsilon(1.0e-5)); 211 | } 212 | 213 | TEST_CASE("Exponential DOPRI45", "[exponential]") { 214 | auto result = exponential_test(0.001); 215 | REQUIRE(result.first[0] == Approx(exp(result.second)).epsilon(1.0e-8)); 216 | } 217 | 218 | TEST_CASE("Exponential PC233", "[exponential]") { 219 | auto result = exponential_test(0.001); 220 | REQUIRE(result.first[0] == Approx(exp(result.second)).epsilon(1.0e-6)); 221 | } 222 | 223 | TEST_CASE("Exponential Modular RK4", "[exponential][modular]") { 224 | auto result = exponential_test_mod>(0.001); 225 | REQUIRE(result.first == Approx(exp(result.second)).epsilon(1.0e-8)); 226 | } 227 | 228 | TEST_CASE("Exponential Modular RK2", "[exponential][modular]") { 229 | auto result = exponential_test_mod>(0.001); 230 | REQUIRE(result.first == Approx(exp(result.second)).epsilon(1.0e-5)); 231 | } 232 | 233 | TEST_CASE("Exponential Modular PC233", "[exponential][modular]") { 234 | auto result = exponential_test_mod>(0.001); 235 | REQUIRE(result.first == Approx(exp(result.second)).epsilon(1.0e-6)); 236 | } -------------------------------------------------------------------------------- /hunter_base/include/hunter_base/bicycle_model.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * bicycle_model.hpp 3 | * 4 | * Created on: Mar 20, 2018 17:18 5 | * Description: 6 | * 7 | * Reference: 8 | * [1] Paden, Brian, Michal Cap, Sze Zheng Yong, Dmitry Yershov, and Emilio 9 | * Frazzoli. 2016. “A Survey of Motion Planning and Control Techniques for 10 | * Self-Driving Urban Vehicles.” arXiv [cs.RO]. arXiv. 11 | * http://arxiv.org/abs/1604.07446. 12 | * 13 | * Copyright (c) 2018 Ruixiang Du (rdu) 14 | */ 15 | 16 | #ifndef BICYCLE_MODEL_HPP 17 | #define BICYCLE_MODEL_HPP 18 | 19 | #include "ascent/Ascent.h" 20 | #include "ascent/Utility.h" 21 | #include "hunter_base/hunter_params.hpp" 22 | 23 | namespace westonrobot { 24 | /* 25 | * Bicycle kinematics model (rear wheel): 26 | * dot_x = v(t) * cos(theta(t)) 27 | * dot_y = v(t) * sin(theta(t)) 28 | * dot_theta = v(t)/L * tan(delta(t)) 29 | * State: (x, y, theta) 30 | * Control input: (v, delta) - velocity, steering angle of front wheel 31 | */ 32 | class BicycleKinematics { 33 | public: 34 | struct CtrlInput { 35 | CtrlInput(double vel = 0.0, double ster = 0.0) : v(vel), delta(ster) {} 36 | 37 | double v; 38 | double delta; 39 | }; 40 | 41 | using control_t = CtrlInput; 42 | 43 | ///////////////////////////////////// 44 | 45 | BicycleKinematics(control_t u); 46 | 47 | // x1 = x, x2 = y, x3 = theta 48 | void operator()(const asc::state_t &x, asc::state_t &xd, const double); 49 | 50 | private: 51 | control_t u_ = {0.0, 0.0}; 52 | static constexpr double L = HunterV2Params::wheelbase; 53 | }; 54 | } // namespace westonrobot 55 | 56 | #endif /* BICYCLE_MODEL_HPP */ 57 | -------------------------------------------------------------------------------- /hunter_base/include/hunter_base/hunter_messenger.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * hunter_messenger.hpp 3 | * 4 | * Created on: Jun 01, 2020 15:18 5 | * Description: 6 | * 7 | * Copyright (c) 2019 Ruixiang Du (rdu) 8 | */ 9 | 10 | #ifndef HUNTER_MESSENGER_HPP 11 | #define HUNTER_MESSENGER_HPP 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "ugv_sdk/mobile_robot/hunter_robot.hpp" 22 | #include "ugv_sdk/utilities/protocol_detector.hpp" 23 | #include 24 | #include "ascent/Ascent.h" 25 | #include "ascent/Utility.h" 26 | #include "hunter_base/bicycle_model.hpp" 27 | #include "hunter_base/hunter_params.hpp" 28 | 29 | 30 | namespace westonrobot { 31 | template 32 | class SystemPropagator { 33 | public: 34 | asc::state_t Propagate(asc::state_t init_state, 35 | typename SystemModel::control_t u, double t0, 36 | double tf, double dt) { 37 | double t = t0; 38 | asc::state_t x = init_state; 39 | 40 | while (t <= tf) { 41 | integrator_(SystemModel(u), x, t, dt); 42 | // Note: you may need to add additional constraints to [x] 43 | } 44 | 45 | return x; 46 | } 47 | 48 | private: 49 | asc::RK4 integrator_; 50 | }; 51 | 52 | class HunterROSMessenger { 53 | public: 54 | explicit HunterROSMessenger(ros::NodeHandle *nh); 55 | HunterROSMessenger(HunterRobot *hunter, ros::NodeHandle *nh); 56 | 57 | std::string odom_frame_; 58 | std::string base_frame_; 59 | 60 | bool simulated_robot_ = false; 61 | int sim_control_rate_ = 50; 62 | bool publish_tf_ = true; 63 | int version = 2; 64 | 65 | void SetupSubscription(); 66 | void ResetOdometry(); 67 | 68 | void PublishStateToROS(); 69 | void PublishSimStateToROS(double linear, double angular); 70 | 71 | void GetCurrentMotionCmdForSim(double &linear, double &angular); 72 | void SetWeelbase(float Weelbase){ 73 | l = Weelbase; 74 | } 75 | void SetTrack(float Track){ 76 | w = Track; 77 | } 78 | void SetMaxSteerAngleCentral(float Angle){ 79 | max_steer_angle_central = Angle; 80 | } 81 | void SetMaxSteerAngle(float Angle){ 82 | max_steer_angle = Angle; 83 | } 84 | private: 85 | HunterRobot *hunter_; 86 | ros::NodeHandle *nh_; 87 | 88 | std::mutex twist_mutex_; 89 | geometry_msgs::Twist current_twist_; 90 | 91 | ros::Publisher odom_publisher_; 92 | ros::Publisher status_publisher_; 93 | ros::Publisher BMS_status_publisher_; 94 | ros::Subscriber motion_cmd_subscriber_; 95 | ros::Subscriber integrator_reset_subscriber_; 96 | tf2_ros::TransformBroadcaster tf_broadcaster_; 97 | 98 | // control inputs 99 | double linear_speed_ = 0.0; 100 | double steering_angle_ = 0.0; 101 | 102 | // static constexpr double l = HunterV2Params::wheelbase; 103 | // static constexpr double w = HunterV2Params::track; 104 | static constexpr double steer_angle_tolerance = 0.005; // ~+-0.287 degrees 105 | double l = 0.0; 106 | double w = 0.0; 107 | double max_steer_angle_central = 0.0; 108 | double max_steer_angle = 0.0; 109 | // state variables 110 | double position_x_ = 0.0; 111 | double position_y_ = 0.0; 112 | double theta_ = 0.0; 113 | 114 | SystemPropagator model_; 115 | 116 | ros::Time last_time_; 117 | ros::Time current_time_; 118 | 119 | double ConvertInnerAngleToCentral(double angle); 120 | double ConvertCentralAngleToInner(double angle); 121 | double AngelVelocity2Angel(geometry_msgs::Twist msg,double &radius); 122 | 123 | void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg); 124 | void ResetOdomIntegratorCallback(const std_msgs::Bool::ConstPtr &msg); 125 | void PublishOdometryToROS(double linear, double angular, double dt); 126 | }; 127 | } // namespace westonrobot 128 | 129 | #endif /* HUNTER_MESSENGER_HPP */ 130 | -------------------------------------------------------------------------------- /hunter_base/include/hunter_base/hunter_params.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * hunter_params.hpp 3 | * 4 | * Created on: Jun 11, 2020 17:23 5 | * Description: 6 | * 7 | * Copyright (c) 2019 Ruixiang Du (rdu) 8 | */ 9 | 10 | #ifndef HUNTER_PARAMS_HPP 11 | #define HUNTER_PARAMS_HPP 12 | 13 | #include 14 | 15 | namespace westonrobot { 16 | /* hunter Parameters */ 17 | struct HunterV2Params { 18 | static constexpr double track = 19 | 0.605; // in meter (left & right wheel distance) 20 | static constexpr double wheelbase = 21 | 0.650; // in meter (front & rear wheel distance) 22 | static constexpr double wheel_radius = 0.165; // in meter 23 | static constexpr double transmission_reduction_rate = 30; // 1:30 24 | 25 | // from user manual v1.2.6_S P4 26 | // max linear velocity: 1.5 m/s 27 | static constexpr double max_steer_angle = 28 | 0.58; // in rad, 0.75 for inner wheel 29 | static constexpr double max_steer_angle_central = 30 | 0.461; // max central angle 31 | static constexpr double max_linear_speed = 1.5; // in m/ss 32 | }; 33 | 34 | struct HunterV1Params { 35 | static constexpr double track = 36 | 0.578; // in meter (left & right wheel distance) 37 | static constexpr double wheelbase = 38 | 0.650; // in meter (front & rear wheel distance) 39 | static constexpr double wheel_radius = 0.165; // in meter 40 | static constexpr double transmission_reduction_rate = 30; // 1:30 41 | 42 | // from user manual v1.2.6_S P4 43 | // max linear velocity: 1.5 m/s 44 | static constexpr double max_steer_angle = 45 | 0.444; // in rad, 0.75 for inner wheel 46 | static constexpr double max_steer_angle_central = 47 | 0.374; // max central angle 48 | static constexpr double max_linear_speed = 1.5; // in m/ss 49 | }; 50 | } // namespace westonrobot 51 | 52 | #endif /* HUNTER_PARAMS_HPP */ 53 | -------------------------------------------------------------------------------- /hunter_base/launch/hunter_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /hunter_base/launch/hunter_base_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /hunter_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hunter_base 4 | 0.0.0 5 | The hunter_base package 6 | 7 | 8 | 9 | 10 | rdu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | hunter_msgs 53 | roscpp 54 | sensor_msgs 55 | std_msgs 56 | tf2 57 | tf2_ros 58 | ugv_sdk 59 | hunter_msgs 60 | roscpp 61 | sensor_msgs 62 | std_msgs 63 | tf2 64 | tf2_ros 65 | ugv_sdk 66 | hunter_msgs 67 | roscpp 68 | sensor_msgs 69 | std_msgs 70 | tf2 71 | tf2_ros 72 | ugv_sdk 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /hunter_base/src/bicycle_model.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * bicycle_model.cpp 3 | * 4 | * Created on: Mar 20, 2018 17:33 5 | * Description: 6 | * 7 | * Copyright (c) 2018 Ruixiang Du (rdu) 8 | */ 9 | 10 | #include "hunter_base/bicycle_model.hpp" 11 | 12 | #include 13 | #include 14 | 15 | namespace westonrobot { 16 | 17 | BicycleKinematics::BicycleKinematics(control_t u) : u_(u) {} 18 | 19 | // x1 = x, x2 = y, x4 = theta 20 | void BicycleKinematics::operator()(const asc::state_t &x, asc::state_t &xd, 21 | const double) { 22 | xd[0] = u_.v * std::cos(x[2]); 23 | xd[1] = u_.v * std::sin(x[2]); 24 | xd[2] = u_.v / L * std::tan(u_.delta); 25 | } 26 | } // namespace westonrobot -------------------------------------------------------------------------------- /hunter_base/src/hunter_base_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Author: your name 3 | * @Date: 2021-08-26 15:23:22 4 | * @LastEditTime: 2021-08-27 10:25:35 5 | * @LastEditors: your name 6 | * @Description: In User Settings Edit 7 | * @FilePath: /hunter_ros/hunter_base/src/hunter_base_node.cpp 8 | */ 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "hunter_base/hunter_messenger.hpp" 17 | 18 | using namespace westonrobot; 19 | std::unique_ptr robot; 20 | 21 | int main(int argc, char **argv) { 22 | // setup ROS node 23 | ros::init(argc, argv, "hunter_base"); 24 | ros::NodeHandle node(""), private_node("~"); 25 | 26 | // fetch parameters before connecting to robot 27 | std::string port_name; 28 | private_node.param("port_name", port_name, std::string("can0")); 29 | int version = 2; 30 | // check protocol version 31 | ProtocolDetector detector; 32 | try 33 | { 34 | detector.Connect(port_name); 35 | auto proto = detector.DetectProtocolVersion(5); 36 | if (proto == ProtocolVersion::AGX_V1) { 37 | std::cout << "Detected protocol: AGX_V1" << std::endl; 38 | robot = std::unique_ptr( 39 | new HunterRobot(ProtocolVersion::AGX_V1)); 40 | version = 1; 41 | } else if (proto == ProtocolVersion::AGX_V2) { 42 | std::cout << "Detected protocol: AGX_V2" << std::endl; 43 | robot = std::unique_ptr( 44 | new HunterRobot(ProtocolVersion::AGX_V2)); 45 | } else { 46 | std::cout << "Detected protocol: UNKONWN" << std::endl; 47 | return -1; 48 | } 49 | if (robot == nullptr) 50 | std::cout << "Failed to create robot object" << std::endl; 51 | } 52 | catch (std::exception error) 53 | { 54 | ROS_ERROR("please bringup up can or make sure can port exist"); 55 | ros::shutdown(); 56 | } 57 | 58 | 59 | // instantiate a robot object 60 | HunterROSMessenger messenger(robot.get(), &node); 61 | if(version == 1) 62 | { 63 | messenger.SetTrack(HunterV1Params::track); 64 | messenger.SetWeelbase(HunterV1Params::wheelbase); 65 | messenger.SetMaxSteerAngleCentral(HunterV1Params::max_steer_angle_central); 66 | messenger.SetMaxSteerAngle(HunterV1Params::max_steer_angle); 67 | } 68 | else 69 | { 70 | messenger.SetTrack(HunterV2Params::track); 71 | messenger.SetWeelbase(HunterV2Params::wheelbase); 72 | messenger.SetMaxSteerAngleCentral(HunterV2Params::max_steer_angle_central); 73 | messenger.SetMaxSteerAngle(HunterV2Params::max_steer_angle); 74 | } 75 | 76 | // fetch parameters before connecting to robot 77 | private_node.param("odom_frame", messenger.odom_frame_, 78 | std::string("odom")); 79 | private_node.param("base_frame", messenger.base_frame_, 80 | std::string("base_link")); 81 | private_node.param("simulated_robot", messenger.simulated_robot_, 82 | false); 83 | private_node.param("control_rate", messenger.sim_control_rate_, 50); 84 | private_node.param("publish_tf", messenger.publish_tf_, true); 85 | 86 | 87 | // connect to robot and setup ROS subscription 88 | if (port_name.find("can") != std::string::npos) { 89 | robot->Connect(port_name); 90 | robot->EnableCommandedMode(); 91 | ROS_INFO("Using CAN bus to talk with the robot: %s", port_name.c_str()); 92 | } else { 93 | ROS_ERROR("Only CAN bus interface is supported for now"); 94 | return -1; 95 | } 96 | 97 | messenger.SetupSubscription(); 98 | // publish robot state at 50Hz while listening to twist commands 99 | ros::Rate rate_50hz(50); // 50Hz 100 | //int cnt = 0; 101 | while (ros::ok()) { 102 | messenger.PublishStateToROS(); 103 | ros::spinOnce(); 104 | rate_50hz.sleep(); 105 | // if(++cnt == 390) break; 106 | } 107 | 108 | return 0; 109 | } 110 | -------------------------------------------------------------------------------- /hunter_base/src/hunter_base_sim_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "hunter_base/hunter_messenger.hpp" 9 | 10 | using namespace westonrobot; 11 | 12 | std::unique_ptr robot; 13 | 14 | void DetachRobot(int signal) { 15 | robot.Disconnect(); 16 | robot.Terminate(); 17 | } 18 | 19 | int main(int argc, char **argv) { 20 | // setup ROS node 21 | ros::init(argc, argv, "hunter_odom"); 22 | ros::NodeHandle node(""), private_node("~"); 23 | 24 | std::signal(SIGINT, DetachRobot); 25 | 26 | // instantiate a robot object 27 | HunterBase robot; 28 | HunterROSMessenger messenger(&robot, &node); 29 | 30 | // fetch parameters before connecting to robot 31 | std::string port_name; 32 | private_node.param("port_name", port_name, std::string("can0")); 33 | private_node.param("odom_frame", messenger.odom_frame_, 34 | std::string("odom")); 35 | private_node.param("base_frame", messenger.base_frame_, 36 | std::string("base_link")); 37 | private_node.param("simulated_robot", messenger.simulated_robot_, true); 38 | private_node.param("control_rate", messenger.sim_control_rate_, 50); 39 | private_node.param("publish_tf", messenger.publish_tf_, true); 40 | 41 | // no connection for simulated robot 42 | // setup ROS subscription 43 | messenger.SetupSubscription(); 44 | 45 | // publish robot state at 50Hz while listening to twist commands 46 | double linear, angular; 47 | ros::Rate rate_50hz(50); // 50Hz 48 | while (ros::ok()) { 49 | messenger.GetCurrentMotionCmdForSim(linear, angular); 50 | messenger.PublishSimStateToROS(linear, angular); 51 | ros::spinOnce(); 52 | rate_50hz.sleep(); 53 | } 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /hunter_base/src/hunter_status_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * demo_hunter_can.cpp 3 | * 4 | * Created on: Jun 12, 2019 05:03 5 | * Description: 6 | * 7 | * Copyright (c) 2019 Ruixiang Du (rdu) 8 | */ 9 | 10 | #include "ugv_sdk/hunter/hunter_base.hpp" 11 | 12 | using namespace westonrobot; 13 | 14 | int main(int argc, char **argv) { 15 | HunterBase hunter; 16 | hunter.Connect("can0"); 17 | 18 | int count = 0; 19 | while (true) { 20 | auto state = hunter.GetHunterState(); 21 | std::cout << "-------------------------------" << std::endl; 22 | std::cout << "count: " << count << std::endl; 23 | std::cout << "control mode: " << static_cast(state.control_mode) 24 | << " , base state: " << static_cast(state.base_state) 25 | << std::endl; 26 | std::cout << "parking mode: " << static_cast(state.park_mode) 27 | << std::endl; 28 | std::cout << "battery voltage: " << state.battery_voltage << std::endl; 29 | std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " 30 | << state.steering_angle << std::endl; 31 | for (int i = 0; i < 3; ++i) 32 | std::cout << "motor rpm " << i << ": " << state.actuator_states[i].motor_rpm 33 | << std::endl; 34 | std::cout << "-------------------------------" << std::endl; 35 | 36 | sleep(1); 37 | ++count; 38 | } 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /hunter_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hunter_bringup) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES hunter_bringup 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/hunter_bringup.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/hunter_bringup_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | install(DIRECTORY launch scripts 158 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 159 | 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # install(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables and/or libraries for installation 172 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 173 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark cpp header files for installation 179 | # install(DIRECTORY include/${PROJECT_NAME}/ 180 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 181 | # FILES_MATCHING PATTERN "*.h" 182 | # PATTERN ".svn" EXCLUDE 183 | # ) 184 | 185 | ## Mark other files for installation (e.g. launch and bag files, etc.) 186 | # install(FILES 187 | # # myfile1 188 | # # myfile2 189 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 190 | # ) 191 | 192 | ############# 193 | ## Testing ## 194 | ############# 195 | 196 | ## Add gtest based cpp test target and link libraries 197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_hunter_bringup.cpp) 198 | # if(TARGET ${PROJECT_NAME}-test) 199 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 200 | # endif() 201 | 202 | ## Add folders to be run by python nosetests 203 | # catkin_add_nosetests(test) 204 | -------------------------------------------------------------------------------- /hunter_bringup/launch/hunter_robot_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /hunter_bringup/launch/hunter_teleop_keyboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /hunter_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hunter_bringup 4 | 0.0.0 5 | The hunter_bringup package 6 | 7 | 8 | 9 | 10 | Ruixiang Du 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /hunter_bringup/scripts/bringup_can2usb.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # bring up can interface 4 | sudo ip link set can0 up type can bitrate 500000 -------------------------------------------------------------------------------- /hunter_bringup/scripts/setup_can2usb.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # enable kernel module: gs_usb 4 | sudo modprobe gs_usb 5 | 6 | # bring up can interface 7 | sudo ip link set can0 up type can bitrate 500000 8 | 9 | # install can utils 10 | sudo apt install -y can-utils -------------------------------------------------------------------------------- /hunter_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hunter_msgs) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | message_generation 12 | std_msgs 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | add_message_files( 50 | FILES 51 | HunterMotorState.msg 52 | HunterStatus.msg 53 | HunterDriverState.msg 54 | HunterBmsStatus.msg 55 | ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | generate_messages( 73 | DEPENDENCIES 74 | std_msgs 75 | ) 76 | 77 | catkin_package(CATKIN_DEPENDS std_msgs message_runtime) 78 | 79 | ################################################ 80 | ## Declare ROS dynamic reconfigure parameters ## 81 | ################################################ 82 | 83 | ## To declare and build dynamic reconfigure parameters within this 84 | ## package, follow these steps: 85 | ## * In the file package.xml: 86 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 87 | ## * In this file (CMakeLists.txt): 88 | ## * add "dynamic_reconfigure" to 89 | ## find_package(catkin REQUIRED COMPONENTS ...) 90 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 91 | ## and list every .cfg file to be processed 92 | 93 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 94 | # generate_dynamic_reconfigure_options( 95 | # cfg/DynReconf1.cfg 96 | # cfg/DynReconf2.cfg 97 | # ) 98 | 99 | ################################### 100 | ## catkin specific configuration ## 101 | ################################### 102 | ## The catkin_package macro generates cmake config files for your package 103 | ## Declare things to be passed to dependent projects 104 | ## INCLUDE_DIRS: uncomment this if your package contains header files 105 | ## LIBRARIES: libraries you create in this project that dependent projects also need 106 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 107 | ## DEPENDS: system dependencies of this project that dependent projects also need 108 | catkin_package( 109 | # INCLUDE_DIRS include 110 | # LIBRARIES hunter_msgs 111 | # CATKIN_DEPENDS message_generation std_msgs 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | include_directories( 122 | # include 123 | ${catkin_INCLUDE_DIRS} 124 | ) 125 | 126 | ## Declare a C++ library 127 | # add_library(${PROJECT_NAME} 128 | # src/${PROJECT_NAME}/hunter_msgs.cpp 129 | # ) 130 | 131 | ## Add cmake target dependencies of the library 132 | ## as an example, code may need to be generated before libraries 133 | ## either from message generation or dynamic reconfigure 134 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Declare a C++ executable 137 | ## With catkin_make all packages are built within a single CMake context 138 | ## The recommended prefix ensures that target names across packages don't collide 139 | # add_executable(${PROJECT_NAME}_node src/hunter_msgs_node.cpp) 140 | 141 | ## Rename C++ executable without prefix 142 | ## The above recommended prefix causes long target names, the following renames the 143 | ## target back to the shorter version for ease of user use 144 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 145 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 146 | 147 | ## Add cmake target dependencies of the executable 148 | ## same as for the library above 149 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 150 | 151 | ## Specify libraries to link a library or executable target against 152 | # target_link_libraries(${PROJECT_NAME}_node 153 | # ${catkin_LIBRARIES} 154 | # ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # catkin_install_python(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_hunter_msgs.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /hunter_msgs/msg/HunterBmsStatus.msg: -------------------------------------------------------------------------------- 1 | #BMS date 2 | uint8 SOC 3 | uint8 SOH 4 | float64 battery_voltage 5 | float64 battery_current 6 | float64 battery_temperature 7 | 8 | #BMS status 9 | uint8 Alarm_Status_1 10 | uint8 Alarm_Status_2 11 | uint8 Warning_Status_1 12 | uint8 Warning_Status_2 13 | -------------------------------------------------------------------------------- /hunter_msgs/msg/HunterDriverState.msg: -------------------------------------------------------------------------------- 1 | float64 driver_voltage 2 | float64 driver_temperature 3 | uint8 driver_state 4 | -------------------------------------------------------------------------------- /hunter_msgs/msg/HunterMotorState.msg: -------------------------------------------------------------------------------- 1 | float64 current 2 | float64 rpm 3 | float64 temperature 4 | float64 motor_pose 5 | -------------------------------------------------------------------------------- /hunter_msgs/msg/HunterStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | int8 MOTOR_ID_FRONT = 0 4 | int8 MOTOR_ID_REAR_LEFT = 1 5 | int8 MOTOR_ID_REAR_RIGHT = 2 6 | 7 | # motion state 8 | float64 linear_velocity 9 | float64 steering_angle 10 | 11 | # base state 12 | uint8 base_state 13 | uint8 control_mode 14 | uint16 fault_code 15 | float64 battery_voltage 16 | uint8 park_mode 17 | 18 | # motor state 19 | HunterMotorState[3] motor_states 20 | # driver state 21 | HunterDriverState[3] driver_states 22 | -------------------------------------------------------------------------------- /hunter_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hunter_msgs 4 | 0.0.0 5 | The hunter_msgs package 6 | 7 | 8 | 9 | 10 | rdu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | message_generation 53 | std_msgs 54 | std_msgs 55 | std_msgs 56 | message_runtime 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | --------------------------------------------------------------------------------