Showing preview only (2,430K chars total). Download the full file or copy to clipboard to get everything.
Repository: ACDSLab/MPPI-Generic
Branch: main
Commit: b5c8daab6a91
Files: 295
Total size: 2.3 MB
Directory structure:
gitextract_ky3rcz46/
├── .clang-format
├── .gitattributes
├── .gitignore
├── .gitmodules
├── CMakeLists.txt
├── LICENSE
├── README.md
├── VERSION
├── cmake/
│ ├── Config.cmake.in
│ ├── MPPIGenericToolsConfig.cmake
│ └── Modules/
│ ├── CMakeLists.txt.gtest.in
│ └── cmake_uninstall.cmake.in
├── doc/
│ └── feedback.md
├── examples/
│ ├── CMakeLists.txt
│ ├── cartpole_example.cu
│ ├── double_integrator_CORL2020.cu
│ └── double_integrator_example.cu
├── include/
│ └── mppi/
│ ├── controllers/
│ │ ├── ColoredMPPI/
│ │ │ ├── colored_mppi_controller.cu
│ │ │ └── colored_mppi_controller.cuh
│ │ ├── MPPI/
│ │ │ ├── mppi_controller.cu
│ │ │ └── mppi_controller.cuh
│ │ ├── Primitives/
│ │ │ ├── primitives_controller.cu
│ │ │ └── primitives_controller.cuh
│ │ ├── R-MPPI/
│ │ │ ├── robust_mppi_controller.cu
│ │ │ └── robust_mppi_controller.cuh
│ │ ├── Tube-MPPI/
│ │ │ ├── tube_mppi_controller.cu
│ │ │ └── tube_mppi_controller.cuh
│ │ ├── controller.cu
│ │ └── controller.cuh
│ ├── core/
│ │ ├── base_plant.hpp
│ │ ├── buffer.hpp
│ │ ├── buffered_plant.hpp
│ │ ├── mppi_common.cu
│ │ ├── mppi_common.cuh
│ │ ├── rmppi_kernels.cu
│ │ └── rmppi_kernels.cuh
│ ├── cost_functions/
│ │ ├── autorally/
│ │ │ ├── ar_robust_cost.cu
│ │ │ ├── ar_robust_cost.cuh
│ │ │ ├── ar_standard_cost.cu
│ │ │ └── ar_standard_cost.cuh
│ │ ├── cartpole/
│ │ │ ├── cartpole_quadratic_cost.cu
│ │ │ └── cartpole_quadratic_cost.cuh
│ │ ├── cost.cu
│ │ ├── cost.cuh
│ │ ├── double_integrator/
│ │ │ ├── double_integrator_circle_cost.cu
│ │ │ ├── double_integrator_circle_cost.cuh
│ │ │ ├── double_integrator_robust_cost.cu
│ │ │ └── double_integrator_robust_cost.cuh
│ │ ├── quadratic_cost/
│ │ │ ├── quadratic_cost.cu
│ │ │ └── quadratic_cost.cuh
│ │ └── quadrotor/
│ │ ├── quadrotor_map_cost.cu
│ │ ├── quadrotor_map_cost.cuh
│ │ ├── quadrotor_quadratic_cost.cu
│ │ └── quadrotor_quadratic_cost.cuh
│ ├── ddp/
│ │ ├── boxqp.h
│ │ ├── ddp.h
│ │ ├── ddp_costs.h
│ │ ├── ddp_dynamics.h
│ │ ├── ddp_model_wrapper.h
│ │ ├── ddp_tracking_costs.h
│ │ ├── result.h
│ │ └── util.h
│ ├── dynamics/
│ │ ├── autorally/
│ │ │ ├── ar_nn_model.cu
│ │ │ └── ar_nn_model.cuh
│ │ ├── bicycle_slip/
│ │ │ ├── bicycle_slip_parametric.cu
│ │ │ └── bicycle_slip_parametric.cuh
│ │ ├── cartpole/
│ │ │ ├── cartpole_dynamics.cu
│ │ │ └── cartpole_dynamics.cuh
│ │ ├── double_integrator/
│ │ │ ├── di_dynamics.cu
│ │ │ └── di_dynamics.cuh
│ │ ├── dubins/
│ │ │ ├── dubins.cu
│ │ │ └── dubins.cuh
│ │ ├── dynamics.cu
│ │ ├── dynamics.cuh
│ │ ├── linear/
│ │ │ ├── linear.cu
│ │ │ └── linear.cuh
│ │ ├── quadrotor/
│ │ │ ├── quadrotor_dynamics.cu
│ │ │ └── quadrotor_dynamics.cuh
│ │ ├── racer_dubins/
│ │ │ ├── racer_dubins.cu
│ │ │ ├── racer_dubins.cuh
│ │ │ ├── racer_dubins_elevation.cu
│ │ │ ├── racer_dubins_elevation.cuh
│ │ │ ├── racer_dubins_elevation_lstm_steering.cu
│ │ │ ├── racer_dubins_elevation_lstm_steering.cuh
│ │ │ ├── racer_dubins_elevation_lstm_unc.cu
│ │ │ ├── racer_dubins_elevation_lstm_unc.cuh
│ │ │ ├── racer_dubins_elevation_suspension_lstm.cu
│ │ │ └── racer_dubins_elevation_suspension_lstm.cuh
│ │ └── racer_suspension/
│ │ ├── racer_suspension.cu
│ │ └── racer_suspension.cuh
│ ├── feedback_controllers/
│ │ ├── CCM/
│ │ │ └── ccm.h
│ │ ├── DDP/
│ │ │ ├── ddp.cu
│ │ │ └── ddp.cuh
│ │ ├── feedback.cu
│ │ └── feedback.cuh
│ ├── instantiations/
│ │ ├── autorally_mppi/
│ │ │ └── autorally_mppi.cuh
│ │ ├── cartpole_mppi/
│ │ │ └── cartpole_mppi.cuh
│ │ ├── double_integrator_mppi/
│ │ │ └── double_integrator_mppi.cuh
│ │ └── quadrotor_mppi/
│ │ └── quadrotor_mppi.cuh
│ ├── sampling_distributions/
│ │ ├── colored_noise/
│ │ │ ├── colored_noise.cu
│ │ │ └── colored_noise.cuh
│ │ ├── gaussian/
│ │ │ ├── gaussian.cu
│ │ │ └── gaussian.cuh
│ │ ├── nln/
│ │ │ ├── nln.cu
│ │ │ └── nln.cuh
│ │ ├── piecewise_linear/
│ │ │ └── piecewise_linear_noise.cuh
│ │ ├── sampling_distribution.cu
│ │ ├── sampling_distribution.cuh
│ │ └── smooth-MPPI/
│ │ ├── smooth-MPPI.cu
│ │ └── smooth-MPPI.cuh
│ ├── shaping_functions/
│ │ ├── CEM/
│ │ │ ├── cem_shaping_function.cu
│ │ │ └── cem_shaping_function.cuh
│ │ ├── shaping_function.cu
│ │ └── shaping_function.cuh
│ ├── utils/
│ │ ├── activation_functions.cuh
│ │ ├── angle_utils.cuh
│ │ ├── cuda_math_utils.cuh
│ │ ├── eigen_type_conversions.h
│ │ ├── file_utils.h
│ │ ├── gpu_err_chk.cuh
│ │ ├── logger.hpp
│ │ ├── managed.cuh
│ │ ├── math_utils.h
│ │ ├── matrix_mult_utils.cuh
│ │ ├── nn_helpers/
│ │ │ ├── fnn_helper.cu
│ │ │ ├── fnn_helper.cuh
│ │ │ ├── lstm_helper.cu
│ │ │ ├── lstm_helper.cuh
│ │ │ ├── lstm_lstm_helper.cu
│ │ │ ├── lstm_lstm_helper.cuh
│ │ │ └── meta_math.h
│ │ ├── numerical_integration.h
│ │ ├── parallel_utils.cuh
│ │ ├── risk_utils.cu
│ │ ├── risk_utils.cuh
│ │ ├── test_helper.h
│ │ ├── texture_helpers/
│ │ │ ├── texture_helper.cu
│ │ │ ├── texture_helper.cuh
│ │ │ ├── three_d_texture_helper.cu
│ │ │ ├── three_d_texture_helper.cuh
│ │ │ ├── two_d_texture_helper.cu
│ │ │ └── two_d_texture_helper.cuh
│ │ └── type_printing.h
│ └── version.h.in
├── resources/
│ ├── PF_1000_cell_init.npz
│ ├── PF_1000_hidden_init.npz
│ ├── PF_1000_lstm.npz
│ ├── PF_1000_output.npz
│ ├── ackerman_test.npz
│ ├── autorally_nnet_09_12_2018.npz
│ ├── bicycle_slip_hybrid.npz
│ ├── bicycle_slip_hybrid_test.npz
│ ├── bicycle_slip_kinematic_test.npz
│ ├── bicycle_slip_test.npz
│ ├── body_loss_bicycle_slip_kinematic_2023_03_10-12_54_39.npz
│ ├── ccrf_track.npz
│ ├── cell_init.npz
│ ├── hidden_init.npz
│ ├── lstm.npz
│ ├── lstm_lstm_ackerman.npz
│ ├── lstm_lstm_bicycle_slip_kinematic.npz
│ ├── lstm_lstm_steering.npz
│ ├── lstm_lstm_steering_accel.npz
│ ├── lstm_lstm_test.npz
│ ├── network_rde_2024_03_22-19_00_26.npz
│ ├── network_rde_test.npz
│ ├── output.npz
│ ├── sim_PF_200_cell_init.npz
│ ├── sim_PF_200_hidden_init.npz
│ ├── sim_PF_200_lstm.npz
│ └── sim_PF_200_output.npz
├── scripts/
│ ├── autorally/
│ │ ├── lstm_converter.py
│ │ └── test/
│ │ ├── generateTestMaps.py
│ │ └── generateTestNetwork.py
│ ├── colored_noise.py
│ ├── double_integrator/
│ │ ├── generate_free_energy_video.py
│ │ ├── generate_trajectory_video.py
│ │ ├── plot_DI_test_trajectories.py
│ │ ├── plot_DI_test_trajectories_tube_only.py
│ │ └── plot_stuff.py
│ └── run_autoformatter.sh
├── src/
│ ├── CMakeLists.txt
│ └── controllers/
│ ├── CMakeLists.txt
│ ├── autorally/
│ │ ├── CMakeLists.txt
│ │ └── autorally_mppi.cu
│ ├── cartpole/
│ │ ├── CMakeLists.txt
│ │ └── cartpole_mppi.cu
│ ├── double_integrator/
│ │ ├── CMakeLists.txt
│ │ └── double_integrator_mppi.cu
│ └── quadrotor/
│ ├── CMakeLists.txt
│ └── quadrotor_mppi.cu
└── tests/
├── CMakeLists.txt
├── controllers/
│ ├── CMakeLists.txt
│ ├── controller_generic_tests.cu
│ ├── controller_kernel_testing.cu
│ ├── rmppi_test.cu
│ ├── tube_mppi_test.cu
│ └── vanilla_mppi_test.cu
├── cost_functions/
│ ├── CMakeLists.txt
│ ├── autorally_robust_cost_test.cu
│ ├── autorally_standard_cost_test.cu
│ ├── cartpole_quadratic_cost_test.cu
│ ├── general_cost_test.cu
│ ├── general_quadratic_costs.cu
│ ├── quadrotor_map_cost_test.cu
│ └── quadrotor_quadratic_cost_test.cu
├── dynamics/
│ ├── CMakeLists.txt
│ ├── angle_utils_test.cu
│ ├── ar_dynamics_nn_test.cu
│ ├── bicycle_slip_parametric_model_test.cu
│ ├── cartpole_dynamics_tests.cu
│ ├── dubins_dynamics_tests.cu
│ ├── dynamics_generic_tests.cu
│ ├── linear_dynamics_tests.cu
│ ├── quadrotor_dynamics_tests.cu
│ ├── racer_dubins_elevation_lstm_steering_model_test.cu
│ ├── racer_dubins_elevation_lstm_uncertainty_model_test.cu
│ ├── racer_dubins_elevation_model_test.cu
│ ├── racer_dubins_elevation_suspension_test.cu
│ ├── racer_dubins_model_test.cu
│ └── racer_suspension_model_test.cu
├── feedback_controllers/
│ ├── CMakeLists.txt
│ ├── ddp_test.cu
│ └── generic_feedback_controller_test.cu
├── include/
│ ├── kernel_tests/
│ │ ├── core/
│ │ │ ├── normexp_kernel_test.cu
│ │ │ ├── normexp_kernel_test.cuh
│ │ │ ├── rmppi_kernel_test.cu
│ │ │ ├── rmppi_kernel_test.cuh
│ │ │ ├── rollout_kernel_test.cu
│ │ │ ├── rollout_kernel_test.cuh
│ │ │ ├── weightedreduction_kernel_test.cu
│ │ │ └── weightedreduction_kernel_test.cuh
│ │ ├── cost_functions/
│ │ │ ├── autorally/
│ │ │ │ ├── ar_robust_cost_kernel_test.cu
│ │ │ │ ├── ar_robust_cost_kernel_test.cuh
│ │ │ │ ├── ar_standard_cost_kernel_test.cu
│ │ │ │ └── ar_standard_cost_kernel_test.cuh
│ │ │ ├── cartpole/
│ │ │ │ ├── cartpole_quadratic_cost_kernel_test.cu
│ │ │ │ └── cartpole_quadratic_cost_kernel_test.cuh
│ │ │ ├── cost_generic_kernel_tests.cu
│ │ │ └── cost_generic_kernel_tests.cuh
│ │ ├── dynamics/
│ │ │ ├── autorally/
│ │ │ │ ├── ar_nn_dynamics_kernel_test.cu
│ │ │ │ └── ar_nn_dynamics_kernel_test.cuh
│ │ │ ├── cartpole/
│ │ │ │ ├── cartpole_dynamics_kernel_test.cu
│ │ │ │ └── cartpole_dynamics_kernel_test.cuh
│ │ │ ├── dynamics_generic_kernel_tests.cu
│ │ │ └── dynamics_generic_kernel_tests.cuh
│ │ ├── shaping_functions/
│ │ │ ├── shaping_function_kernels_tests.cu
│ │ │ └── shaping_function_kernels_tests.cuh
│ │ └── utils/
│ │ ├── network_helper_kernel_test.cuh
│ │ └── texture_test_kernels.cuh
│ └── mppi_test/
│ └── mock_classes/
│ ├── mock_classes.h
│ ├── mock_controller.h
│ ├── mock_costs.h
│ ├── mock_dynamics.h
│ ├── mock_feedback.h
│ └── mock_sampler.h
├── integration/
│ ├── CMakeLists.txt
│ └── integrator_tests.cu
├── math_utils/
│ ├── CMakeLists.txt
│ ├── cuda_math_utils_tests.cu
│ └── math_utils_test.cu
├── misc/
│ ├── CMakeLists.txt
│ ├── di_dynamics_kernel_tests.cu
│ ├── di_dynamics_kernel_tests.cuh
│ └── miscellaneous_tests.cu
├── mppi_core/
│ ├── CCM_tests.cu
│ ├── CMakeLists.txt
│ ├── base_plant_tester.cu
│ ├── buffered_plant_tester.cu
│ ├── normexp_kernel_tests.cu
│ ├── rmppi_kernel_tests.cu
│ ├── rollout_kernel_tests.cu
│ ├── viz_kernels_test.cu
│ └── weightedreduction_kernel_tests.cu
├── nn_helpers/
│ ├── CMakeLists.txt
│ ├── activation_functions_tests.cu
│ ├── fnn_helper_test.cu
│ ├── lstm_helper_test.cu
│ └── lstm_lstm_helper_test.cu
├── sampling_distributions/
│ ├── CMakeLists.txt
│ ├── colored_noise_tests.cu
│ ├── gaussian_noise_tests.cu
│ └── generic_sampling_distribution_tests.cu
├── shaping_functions/
│ ├── CMakeLists.txt
│ ├── cem_shaping_function_test.cu
│ └── shaping_function_test.cu
├── templated_headers/
│ ├── autorally_test_map.h.in
│ ├── autorally_test_network.h.in
│ ├── racer_test_networks.h.in
│ └── test_networks.h.in
├── test_main.cpp
└── texture_helpers/
├── CMakeLists.txt
├── texture_helper_test.cu
├── three_d_texture_helper_test.cu
└── two_d_texture_helper_test.cu
================================================
FILE CONTENTS
================================================
================================================
FILE: .clang-format
================================================
---
# roscpp style file taken from https://github.com/PickNikRobotics/roscpp_code_format.git
BasedOnStyle: Google
AccessModifierOffset: -2
ConstructorInitializerIndentWidth: 2
AlignEscapedNewlinesLeft: false
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AllowShortFunctionsOnASingleLine: None
AlwaysBreakTemplateDeclarations: true
AlwaysBreakBeforeMultilineStrings: true
BreakBeforeBinaryOperators: false
BreakBeforeTernaryOperators: false
BreakConstructorInitializersBeforeComma: true
BinPackParameters: true
ColumnLimit: 120
ConstructorInitializerAllOnOneLineOrOnePerLine: true
DerivePointerBinding: false
PointerBindsToType: true
ExperimentalAutoDetectBinPacking: false
IndentCaseLabels: true
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 60
PenaltyBreakString: 1
PenaltyBreakFirstLessLess: 1000
PenaltyExcessCharacter: 1000
PenaltyReturnTypeOnItsOwnLine: 90
SpacesBeforeTrailingComments: 2
Cpp11BracedListStyle: false
Standard: Auto
IndentWidth: 2
TabWidth: 2
UseTab: Never
IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false
SpacesInAngles: false
SpaceInEmptyParentheses: false
SpacesInCStyleCastParentheses: false
SpaceAfterControlStatementKeyword: true
SpaceBeforeAssignmentOperators: true
ContinuationIndentWidth: 4
SortIncludes: false
SpaceAfterCStyleCast: false
# Configure each individual brace in BraceWrapping
BreakBeforeBraces: Custom
# Control of individual brace wrapping cases
BraceWrapping: {
AfterClass: 'true'
AfterControlStatement: 'true'
AfterEnum : 'true'
AfterFunction : 'true'
AfterNamespace : 'true'
AfterStruct : 'true'
AfterUnion : 'true'
BeforeCatch : 'true'
BeforeElse : 'true'
IndentBraces : 'false'
}
...
================================================
FILE: .gitattributes
================================================
*.npz filter=lfs diff=lfs merge=lfs -text
================================================
FILE: .gitignore
================================================
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
# clion ide
.idea
cmake-build-debug*
cmake-build-release*
.clion*
# vscode ide
.vscode
# Build directories
build
# clangd build
.clangd
compile_commands.json
.cache
# resouces folder
resource
# video files
*.mp4
*.mpeg
================================================
FILE: .gitmodules
================================================
[submodule "submodules/cnpy"]
path = submodules/cnpy
url = https://github.com/ACDSLab/cnpy.git
================================================
FILE: CMakeLists.txt
================================================
if (CMAKE_VERSION VERSION_LESS 3.12)
cmake_minimum_required(VERSION 3.8)
else()
cmake_minimum_required(VERSION 3.8...3.24)
endif()
# https://cmake.org/cmake/help/latest/policy/CMP0104.html
if (CMAKE_VERSION VERSION_LESS 3.24 AND POLICY CMP0104)
cmake_policy(SET CMP0104 OLD)
endif()
# Get Version from file
file(READ "${CMAKE_CURRENT_SOURCE_DIR}/VERSION" MPPI_VERSION_FROM_FILE)
# Clean up trailng whitespace and newlines
string(STRIP "${MPPI_VERSION_FROM_FILE}" MPPI_VERSION)
project(MPPI-Generic VERSION ${MPPI_VERSION} LANGUAGES C CXX CUDA)
####################################
# Set up options for configuration #
####################################
option(MPPI_BUILD_TESTS "Build unit tests." OFF)
option(MPPI_BUILD_EXAMPLES "Build example code." OFF)
option(MPPI_USE_CCACHE "Use ccache (when available) to potentially speed up repeated builds" ON)
option(MPPI_USE_CUDA_BARRIERS "Compile MPPI-Generic with cuda barrier support. Turn off for GPUs older than the 20 series." ON)
option(MPPI_USE_CUDA_BARRIERS_COST "Compile MPPI-Generic with cuda barriers for cost-only kernels when general barrier support is enabled." OFF)
option(MPPI_USE_CUDA_BARRIERS_DYN "Compile MPPI-Generic with cuda barriers for dynamics-only kernels when general barrier support is enabled." ON)
option(MPPI_USE_CUDA_BARRIERS_ROLLOUT "Compile MPPI-Generic with cuda barriers for combined kernels when general barrier support is enabled." ON)
option(MPPI_EXPORT_PACKAGE "Export this folder as the MPPI installation to the global CMake package repository" OFF)
set(MPPI_CUDA_ARCH_LIST "" CACHE STRING "Specific CUDA Architectures to build for. Leave empty for automatic selection.")
# Configure CMake Cuda Flags for MPPI
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
include(MPPIGenericToolsConfig)
set(BUILD_FLAGS "-Wuninitialized -Wall")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${BUILD_FLAGS}")
set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -w")
# Allow the lib location to be overwritten from command line
if (NOT CMAKE_LIBRARY_OUTPUT_DIRECTORY)
get_filename_component(PROJECT_LIBS_DIR ${PROJECT_BINARY_DIR}/lib ABSOLUTE)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib)
else()
get_filename_component(PROJECT_LIBS_DIR ${CMAKE_LIBRARY_OUTPUT_DIRECTORY} ABSOLUTE)
endif()
# set up cnpy
# Don't install cnpy to user package registry
set(TMP_PACKAGE_REGISTRY_EXPORT ${CMAKE_EXPORT_NO_PACKAGE_REGISTRY})
set(CMAKE_EXPORT_NO_PACKAGE_REGISTRY ON)
add_subdirectory(submodules/cnpy)
# Set the no package registry back to default
set(CMAKE_EXPORT_NO_PACKAGE_REGISTRY ${TMP_PACKAGE_REGISTRY_EXPORT})
# Find Eigen
find_package(Eigen3 REQUIRED)
# REQUIRED for CUDA to correctly align Eigen member variables inside structures/classes
# https://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2016/06/msg00006.html
# add_definitions(-DEIGEN_MAX_STATIC_ALIGN_BYTES=0)
# Generate Version Header File from Project Version
configure_file("${PROJECT_SOURCE_DIR}/include/mppi/version.h.in" "${PROJECT_BINARY_DIR}/include/mppi/version.h")
# Create a Header-only MPPI Library
add_library(${MPPI_HEADER_LIBRARY_NAME} INTERFACE)
add_library(MPPI::MPPI ALIAS ${MPPI_HEADER_LIBRARY_NAME})
target_include_directories(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}
"$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>"
"$<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:include>"
)
target_link_libraries(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
${MPPI_GENERIC_CUDA_EXTRA_LIBS}
${CUDA_LIBRARIES}
cnpy
Eigen3::Eigen
)
target_include_directories(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/submodules/cnpy>"
"$<INSTALL_INTERFACE:include/cnpy>"
)
# --display-error-number was added as a compilation flag in CUDA 11.2
if (${CUDA_VERSION} VERSION_GREATER_EQUAL "11.2")
target_compile_options(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
$<$<COMPILE_LANGUAGE:CUDA>:--display-error-number>
)
endif()
# Allow the ability to turn off cuda barriers through CMake. Necessary for GPUs older than
# the 20 series
if (MPPI_USE_CUDA_BARRIERS)
target_compile_definitions(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
CMAKE_USE_CUDA_BARRIERS)
if (MPPI_USE_CUDA_BARRIERS_DYN)
target_compile_definitions(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
CMAKE_USE_CUDA_BARRIERS_DYN)
endif()
if (MPPI_USE_CUDA_BARRIERS_ROLLOUT)
target_compile_definitions(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
CMAKE_USE_CUDA_BARRIERS_ROLLOUT)
endif()
if (MPPI_USE_CUDA_BARRIERS_COST)
target_compile_definitions(${MPPI_HEADER_LIBRARY_NAME} INTERFACE
CMAKE_USE_CUDA_BARRIERS_COST)
endif()
endif()
if (MPPI_BUILD_TESTS OR MPPI_BUILD_EXAMPLES)
# find yaml-cpp
find_package(yaml-cpp REQUIRED)
include_directories(${YAML_CPP_INCLUDE_DIR})
endif()
# Install all library header files
install(
DIRECTORY ${PROJECT_SOURCE_DIR}/include/mppi
DESTINATION include
PATTERN "*.in" EXCLUDE
)
# Install generated header file version.h
install(
DIRECTORY ${PROJECT_BINARY_DIR}/include/mppi
DESTINATION include
)
###########################################
# Set up CMake configuration installation #
###########################################
set(CMAKE_CONFIG_DEST "lib/cmake/${PROJECT_NAME}")
install(
TARGETS ${MPPI_HEADER_LIBRARY_NAME}
EXPORT ${PROJECT_NAME}-targets
)
install(
EXPORT ${PROJECT_NAME}-targets
NAMESPACE MPPI::
DESTINATION ${CMAKE_CONFIG_DEST}
FILE ${PROJECT_NAME}Targets.cmake
)
install(
EXPORT ${PROJECT_NAME}-targets
DESTINATION ${CMAKE_CONFIG_DEST}
FILE ${PROJECT_NAME}TargetsNoNamespace.cmake
)
# Set up MPPI to export the header library as MPPI in the MPPI namespace
set_target_properties (${MPPI_HEADER_LIBRARY_NAME} PROPERTIES EXPORT_NAME MPPI)
export (TARGETS ${MPPI_HEADER_LIBRARY_NAME}
NAMESPACE MPPI::
FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake
)
# Export the package for use from the build-tree
# (this registers the build-tree with a global CMake-registry ~/.cmake)
if (MPPI_EXPORT_PACKAGE)
export(PACKAGE ${PROJECT_NAME})
endif()
#################################################################################
# Create CMake Config and Version files and install in the appropriate location #
#################################################################################
# Create a CMake variable of the appropriate include directory depending on
# whether it is consider from the build tree or the install tree
set(TMP_MPPI_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/include")
set(TMP_MPPI_TARGET_CMAKE_FILE "${PROJECT_BINARY_DIR}/${PROJECT_NAME}Targets.cmake")
# Set up build tree config file
configure_package_config_file(
${PROJECT_SOURCE_DIR}/cmake/Config.cmake.in
${PROJECT_BINARY_DIR}/${PROJECT_NAME}Config.cmake
INSTALL_DESTINATION ${PROJECT_BINARY_DIR}
PATH_VARS TMP_MPPI_INCLUDE_DIRS TMP_MPPI_TARGET_CMAKE_FILE
)
# Set up install tree config file
set(TMP_MPPI_INCLUDE_DIRS "include/")
set(TMP_MPPI_TARGET_CMAKE_FILE "${CMAKE_CONFIG_DEST}/${PROJECT_NAME}Targets.cmake")
configure_package_config_file(
${PROJECT_SOURCE_DIR}/cmake/Config.cmake.in
${PROJECT_BINARY_DIR}/${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake
INSTALL_DESTINATION ${CMAKE_CONFIG_DEST}
PATH_VARS TMP_MPPI_INCLUDE_DIRS TMP_MPPI_TARGET_CMAKE_FILE
)
write_basic_package_version_file(
${PROJECT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake
VERSION ${PROJECT_VERSION}
COMPATIBILITY SameMinorVersion
)
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/cmake/MPPIGenericToolsConfig.cmake
${PROJECT_BINARY_DIR}/MPPIGenericToolsConfig.cmake
COPYONLY
)
install(FILES
${PROJECT_BINARY_DIR}/${CMAKE_FILES_DIRECTORY}/${PROJECT_NAME}Config.cmake
${PROJECT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake
${CMAKE_CURRENT_SOURCE_DIR}/cmake/MPPIGenericToolsConfig.cmake
DESTINATION ${CMAKE_CONFIG_DEST}
)
if (MPPI_BUILD_TESTS OR MPPI_BUILD_EXAMPLES)
add_subdirectory(src)
endif()
if (MPPI_BUILD_EXAMPLES)
add_subdirectory(examples)
endif()
# Add custom cmake finds
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules")
# Use CMake to download gtest as part of the configure step
###################################################################
# Add gtest
###################################################################
if (NOT DEFINED CMAKE_TOOLCHAIN_FILE AND MPPI_BUILD_TESTS)
message(STATUS "Building MPPI Generic tests")
enable_testing()
############################################################
# copied from
# https://github.com/google/googletest/tree/master/googletest#incorporating-into-an-existing-cmake-project
############################################################
# Download and unpack googletest at configure time
if (NOT TARGET gtest_main)
list(GET CMAKE_MODULE_PATH -1 MPPI_GENERIC_MODULES)
configure_file(${MPPI_GENERIC_MODULES}/CMakeLists.txt.gtest.in
${PROJECT_BINARY_DIR}/googletest-download/CMakeLists.txt)
execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_BINARY_DIR}/googletest-download )
if(result)
message(FATAL_ERROR "CMake step for googletest failed: ${result}")
endif()
execute_process(COMMAND ${CMAKE_COMMAND} --build .
RESULT_VARIABLE result
WORKING_DIRECTORY ${PROJECT_BINARY_DIR}/googletest-download )
if(result)
message(FATAL_ERROR "Build step for googletest failed: ${result}")
endif()
# Prevent overriding the parent project's compiler/linker
# settings on Windows
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
# Prevent googletest installation
set(INSTALL_GTEST OFF)
# Add googletest directly to our build. This defines
# the gtest and gtest_main targets.
add_subdirectory(${PROJECT_BINARY_DIR}/googletest-src
${PROJECT_BINARY_DIR}/googletest-build)
endif()
include(GoogleTest)
add_subdirectory(tests)
endif()
# Uninstall
if(NOT TARGET uninstall)
configure_file(
"${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules/cmake_uninstall.cmake.in"
"${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
IMMEDIATE @ONLY)
add_custom_target(uninstall
COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
endif()
================================================
FILE: LICENSE
================================================
BSD 2-Clause License
Copyright (c) 2020, Georgia Institute of Technology
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
================================================
FILE: README.md
================================================
# MPPI-Generic
MPPI-Generic is a C++/CUDA header-only library implementation of Model Predictive Path Integral Control (MPPI) by [Williams et al.](https://ieeexplore.ieee.org/document/8558663)
## Citation
If you use this library for research purposes, please cite the following [paper](https://arxiv.org/abs/2409.07563):
```
@misc{vlahov2024mppi,
title={MPPI-Generic: A CUDA Library for Stochastic Trajectory Optimization},
author={Bogdan Vlahov and Jason Gibson and Manan Gandhi and Evangelos A. Theodorou},
year={2024},
eprint={2409.07563},
archivePrefix={arXiv},
primaryClass={cs.MS},
url={https://arxiv.org/abs/2409.07563},
}
```
## Requirements
MPPI-Generic relies on the following:
* An NVIDIA GPU
* GCC/G++
* CUDA 10 or newer (CUDA 11.7+ is recommended but our library is compatible back to CUDA 10)
* [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page)
* [CMake](https://cmake.org/) 3.10 or newer
* git and git-lfs
### Unit tests requirements
* [yaml-cpp](https://github.com/jbeder/yaml-cpp)
* python-pil
## Prerequisite Setup (Ubuntu)
1. Follow the instructions to install CUDA provided [here](https://docs.nvidia.com/cuda/cuda-installation-guide-linux/index.html).
2. Install all the other prerequisites through `apt-get`:
```bash
sudo apt-get install libeigen3-dev git git-lfs cmake gcc
# Setup git lfs if it is the first you have installed it
git lfs install
# extra installs if you are wanting to build unit tests
sudo apt-get install libyaml-cpp-dev python3-pil
```
Note: If using Pop!\_OS you can `sudo apt install system76-cuda` instead of installing CUDA manually.
## Download the repo
```bash
cd /path/to/repos
git clone https://github.com/ACDSLab/MPPI-Generic.git
cd MPPI-Generic
git submodule update --init --recursive
```
## Building MPPI-Generic with tests
The default is to build the library with tests OFF.
If you would like to turn on the tests when building, pass the flag `-DMPPI_BUILD_TESTS=ON` when configuring cmake.
```bash
mkdir build
cd build
cmake -DMPPI_BUILD_TESTS=ON ..
make
make test
```
## Acknowledgements
Approved for Public Release, Distribution Unlimited.
================================================
FILE: VERSION
================================================
0.9.0
================================================
FILE: cmake/Config.cmake.in
================================================
# - Config file for the MPPI-Generic package
# It defines the following variables
# MPPI_INCLUDE_DIRS - include directories for MPPI-Generic
# MPPI_INCLUDE_DIR - include directories for MPPI-Generic
# MPPI_LIBRARIES - libraries to link against
@PACKAGE_INIT@
include(CMakeFindDependencyMacro)
set(MPPI_LIBRARIES "MPPI::MPPI")
set_and_check(MPPI_INCLUDE_DIRS "@PACKAGE_TMP_MPPI_INCLUDE_DIRS@")
set_and_check(MPPI_TARGET_CMAKE_FILE "@PACKAGE_TMP_MPPI_TARGET_CMAKE_FILE@")
# Find dependencies of MPPI-Generic
find_dependency(cnpy REQUIRED HINTS ${PACKAGE_PREFIX_DIR})
find_dependency(Eigen3 REQUIRED)
# Set up cmake targets and autodetection for CUDA architectures
include(${MPPI_TARGET_CMAKE_FILE})
include("${CMAKE_CURRENT_LIST_DIR}/MPPIGenericToolsConfig.cmake")
set(MPPI_INCLUDE_DIR ${MPPI_INCLUDE_DIRS})
check_required_components(@PROJECT_NAME@)
================================================
FILE: cmake/MPPIGenericToolsConfig.cmake
================================================
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type options: Release, RelWithDebInfo, Debug" FORCE)
message(STATUS "Setting Build Type to ${CMAKE_BUILD_TYPE} by default")
endif()
if(NOT DEFINED CMAKE_CXX_STANDARD)
if (CMAKE_CUDA_COMPILER_VERSION VERSION_GREATER_EQUAL "13")
# Cuda's libcu++ requires C++17 on CUDA 13+
set(CMAKE_CXX_STANDARD 17)
else()
set(CMAKE_CXX_STANDARD 11)
endif()
endif()
if (NOT DEFINED CMAKE_CUDA_STANDARD)
set(CMAKE_CUDA_STANDARD ${CMAKE_CXX_STANDARD})
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CUDA_STANDARD_REQUIRED ON)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# Look for ccache to potentially speed up repeated compilations
find_program(CCACHE_PROGRAM ccache)
if (CCACHE_PROGRAM AND NOT MPPI_USE_CCACHE)
message(STATUS "Using ccache to speed up repeated builds")
set(CMAKE_C_COMPILER_LAUNCHER ${CCACHE_PROGRAM})
set(CMAKE_CXX_COMPILER_LAUNCHER ${CCACHE_PROGRAM})
set(CMAKE_CUDA_COMPILER_LAUNCHER ${CCACHE_PROGRAM})
elseif(NOT CCACHE_PROGRAM AND NOT MPPI_USE_CCACHE)
message(STATUS "ccache not found. Using ccache can speed up repeated builds.")
endif()
# Add debug flags so cuda-gdb can be used to stop inside a kernel.
# NOTE: You may have to run make multiple times for it to compile successfully.
set(CMAKE_CUDA_FLAGS_DEBUG "${CMAKE_CUDA_FLAGS_DEBUG} -G --keep")
set(CMAKE_CUDA_FLAGS_RELWITHDEBINFO "${CMAKE_CUDA_FLAGS_RELWITHDEBINFO} --generate-line-info")
# Generate variable for all the extra cuda libraries we use
set(MPPI_GENERIC_CUDA_EXTRA_LIBS "")
if (CMAKE_VERSION VERSION_LESS 3.24)
# required for curand on some systems
find_package(CUDA REQUIRED)
if(${CUDA_curand_LIBRARY} MATCHES "NOTFOUND")
message(ERROR "cuRAND library not found.")
endif()
list(APPEND MPPI_GENERIC_CUDA_EXTRA_LIBS ${CUDA_curand_LIBRARY} ${CUDA_CUFFT_LIBRARIES})
else()
find_package(CUDAToolkit REQUIRED)
set(CUDA_VERSION ${CUDAToolkit_VERSION})
list(APPEND MPPI_GENERIC_CUDA_EXTRA_LIBS CUDA::curand CUDA::cufft)
endif()
# Generate name for MPPI header library
set(MPPI_HEADER_LIBRARY_NAME mppi_header_only_lib)
set(CUDA_PROPAGATE_HOST_FLAGS OFF)
#################################################################
# Autodetect Cuda Architecture on system and add to executables #
#################################################################
# CMake 3.24 added '-arch=native' support so until that version, we need to use the old method of autodetection
if (CMAKE_VERSION VERSION_LESS 3.24)
# Don't rerun autodetection when used as a submodule
if (NOT DEFINED MPPI_ARCH_FLAGS)
# More info for autodetection:
# https://stackoverflow.com/questions/35485087/determining-which-gencode-compute-arch-values-i-need-for-nvcc-within-cmak
CUDA_SELECT_NVCC_ARCH_FLAGS(MPPI_ARCH_FLAGS ${MPPI_CUDA_ARCH_LIST})
if (MPPI_ARCH_FLAGS STREQUAL "")
set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -maxrregcount=32 -arch=sm_35")
else()
string(REGEX REPLACE "-gencode;arch" "-gencode=arch" MPPI_ARCH_FLAGS "${MPPI_ARCH_FLAGS}")
string(REPLACE ";" " " MPPI_ARCH_FLAGS "${MPPI_ARCH_FLAGS}")
# string(REGEX REPLACE "^-gencode=arch" "-arch" MPPI_ARCH_FLAGS "${MPPI_ARCH_FLAGS}")
message(STATUS "CUDA Architecture(s): ${MPPI_ARCH_FLAGS}")
set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} ${MPPI_ARCH_FLAGS}")
endif()
else()
message(STATUS "Autodetection already ran and found ${MPPI_ARCH_FLAGS}.")
endif()
else()
# Don't rerun autodetection when used as a submodule
if (NOT DEFINED MPPI_ARCH_FLAGS)
if ("${MPPI_CUDA_ARCH_LIST}" STREQUAL "")
set(CMAKE_CUDA_ARCHITECTURES native)
else()
set(CMAKE_CUDA_ARCHITECTURES "${MPPI_CUDA_ARCH_LIST}")
endif()
set(MPPI_ARCH_FLAGS "${CMAKE_CUDA_ARCHITECTURES}")
message(STATUS "CUDA Architecture(s): ${CMAKE_CUDA_ARCHITECTURES}")
else()
message(STATUS "Autodetection already ran and found ${MPPI_ARCH_FLAGS}.")
endif()
endif()
================================================
FILE: cmake/Modules/CMakeLists.txt.gtest.in
================================================
cmake_minimum_required(VERSION 2.8.2)
project(googletest-download NONE)
include(ExternalProject)
ExternalProject_Add(googletest
GIT_REPOSITORY https://github.com/google/googletest.git
GIT_TAG release-1.12.1
SOURCE_DIR "${PROJECT_BINARY_DIR}/googletest-src"
BINARY_DIR "${PROJECT_BINARY_DIR}/googletest-build"
CONFIGURE_COMMAND ""
BUILD_COMMAND ""
INSTALL_COMMAND ""
TEST_COMMAND ""
)
================================================
FILE: cmake/Modules/cmake_uninstall.cmake.in
================================================
# ============================================================================
# Copyright (c) 2011-2012 University of Pennsylvania
# Copyright (c) 2013-2014 Carnegie Mellon University
# Copyright (c) 2013-2016 Andreas Schuh
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# See COPYING file for license information or visit
# https://cmake-basis.github.io/download.html#license
# ============================================================================
##############################################################################
# @file cmake_uninstall.cmake
# @brief Uninstallation script based on install_manifest*.txt files.
#
# @ingroup CMakeTools
##############################################################################
cmake_minimum_required (VERSION 2.8.12 FATAL_ERROR)
# ----------------------------------------------------------------------------
# set the install prefix
if (NOT DEFINED CMAKE_INSTALL_PREFIX)
set (CMAKE_INSTALL_PREFIX "@CMAKE_INSTALL_PREFIX@")
endif ()
# ----------------------------------------------------------------------------
# set the install configuration name
if (NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
if (BUILD_TYPE)
string (REGEX REPLACE "^[^A-Za-z0-9_]+" "" CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
else ()
set (CMAKE_INSTALL_CONFIG_NAME "@CMAKE_BUILD_TYPE@")
endif ()
message (STATUS "Uninstall configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
endif ()
# ----------------------------------------------------------------------------
# set the component getting uninstalled
if (NOT CMAKE_INSTALL_COMPONENT)
if (COMPONENT)
message (STATUS "Uninstall component: \"${COMPONENT}\"")
set (CMAKE_INSTALL_COMPONENT "${COMPONENT}")
else ()
set (CMAKE_INSTALL_COMPONENT)
endif ()
endif ()
# ----------------------------------------------------------------------------
# read manifest file
if (MANIFEST_FILE)
if (NOT EXISTS "${MANIFEST_FILE}")
message (FATAL_ERROR "Manifest file ${MANIFEST_FILE} does not exist!")
endif ()
set (MANIFEST_FILES "${MANIFEST_FILE}")
else ()
file (GLOB MANIFEST_FILES "${CMAKE_CURRENT_LIST_DIR}/@PROJECT_PACKAGE_CONFIG_PREFIX@*InstallManifest.txt")
if (NOT MANIFEST_FILES)
if (CMAKE_INSTALL_COMPONENT)
set (MANIFEST_FILE "${CMAKE_CURRENT_LIST_DIR}/install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
else ()
set (MANIFEST_FILE "${CMAKE_CURRENT_LIST_DIR}/install_manifest.txt")
endif ()
if (NOT EXISTS "${MANIFEST_FILE}")
message ("No manifest file found.")
return ()
endif ()
set (MANIFEST_FILES "${MANIFEST_FILE}")
endif ()
endif ()
set (MANIFEST)
foreach (MANIFEST_FILE IN LISTS MANIFEST_FILES)
file (READ "${MANIFEST_FILE}" _MANIFEST)
string (REGEX REPLACE "\n" ";" _MANIFEST "${_MANIFEST}")
list (REVERSE _MANIFEST)
list (APPEND MANIFEST "${_MANIFEST}")
endforeach ()
# ----------------------------------------------------------------------------
# remove package from CMake package registry
set (REGISTERED "@BASIS_REGISTER@")
if (WIN32 AND REGISTERED)
set (PKGUID "@TOPLEVEL_PROJECT_PACKAGE_UID@")
execute_process (
COMMAND reg delete "HKCU\\Software\\Kitware\\CMake\\Packages\\@PROJECT_PACKAGE_CONFIG_PREFIX@" /v "${PKGUID}" /f
RESULT_VARIABLE RT
ERROR_VARIABLE ERR
)
if (RT EQUAL 0)
message (STATUS "Deregister: Removed HKCU\\Software\\Kitware\\CMake\\Packages\\@PROJECT_PACKAGE_CONFIG_PREFIX@\\${PKGUID}")
else ()
string (STRIP "${ERR}" ERR)
message (STATUS "Deregister: Failed to remove package from registry: ${ERR}")
endif ()
endif ()
# ----------------------------------------------------------------------------
# remove installed files
foreach (F ${MANIFEST}) # skip empty entries, i.e., blank lines
set (F "$ENV{DESTDIR}${F}") # support change of root
if (EXISTS "${F}")
set (FILE_IN_USE FALSE)
if (NOT FILE_IN_USE)
message (STATUS "Uninstalling: ${F}")
execute_process (COMMAND "${CMAKE_COMMAND}" -E remove -f "${F}" RESULT_VARIABLE RT)
if (NOT RT EQUAL 0)
set (OK FALSE)
message (STATUS "Failed to uninstall ${F}")
endif ()
# remove .pyc files of .py files
if (F MATCHES "\\.py$" AND EXISTS "${F}c")
message (STATUS "Uninstalling: ${F}c")
execute_process (COMMAND "${CMAKE_COMMAND}" -E remove -f "${F}c" RESULT_VARIABLE RT)
if (NOT RT EQUAL 0)
message (STATUS "Failed to uninstall ${F}c")
endif ()
endif ()
else ()
message (STATUS "File-in-use: ${F}")
endif ()
else ()
message (STATUS "Non-existent: ${F}")
endif ()
endforeach ()
foreach (MANIFEST_FILE IN LISTS MANIFEST_FILES)
if (EXISTS "${MANIFEST_FILE}")
execute_process (COMMAND "${CMAKE_COMMAND}" -E remove -f "${MANIFEST_FILE}")
endif ()
endforeach ()
# ----------------------------------------------------------------------------
# remove empty directories
list (APPEND EXCLUDE_DIRS
"/"
"/usr"
"/usr/local"
"/opt"
"/opt/local"
"$ENV{HOME}"
"$ENV{HOME}/local"
# these should anyway never be used as installation prefix without subdirectory
"/bin"
"/boot"
"/dev"
"/etc"
"/home"
"/lib"
"/lib32"
"/lib64"
"/media"
"/mnt"
"/root"
"/proc"
"/sys"
"/var"
"/tmp"
"/lost+found"
"/cdrom"
)
if (WIN32)
get_filename_component (PROGRAM_FILES_DIR "[HKEY_LOCAL_MACHINE\\SOFTWARE\\Microsoft\\Windows\\CurrentVersion;ProgramFilesDir]" ABSOLUTE)
if (NOT PROGRAM_FILES_DIR OR PROGRAM_FILES_DIR MATCHES "/registry")
set (PROGRAM_FILES_DIR "C:/Program Files")
endif ()
list (APPEND EXCLUDE_DIRS "${PROGRAM_FILES_DIR}")
string (REPLACE "/" "\\" PROGRAM_FILES_DIR "${PROGRAM_FILES_DIR}")
list (APPEND EXCLUDE_DIRS "${PROGRAM_FILES_DIR}")
endif ()
# stop removing directories at root installation directory
# the subdirectory will be still removed if it is not in the
# list of excluded system directories
get_filename_component (D "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}" PATH)
list (APPEND EXCLUDE_DIRS "${D}")
string (REPLACE "." "\\." CMAKE_INSTALL_PREFIX_RE "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
string (REPLACE "." "\\." CMAKE_REGISTRY_PREFIX_RE "$ENV{HOME}/.cmake")
foreach (F ${MANIFEST}) # skip empty entries, i.e., blank lines
# remove directories only if file was installed inside the installation root
# or the CMake package registration on Unix
if (F MATCHES "^${CMAKE_INSTALL_PREFIX_RE}" OR
(UNIX AND F MATCHES "^${CMAKE_REGISTRY_PREFIX_RE}"))
get_filename_component (D "$ENV{DESTDIR}${F}" PATH)
while (D)
# skip directory if we removed it already
if (NOT EXISTS "${D}" OR NOT IS_DIRECTORY "${D}")
if ("${D}" STREQUAL "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
return () # we are done, the installation root has been removed
endif ()
break ()
endif ()
# skip directory if it is in list of excluded directories
list (FIND EXCLUDE_DIRS "${D}" IDX)
if (NOT IDX EQUAL -1)
break ()
endif ()
# glob files in directory to make sure it is empty
file (GLOB FILES "${D}/*")
if (NOT FILES)
# remove directory
message (STATUS "Uninstalling: ${D}")
execute_process (COMMAND "${CMAKE_COMMAND}" -E remove_directory "${D}" RESULT_VARIABLE RT)
if (NOT RT EQUAL 0)
set (OK FALSE)
message (STATUS "Failed to remove ${D}")
endif ()
endif ()
if ("${D}" STREQUAL "$ENV{DESTDIR}${CMAKE_INSTALL_PREFIX}")
# we reached the root installation direcory
break ()
endif ()
# procede with parent directory
get_filename_component (D "${D}" PATH)
endwhile ()
endif ()
endforeach ()
================================================
FILE: doc/feedback.md
================================================
# Overview
The overall structure for the Feedback Controller is a bit confusing but I will try to lay out the basic idea here.
There are two halves of the Feedback Controller, the GPU portion and the CPU portion.
These are separated into different classes as we need to do different things on the different devices.
The GPU portion just needs to provide feedback control while the CPU portion needs to do that and have the ability to update the feedback controller itself.
# Steps to creating a new FeedbackController
## Create a new FeedbackParams struct
This should be where the necessary data for the CPU portion of the controller resides.
This does not get copied over to the GPU so the struct can have Eigen matrices in it.
For example, this struct has the Q, Q_f, and R matrices for DDP and could have the desired poles for a PID controller.
## Create a new FeedbackState struct
This is the data structure that will contain everything necessary to actually compute feedback on the GPU and CPU.
This struct does get copied to the GPU so try to use float arrays of known sizes to prevent a need to rewrite the copy to GPU code.
It should also inherit from the GPUState struct in `feedback.cuh` as that has a necessary variable SHARED_MEM_SIZE that is used to allocate shared memory on the GPU for the feedback controller to use.
This space would be needed for keeping track of PID integrated errors for each sample of MPPI as an example.
For DDP, this is where the trajectory of feedback gains is stored, and for a PID controller, this would be where you put your P, I, and D gains.
## Create a GPUFeedbackController Class
This should inherit from the GPUFeedbackController template in `feedback.cuh` as that provides a lot of methods already.
You at minimum need to write the `void k(x_act, x_goal, t, theta, control_output)` method to return the feedback control (`control output`) you would expect given the current state (`x_act`), the goal state (`x_goal`), the timestep you are on (`t`), and some potentially scratch space (`theta`).
For DDP, this is ends up being essentially `feedback_gain_traj[t] * (x_act - x_goal)` (no use for `theta`).
This class also has methods you can overwrite for copyToDevice() and copyFromDevice() if you have some fancy GPUFeedback or want to get diagnostic information back from the GPU.
## Create a FeedbackController Class
This should inherit from the FeedbackController template in `feedback.cuh` which requires the GPUFeedback Class and the FeedbackParams struct you made earlier.
There are 3 methods at minimum you need to overwrite for this class.
1. `void initTrackingController()` is where you can do some setup that might not be done in the constructor (DDP uses this to setup the ddp optimizer using the current params)
2. `void computeFeedback(init_state, goal_state_traj, control_traj)` is where you can write how to update your feedback controller.
For DDP, this is where the optimization occurs and the new feedback gains trajectory is put into the DDPFeedbackState struct.
3. `control_array k_(x_act, x_goal, t, fb_state)` is the CPU version of calculating the feedback control.
It has the FeedbackState passed in as there are times that the feedback control needs to be calculated from a different place than the internal FeedbackState (the internal state might be in the middle of updating for example).
The resulting control needs to be the same as the GPU Controller output given the same FeedbackState.
# Conclusion
After creating all of the components, you can then use this controller class as the feedback controller template argument in MPPI, Tube, RMPPI, and it "should" plug in without any problems. The DDP implementation is the only example at the moment so use that as a guiding resource if you get lost.
================================================
FILE: examples/CMakeLists.txt
================================================
# add_executable(cartpole_example cartpole_example.cpp)
# target_include_directories(cartpole_example PUBLIC ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES})
# target_link_libraries(cartpole_example
# cnpy
# cartpole_mppi)
file(GLOB CUDA_TARGET_SRCS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cu)
foreach(T_FILE IN LISTS CUDA_TARGET_SRCS)
# Get filename without extension to use as target name
get_filename_component(T_NAME ${T_FILE} NAME_WE)
add_executable(${T_NAME} ${T_FILE})
target_include_directories(${T_NAME} PUBLIC ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES})
target_link_libraries(${T_NAME}
${MPPI_HEADER_LIBRARY_NAME}
)
# set_target_properties(${T_NAME} PROPERTIES FOLDER test)
endforeach()
================================================
FILE: examples/cartpole_example.cu
================================================
#include <mppi/instantiations/cartpole_mppi/cartpole_mppi.cuh>
#include <iostream>
#include <chrono>
using SAMPLER_T = mppi::sampling_distributions::GaussianDistribution<CartpoleDynamics::DYN_PARAMS_T>;
int main(int argc, char** argv)
{
auto model = new CartpoleDynamics(1.0, 1.0, 1.0);
auto cost = new CartpoleQuadraticCost;
model->control_rngs_->x = -5;
model->control_rngs_->y = 5;
CartpoleQuadraticCostParams new_params;
new_params.cart_position_coeff = 50;
new_params.pole_angle_coeff = 200;
new_params.cart_velocity_coeff = 10;
new_params.pole_angular_velocity_coeff = 1;
new_params.control_cost_coeff[0] = 0;
new_params.terminal_cost_coeff = 0;
new_params.desired_terminal_state[0] = 20;
new_params.desired_terminal_state[1] = 0;
new_params.desired_terminal_state[2] = M_PI;
new_params.desired_terminal_state[3] = 0;
cost->setParams(new_params);
float dt = 0.02;
int max_iter = 1;
float lambda = 0.25;
float alpha = 0.0;
const int num_timesteps = 100;
// Set up Gaussian Distribution
auto sampler_params = SAMPLER_T::SAMPLING_PARAMS_T();
for (int i = 0; i < CartpoleDynamics::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 5.0;
}
auto sampler = new SAMPLER_T(sampler_params);
// Feedback Controller
auto fb_controller = new DDPFeedback<CartpoleDynamics, num_timesteps>(model, dt);
auto CartpoleController =
new VanillaMPPIController<CartpoleDynamics, CartpoleQuadraticCost, DDPFeedback<CartpoleDynamics, num_timesteps>,
num_timesteps, 2048>(model, cost, fb_controller, sampler, dt, max_iter, lambda, alpha);
auto controller_params = CartpoleController->getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 4, 1);
controller_params.cost_rollout_dim_ = dim3(64, 4, 1);
CartpoleController->setParams(controller_params);
CartpoleDynamics::state_array current_state = model->getZeroState();
CartpoleDynamics::state_array next_state = model->getZeroState();
CartpoleDynamics::output_array output = CartpoleDynamics::output_array::Zero();
int time_horizon = 5000;
CartpoleDynamics::state_array xdot = model->getZeroState();
auto time_start = std::chrono::system_clock::now();
for (int i = 0; i < time_horizon; ++i)
{
// Compute the control
CartpoleController->computeControl(current_state, 1);
// Increment the state
CartpoleDynamics::control_array control;
control = CartpoleController->getControlSeq().block(0, 0, CartpoleDynamics::CONTROL_DIM, 1);
model->enforceConstraints(current_state, control);
model->step(current_state, next_state, xdot, control, output, i, dt);
current_state = next_state;
if (i % 50 == 0)
{
printf("Current Time: %f ", i * dt);
printf("Current Baseline Cost: %f ", CartpoleController->getBaselineCost());
model->printState(current_state.data());
// std::cout << control << std::endl;
}
// Slide the controls down before calling the optimizer again
CartpoleController->slideControlSequence(1);
}
auto time_end = std::chrono::system_clock::now();
auto diff = std::chrono::duration<double, std::milli>(time_end - time_start);
printf("The elapsed time is: %f milliseconds\n", diff.count());
// std::cout << "The current control at timestep " << i << " is: " << CartpoleController.get_control_seq()[i] <<
// std::endl;
// cost->freeCudaMem();
delete (CartpoleController);
delete (cost);
delete (model);
delete (fb_controller);
delete sampler;
return 0;
}
================================================
FILE: examples/double_integrator_CORL2020.cu
================================================
#include <mppi/dynamics/double_integrator/di_dynamics.cuh>
#include <mppi/cost_functions/double_integrator/double_integrator_circle_cost.cuh>
#include <mppi/cost_functions/double_integrator/double_integrator_robust_cost.cuh>
#include <mppi/controllers/MPPI/mppi_controller.cuh>
#include <mppi/controllers/Tube-MPPI/tube_mppi_controller.cuh>
#include <mppi/controllers/R-MPPI/robust_mppi_controller.cuh>
#include <mppi/feedback_controllers/DDP/ddp.cuh>
#include <cnpy.h>
#include <random> // Used to generate random noise for control trajectories
bool tubeFailure(float* s)
{
float inner_path_radius2 = 1.675 * 1.675;
float outer_path_radius2 = 2.325 * 2.325;
float radial_position = s[0] * s[0] + s[1] * s[1];
if ((radial_position < inner_path_radius2) || (radial_position > outer_path_radius2))
{
return true;
}
else
{
return false;
}
}
using Dyn = DoubleIntegratorDynamics;
using SCost = DoubleIntegratorCircleCost;
using RCost = DoubleIntegratorRobustCost;
const int num_timesteps = 50; // Optimization time horizon
const int total_time_horizon = 5000;
using Feedback = DDPFeedback<Dyn, num_timesteps>;
using Sampler = mppi::sampling_distributions::GaussianDistribution<Dyn::DYN_PARAMS_T>;
// Problem setup
const float dt = 0.02; // Timestep of dynamics propagation
const int max_iter = 1; // Maximum running iterations of optimization
const float lambda = 2; // Learning rate parameter
const float alpha = 0.0;
typedef Eigen::Matrix<float, Dyn::STATE_DIM, num_timesteps> state_trajectory;
void saveTraj(const Eigen::Ref<const state_trajectory>& traj, int t, std::vector<float>& vec)
{
for (int i = 0; i < num_timesteps; i++)
{
for (int j = 0; j < Dyn::STATE_DIM; j++)
{
vec[t * num_timesteps * Dyn::STATE_DIM + i * Dyn::STATE_DIM + j] = traj(j, i);
}
}
}
void saveState(const Eigen::Ref<const Dyn::state_array>& state, int t, std::vector<float>& vec)
{
for (int j = 0; j < Dyn::STATE_DIM; j++)
{
vec[t * Dyn::STATE_DIM + j] = state(j);
}
}
void runVanilla(const Eigen::Ref<const Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon>>& noise)
{
// Set the initial state
Dyn::state_array x;
x << 2, 0, 0, 1;
Dyn::state_array xdot;
// control variance
Sampler::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < Dyn::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 1;
}
// Save actual trajectories, nominal_trajectory, free energy
std::vector<float> van_trajectory(Dyn::STATE_DIM * total_time_horizon, 0);
std::vector<float> van_nominal_traj(Dyn::STATE_DIM * num_timesteps * total_time_horizon, 0);
std::vector<float> van_free_energy(total_time_horizon, 0);
// Initialize the controllers
Dyn model;
SCost cost;
Sampler sampler(sampler_params);
// DDP cost parameters
Feedback fb_controller(&model, dt);
auto fb_params = fb_controller.getParams();
fb_params.Q.diagonal() << 500, 500, 100, 100;
fb_controller.setParams(fb_params);
auto controller = VanillaMPPIController<Dyn, SCost, Feedback, num_timesteps, 1024, Sampler>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
controller.initFeedback();
// Start the loop
for (int t = 0; t < total_time_horizon; ++t)
{
/********************** Vanilla **********************/
// Compute the control
controller.computeControl(x, 1);
// Compute the feedback gains
controller.computeFeedback(x);
// Propagate the feedback trajectory
controller.computeFeedbackPropagatedStateSeq();
auto nominal_trajectory = controller.getTargetStateSeq();
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
// Save everything
saveState(x, t, van_trajectory);
saveTraj(nominal_trajectory, t, van_nominal_traj);
van_free_energy[t] = fe_stat.real_sys.freeEnergyMean;
// Get the open loop control
DoubleIntegratorDynamics::control_array current_control = nominal_control.col(0);
// Apply the feedback given the current state
Dyn::control_array fb_control = controller.getFeedbackControl(x, nominal_trajectory.col(0), 0);
current_control += fb_control;
// Propagate the state forward
model.computeDynamics(x, current_control, xdot);
model.updateState(x, xdot, dt);
// Add disturbance
x += noise.col(t) * sqrt(model.getParams().system_noise) * dt;
// Slide the control sequence
controller.slideControlSequence(1);
}
/************* Save CNPY *********************/
cnpy::npy_save("vanilla_state_trajectory.npy", van_trajectory.data(),
{ total_time_horizon, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("vanilla_nominal_trajectory.npy", van_nominal_traj.data(),
{ total_time_horizon, num_timesteps, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("vanilla_free_energy.npy", van_free_energy.data(), { total_time_horizon }, "w");
}
void runVanillaLarge(const Eigen::Ref<const Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon>>& noise)
{
// Set the initial state
DoubleIntegratorDynamics::state_array x;
x << 2, 0, 0, 1;
DoubleIntegratorDynamics::state_array xdot;
// control variance
Sampler::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < Dyn::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 1;
}
// Save actual trajectories, nominal_trajectory, free energy
std::vector<float> van_large_trajectory(Dyn::STATE_DIM * total_time_horizon, 0);
std::vector<float> van_large_nominal_traj(Dyn::STATE_DIM * num_timesteps * total_time_horizon, 0);
std::vector<float> van_large_free_energy(total_time_horizon, 0);
// Initialize the controllers
Dyn model(100);
SCost cost;
Sampler sampler(sampler_params);
// DDP cost parameters
Feedback fb_controller(&model, dt);
auto fb_params = fb_controller.getParams();
fb_params.Q.diagonal() << 500, 500, 100, 100;
fb_controller.setParams(fb_params);
auto controller = VanillaMPPIController<Dyn, SCost, Feedback, num_timesteps, 1024, Sampler>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
controller.initFeedback();
// Start the loop
for (int t = 0; t < total_time_horizon; ++t)
{
/********************** Vanilla Large **********************/
// Compute the control
controller.computeControl(x, 1);
// Compute the feedback gains
controller.computeFeedback(x);
// Propagate the feedback trajectory
controller.computeFeedbackPropagatedStateSeq();
auto nominal_trajectory = controller.getTargetStateSeq();
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
// Save everything
saveState(x, t, van_large_trajectory);
saveTraj(nominal_trajectory, t, van_large_nominal_traj);
van_large_free_energy[t] = fe_stat.real_sys.freeEnergyMean;
// Get the open loop control
DoubleIntegratorDynamics::control_array current_control = nominal_control.col(0);
// Apply the feedback given the current state
Dyn::control_array fb_control = controller.getFeedbackControl(x, nominal_trajectory.col(0), 0);
current_control += fb_control;
// Propagate the state forward
model.computeDynamics(x, current_control, xdot);
model.updateState(x, xdot, dt);
// Add disturbance
x += noise.col(t) * sqrt(model.getParams().system_noise) * dt;
// Slide the control sequence
controller.slideControlSequence(1);
}
/************* Save CNPY *********************/
cnpy::npy_save("vanilla_large_state_trajectory.npy", van_large_trajectory.data(),
{ total_time_horizon, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("vanilla_large_nominal_trajectory.npy", van_large_nominal_traj.data(),
{ total_time_horizon, num_timesteps, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("vanilla_large_free_energy.npy", van_large_free_energy.data(), { total_time_horizon }, "w");
}
void runVanillaLargeRC(const Eigen::Ref<const Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon>>& noise)
{
// Set the initial state
DoubleIntegratorDynamics::state_array x;
x << 2, 0, 0, 1;
DoubleIntegratorDynamics::state_array xdot;
// control variance
Sampler::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < Dyn::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 1;
}
// Save actual trajectories, nominal_trajectory, free energy
std::vector<float> van_large_trajectory(Dyn::STATE_DIM * total_time_horizon, 0);
std::vector<float> van_large_nominal_traj(Dyn::STATE_DIM * num_timesteps * total_time_horizon, 0);
std::vector<float> van_large_free_energy(total_time_horizon, 0);
// Initialize the controllers
Dyn model(100);
RCost cost;
Sampler sampler(sampler_params);
auto params = cost.getParams();
params.crash_cost = 100;
cost.setParams(params);
// DDP cost parameters
Feedback fb_controller(&model, dt);
auto fb_params = fb_controller.getParams();
fb_params.Q.diagonal() << 500, 500, 100, 100;
fb_controller.setParams(fb_params);
auto controller = VanillaMPPIController<Dyn, RCost, Feedback, num_timesteps, 1024, Sampler>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
controller.initFeedback();
// Start the loop
for (int t = 0; t < total_time_horizon; ++t)
{
/********************** Vanilla Large **********************/
// Compute the control
controller.computeControl(x, 1);
// Compute the feedback gains
controller.computeFeedback(x);
// Propagate the feedback trajectory
controller.computeFeedbackPropagatedStateSeq();
auto nominal_trajectory = controller.getTargetStateSeq();
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
// Save everything
saveState(x, t, van_large_trajectory);
saveTraj(nominal_trajectory, t, van_large_nominal_traj);
van_large_free_energy[t] = fe_stat.real_sys.freeEnergyMean;
// Get the open loop control
DoubleIntegratorDynamics::control_array current_control = nominal_control.col(0);
// Apply the feedback given the current state
Dyn::control_array fb_control = controller.getFeedbackControl(x, nominal_trajectory.col(0), 0);
current_control += fb_control;
// Propagate the state forward
model.computeDynamics(x, current_control, xdot);
model.updateState(x, xdot, dt);
// Add disturbance
x += noise.col(t) * sqrt(model.getParams().system_noise) * dt;
// Slide the control sequence
controller.slideControlSequence(1);
}
/************* Save CNPY *********************/
cnpy::npy_save("vanilla_large_robust_state_trajectory.npy", van_large_trajectory.data(),
{ total_time_horizon, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("vanilla_large_robust_nominal_trajectory.npy", van_large_nominal_traj.data(),
{ total_time_horizon, num_timesteps, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("vanilla_large_robust_free_energy.npy", van_large_free_energy.data(), { total_time_horizon }, "w");
}
void runTube(const Eigen::Ref<const Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon>>& noise)
{
// Set the initial state
DoubleIntegratorDynamics::state_array x;
x << 2, 0, 0, 1;
DoubleIntegratorDynamics::state_array xdot;
// control variance
Sampler::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < Dyn::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 1;
}
// Save actual trajectories, nominal_trajectory, free energy
std::vector<float> tube_trajectory(Dyn::STATE_DIM * total_time_horizon, 0);
std::vector<float> tube_nominal_traj(Dyn::STATE_DIM * num_timesteps * total_time_horizon, 0);
std::vector<float> tube_nominal_free_energy(total_time_horizon, 0);
std::vector<float> tube_real_free_energy(total_time_horizon, 0);
std::vector<float> tube_nominal_state_used(total_time_horizon, 0);
// Initialize the controllers
Dyn model(100);
SCost cost;
Sampler sampler(sampler_params);
// DDP cost parameters
Feedback fb_controller(&model, dt);
auto fb_params = fb_controller.getParams();
fb_params.Q.diagonal() << 500, 500, 100, 100;
fb_controller.setParams(fb_params);
auto controller = TubeMPPIController<Dyn, SCost, Feedback, num_timesteps, 1024, Sampler>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
controller.setNominalThreshold(20);
// Start the loop
for (int t = 0; t < total_time_horizon; ++t)
{
/********************** Tube **********************/
// Compute the control
controller.computeControl(x, 1);
// Compute the feedback gains
controller.computeFeedback(x);
// Propagate the feedback trajectory
controller.computeFeedbackPropagatedStateSeq();
auto nominal_trajectory = controller.getTargetStateSeq();
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
// Save everything
saveState(x, t, tube_trajectory);
saveTraj(nominal_trajectory, t, tube_nominal_traj);
tube_nominal_free_energy[t] = fe_stat.nominal_sys.freeEnergyMean;
tube_real_free_energy[t] = fe_stat.real_sys.freeEnergyMean;
tube_nominal_state_used[t] = fe_stat.nominal_state_used;
// Get the open loop control
DoubleIntegratorDynamics::control_array current_control = nominal_control.col(0);
// Apply the feedback given the current state
Dyn::control_array fb_control = controller.getFeedbackControl(x, nominal_trajectory.col(0), 0);
current_control += fb_control;
// Propagate the state forward
model.computeDynamics(x, current_control, xdot);
model.updateState(x, xdot, dt);
controller.updateNominalState(current_control);
// Add disturbance
x += noise.col(t) * sqrt(model.getParams().system_noise) * dt;
// Slide the control sequence
controller.slideControlSequence(1);
}
/************* Save CNPY *********************/
cnpy::npy_save("tube_state_trajectory.npy", tube_trajectory.data(),
{ total_time_horizon, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("tube_nominal_trajectory.npy", tube_nominal_traj.data(),
{ total_time_horizon, num_timesteps, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("tube_nominal_free_energy.npy", tube_nominal_free_energy.data(), { total_time_horizon }, "w");
cnpy::npy_save("tube_real_free_energy.npy", tube_real_free_energy.data(), { total_time_horizon }, "w");
cnpy::npy_save("tube_nominal_state_used.npy", tube_nominal_state_used.data(), { total_time_horizon }, "w");
}
void runTubeRC(const Eigen::Ref<const Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon>>& noise)
{
// Set the initial state
DoubleIntegratorDynamics::state_array x;
x << 2, 0, 0, 1;
DoubleIntegratorDynamics::state_array xdot;
// control variance
Sampler::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < Dyn::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 1;
}
// Save actual trajectories, nominal_trajectory, free energy
std::vector<float> tube_trajectory(Dyn::STATE_DIM * total_time_horizon, 0);
std::vector<float> tube_nominal_traj(Dyn::STATE_DIM * num_timesteps * total_time_horizon, 0);
std::vector<float> tube_nominal_free_energy(total_time_horizon, 0);
std::vector<float> tube_real_free_energy(total_time_horizon, 0);
std::vector<float> tube_nominal_state_used(total_time_horizon, 0);
// Initialize the controllers
Dyn model(100);
RCost cost;
Sampler sampler(sampler_params);
auto params = cost.getParams();
params.crash_cost = 100;
cost.setParams(params);
// DDP cost parameters
Feedback fb_controller(&model, dt);
auto fb_params = fb_controller.getParams();
fb_params.Q.diagonal() << 500, 500, 100, 100;
fb_controller.setParams(fb_params);
auto controller = TubeMPPIController<Dyn, RCost, Feedback, num_timesteps, 1024>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
controller.setNominalThreshold(2);
// Start the loop
for (int t = 0; t < total_time_horizon; ++t)
{
/********************** Tube **********************/
// Compute the control
controller.computeControl(x, 1);
// Compute the feedback gains
controller.computeFeedback(x);
// Propagate the feedback trajectory
controller.computeFeedbackPropagatedStateSeq();
auto nominal_trajectory = controller.getTargetStateSeq();
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
// Save everything
saveState(x, t, tube_trajectory);
saveTraj(nominal_trajectory, t, tube_nominal_traj);
tube_nominal_free_energy[t] = fe_stat.nominal_sys.freeEnergyMean;
tube_real_free_energy[t] = fe_stat.real_sys.freeEnergyMean;
tube_nominal_state_used[t] = fe_stat.nominal_state_used;
// Get the open loop control
DoubleIntegratorDynamics::control_array current_control = nominal_control.col(0);
// Apply the feedback given the current state
Dyn::control_array fb_control = controller.getFeedbackControl(x, nominal_trajectory.col(0), 0);
current_control += fb_control;
// Propagate the state forward
model.computeDynamics(x, current_control, xdot);
model.updateState(x, xdot, dt);
controller.updateNominalState(current_control);
// Add disturbance
x += noise.col(t) * sqrt(model.getParams().system_noise) * dt;
// Slide the control sequence
controller.slideControlSequence(1);
}
/************* Save CNPY *********************/
cnpy::npy_save("tube_robust_state_trajectory.npy", tube_trajectory.data(),
{ total_time_horizon, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("tube_robust_nominal_trajectory.npy", tube_nominal_traj.data(),
{ total_time_horizon, num_timesteps, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("tube_robust_nominal_free_energy.npy", tube_nominal_free_energy.data(), { total_time_horizon }, "w");
cnpy::npy_save("tube_robust_real_free_energy.npy", tube_real_free_energy.data(), { total_time_horizon }, "w");
cnpy::npy_save("tube_robust_nominal_state_used.npy", tube_nominal_state_used.data(), { total_time_horizon }, "w");
}
void runRobustSc(const Eigen::Ref<const Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon>>& noise)
{
// Set the initial state
DoubleIntegratorDynamics::state_array x;
x << 2, 0, 0, 1;
DoubleIntegratorDynamics::state_array xdot;
// control variance
Sampler::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < Dyn::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 1;
}
// Save actual trajectories, nominal_trajectory, free energy
std::vector<float> robust_sc_trajectory(Dyn::STATE_DIM * total_time_horizon, 0);
std::vector<float> robust_sc_nominal_traj(Dyn::STATE_DIM * num_timesteps * total_time_horizon, 0);
std::vector<float> robust_sc_nominal_free_energy(total_time_horizon, 0);
std::vector<float> robust_sc_real_free_energy(total_time_horizon, 0);
std::vector<float> robust_sc_nominal_free_energy_bound(total_time_horizon, 0);
std::vector<float> robust_sc_real_free_energy_bound(total_time_horizon, 0);
std::vector<float> robust_sc_real_free_energy_growth_bound(total_time_horizon, 0);
std::vector<float> robust_sc_nominal_state_used(total_time_horizon, 0);
// Initialize the controllers
Dyn model(100);
SCost cost;
Sampler sampler(sampler_params);
// DDP cost parameters
Feedback fb_controller(&model, dt);
auto fb_params = fb_controller.getParams();
fb_params.Q.diagonal() << 500, 500, 100, 100;
fb_controller.setParams(fb_params);
// Value function threshold
float value_function_threshold = 20.0;
auto controller = RobustMPPIController<Dyn, SCost, Feedback, num_timesteps, 1024, Sampler>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha, value_function_threshold);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
// Start the loop
for (int t = 0; t < total_time_horizon; ++t)
{
/********************** Vanilla **********************/
// Compute the control
controller.updateImportanceSamplingControl(x, 1);
controller.computeControl(x, 1);
// Compute the feedback gains
controller.computeFeedback(x);
// Propagate the feedback trajectory
controller.computeFeedbackPropagatedStateSeq();
auto nominal_trajectory = controller.getTargetStateSeq();
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
// Save everything
saveState(x, t, robust_sc_trajectory);
saveTraj(nominal_trajectory, t, robust_sc_nominal_traj);
robust_sc_nominal_free_energy[t] = fe_stat.nominal_sys.freeEnergyMean;
robust_sc_real_free_energy[t] = fe_stat.real_sys.freeEnergyMean;
robust_sc_nominal_free_energy_bound[t] =
value_function_threshold + 2 * fe_stat.nominal_sys.freeEnergyModifiedVariance;
robust_sc_real_free_energy_bound[t] = 0;
robust_sc_real_free_energy_growth_bound[t] = 0;
robust_sc_nominal_state_used[t] = fe_stat.nominal_state_used;
// Get the open loop control
DoubleIntegratorDynamics::control_array current_control = nominal_control.col(0);
// Apply the feedback given the current state
Dyn::control_array fb_control = controller.getFeedbackControl(x, nominal_trajectory.col(0), 0);
current_control += fb_control;
// Propagate the state forward
model.computeDynamics(x, current_control, xdot);
model.updateState(x, xdot, dt);
// Add disturbance
x += noise.col(t) * sqrt(model.getParams().system_noise) * dt;
// Slide the control sequence
controller.slideControlSequence(1);
}
/************* Save CNPY *********************/
cnpy::npy_save("robust_sc_state_trajectory.npy", robust_sc_trajectory.data(),
{ total_time_horizon, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("robust_sc_nominal_trajectory.npy", robust_sc_nominal_traj.data(),
{ total_time_horizon, num_timesteps, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("robust_sc_nominal_free_energy.npy", robust_sc_nominal_free_energy.data(), { total_time_horizon },
"w");
cnpy::npy_save("robust_sc_real_free_energy.npy", robust_sc_real_free_energy.data(), { total_time_horizon }, "w");
cnpy::npy_save("robust_sc_nominal_state_used.npy", robust_sc_nominal_state_used.data(), { total_time_horizon }, "w");
cnpy::npy_save("robust_sc_real_free_energy_bound.npy", robust_sc_nominal_free_energy_bound.data(),
{ total_time_horizon }, "w");
cnpy::npy_save("robust_sc_nominal_free_energy_bound.npy", robust_sc_real_free_energy_bound.data(),
{ total_time_horizon }, "w");
cnpy::npy_save("robust_sc_real_free_energy_growth_bound.npy", robust_sc_real_free_energy_growth_bound.data(),
{ total_time_horizon }, "w");
}
void runRobustRc(const Eigen::Ref<const Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon>>& noise)
{
// Set the initial state
DoubleIntegratorDynamics::state_array x;
x << 2, 0, 0, 1;
DoubleIntegratorDynamics::state_array xdot;
// control variance
Sampler::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < Dyn::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 1;
}
// Save actual trajectories, nominal_trajectory, free energy
std::vector<float> robust_rc_trajectory(Dyn::STATE_DIM * total_time_horizon, 0);
std::vector<float> robust_rc_nominal_traj(Dyn::STATE_DIM * num_timesteps * total_time_horizon, 0);
std::vector<float> robust_rc_nominal_free_energy(total_time_horizon, 0);
std::vector<float> robust_rc_real_free_energy(total_time_horizon, 0);
std::vector<float> robust_rc_nominal_free_energy_bound(total_time_horizon, 0);
std::vector<float> robust_rc_real_free_energy_bound(total_time_horizon, 0);
std::vector<float> robust_rc_real_free_energy_growth_bound(total_time_horizon, 0);
std::vector<float> robust_rc_nominal_free_energy_growth(total_time_horizon, 0);
std::vector<float> robust_rc_real_free_energy_growth(total_time_horizon, 0);
std::vector<float> robust_rc_nominal_state_used(total_time_horizon, 0);
// Initialize the controllers
Dyn model(100);
RCost cost;
auto params = cost.getParams();
params.crash_cost = 100;
cost.setParams(params);
Sampler sampler(sampler_params);
// DDP cost parameters
Feedback fb_controller(&model, dt);
auto fb_params = fb_controller.getParams();
fb_params.Q.diagonal() << 500, 500, 100, 100;
fb_controller.setParams(fb_params);
// Value function threshold
float value_function_threshold = 20.0;
auto controller = RobustMPPIController<Dyn, RCost, Feedback, num_timesteps, 1024, Sampler>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha, value_function_threshold);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
// Start the loop
for (int t = 0; t < total_time_horizon; ++t)
{
/********************** Robust Robust Cost **********************/
// Compute the control
controller.updateImportanceSamplingControl(x, 1);
controller.computeControl(x, 1);
// Compute the feedback gains
controller.computeFeedback(x);
// Propagate the feedback trajectory
controller.computeFeedbackPropagatedStateSeq();
auto nominal_trajectory = controller.getTargetStateSeq();
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
// Save everything
saveState(x, t, robust_rc_trajectory);
saveTraj(nominal_trajectory, t, robust_rc_nominal_traj);
robust_rc_nominal_free_energy[t] = fe_stat.nominal_sys.freeEnergyMean;
robust_rc_real_free_energy[t] = fe_stat.real_sys.freeEnergyMean;
robust_rc_nominal_free_energy_bound[t] =
value_function_threshold + 2 * fe_stat.nominal_sys.freeEnergyModifiedVariance;
robust_rc_real_free_energy_bound[t] = fe_stat.nominal_sys.freeEnergyMean +
cost.getLipshitzConstantCost() * 1 * (x - nominal_trajectory.col(0)).norm();
robust_rc_real_free_energy_growth_bound[t] = (value_function_threshold - fe_stat.nominal_sys.freeEnergyMean) +
cost.getLipshitzConstantCost() * 8 * 20 * controller.computeDF() +
2 * fe_stat.nominal_sys.freeEnergyModifiedVariance;
robust_rc_nominal_free_energy_growth[t] = fe_stat.nominal_sys.increase;
robust_rc_real_free_energy_growth[t] = fe_stat.real_sys.increase;
robust_rc_nominal_state_used[t] = fe_stat.nominal_state_used;
// Get the open loop control
DoubleIntegratorDynamics::control_array current_control = nominal_control.col(0);
// Apply the feedback given the current state
Dyn::control_array fb_control = controller.getFeedbackControl(x, nominal_trajectory.col(0), 0);
current_control += fb_control;
// Propagate the state forward
model.computeDynamics(x, current_control, xdot);
model.updateState(x, xdot, dt);
// Add disturbance
x += noise.col(t) * sqrt(model.getParams().system_noise) * dt;
// Slide the control sequence
controller.slideControlSequence(1);
}
/************* Save CNPY *********************/
cnpy::npy_save("robust_rc_state_trajectory.npy", robust_rc_trajectory.data(),
{ total_time_horizon, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("robust_rc_nominal_trajectory.npy", robust_rc_nominal_traj.data(),
{ total_time_horizon, num_timesteps, DoubleIntegratorDynamics::STATE_DIM }, "w");
cnpy::npy_save("robust_rc_nominal_free_energy.npy", robust_rc_nominal_free_energy.data(), { total_time_horizon },
"w");
cnpy::npy_save("robust_rc_real_free_energy.npy", robust_rc_real_free_energy.data(), { total_time_horizon }, "w");
cnpy::npy_save("robust_rc_nominal_state_used.npy", robust_rc_nominal_state_used.data(), { total_time_horizon }, "w");
cnpy::npy_save("robust_rc_real_free_energy_bound.npy", robust_rc_real_free_energy_bound.data(),
{ total_time_horizon }, "w");
cnpy::npy_save("robust_rc_nominal_free_energy_bound.npy", robust_rc_nominal_free_energy_bound.data(),
{ total_time_horizon }, "w");
cnpy::npy_save("robust_rc_real_free_energy_growth_bound.npy", robust_rc_real_free_energy_growth_bound.data(),
{ total_time_horizon }, "w");
cnpy::npy_save("robust_rc_real_free_energy_growth.npy", robust_rc_real_free_energy_growth.data(),
{ total_time_horizon }, "w");
cnpy::npy_save("robust_rc_nominal_free_energy_growth.npy", robust_rc_nominal_free_energy_growth.data(),
{ total_time_horizon }, "w");
}
int main()
{
// Run the double integrator example on all the controllers with the SAME noise 20 times.
// Create a random number generator
// Random number generator for system noise
std::mt19937 gen; // Standard mersenne_twister_engine which will be seeded
std::normal_distribution<float> normal_distribution;
gen.seed(7); // Seed the 7, so everyone gets the same noise
normal_distribution = std::normal_distribution<float>(0, 1);
Eigen::Matrix<float, Dyn::STATE_DIM, total_time_horizon> universal_noise;
universal_noise.setZero();
// Create the noise for all systems
for (int t = 0; t < total_time_horizon; ++t)
{
for (int i = 2; i < 4; ++i)
{
universal_noise(i, t) = normal_distribution(gen);
}
}
runVanilla(universal_noise);
std::cout << "Finished Vanilla" << std::endl;
runVanillaLarge(universal_noise);
std::cout << "Finished Vanilla Large" << std::endl;
runVanillaLargeRC(universal_noise);
std::cout << "Finished Vanilla Large with Robust Cost" << std::endl;
runTube(universal_noise);
std::cout << "Finished Tube with Standard Cost" << std::endl;
runTubeRC(universal_noise);
std::cout << "Finished Tube with Robust Cost" << std::endl;
runRobustSc(universal_noise);
std::cout << "Finished RMPPI with Standard Cost" << std::endl;
runRobustRc(universal_noise);
std::cout << "Finished RMPPI with Robust Cost" << std::endl;
return 0;
}
================================================
FILE: examples/double_integrator_example.cu
================================================
#include <mppi/dynamics/double_integrator/di_dynamics.cuh>
#include <mppi/cost_functions/quadratic_cost/quadratic_cost.cuh>
#include <mppi/controllers/MPPI/mppi_controller.cuh>
#include <mppi/feedback_controllers/DDP/ddp.cuh>
#include <mppi/sampling_distributions/colored_noise/colored_noise.cuh>
#include <iomanip>
// #include <sstream>
#define USE_COLORED_NOISE
const int TIMESTEPS = 65;
const int NUM_ROLLOUTS = 128;
using DYN = DoubleIntegratorDynamics;
using COST = QuadraticCost<DYN>;
using FB_CONTROLLER = DDPFeedback<DYN, TIMESTEPS>;
#ifdef USE_COLORED_NOISE
using SAMPLER = mppi::sampling_distributions::ColoredNoiseDistribution<DYN::DYN_PARAMS_T>;
#else
using SAMPLER = mppi::sampling_distributions::GaussianDistribution<DYN::DYN_PARAMS_T>;
#endif
int main()
{
float dt = 0.015;
// Set the initial state
DYN::state_array x;
x << -9, -9, 0.1, 0.1;
DYN::state_array xdot;
SAMPLER::SAMPLING_PARAMS_T sampler_params;
for (int i = 0; i < DYN::CONTROL_DIM; i++)
{
sampler_params.std_dev[i] = 0.5;
#ifdef USE_COLORED_NOISE
sampler_params.exponents[i] = 1.0;
#endif
}
SAMPLER sampler(sampler_params);
// Create the dynamics, cost function, and feedback controller
DYN model;
COST cost;
FB_CONTROLLER fb_controller = FB_CONTROLLER(&model, dt);
auto cost_params = cost.getParams();
// Set up cost function
DYN::state_array x_goal;
x_goal << -4, -4, 0, 0;
DYN::state_array q_coeffs;
q_coeffs << 5, 5, 0.5, 0.5;
for (int i = 0; i < DYN::STATE_DIM; i++)
{
cost_params.s_coeffs[i] = q_coeffs[i];
cost_params.s_goal[i] = x_goal[i];
}
cost.setParams(cost_params);
// Create MPPI Controller
float lambda = 1;
float alpha = 1.0;
int max_iter = 1;
int total_time_horizon = 300;
auto controller = VanillaMPPIController<DYN, COST, FB_CONTROLLER, TIMESTEPS, NUM_ROLLOUTS, SAMPLER>(
&model, &cost, &fb_controller, &sampler, dt, max_iter, lambda, alpha);
auto controller_params = controller.getParams();
controller_params.dynamics_rollout_dim_ = dim3(64, 1, 1);
controller_params.cost_rollout_dim_ = dim3(64, 1, 1);
controller.setParams(controller_params);
/********************** Vanilla MPPI **********************/
float cumulative_cost = 0;
int crash = 0;
for (int t = 0; t < total_time_horizon; ++t)
{
// Compute the control
controller.computeControl(x, 1);
auto nominal_control = controller.getControlSeq();
auto fe_stat = controller.getFreeEnergyStatistics();
DYN::control_array current_control = nominal_control.col(0);
// Propagate dynamics forward
DYN::output_array y;
DYN::state_array x_next;
// model.computeDynamics(x, current_control, xdot);
// model.updateState(x, xdot, dt);
model.step(x, x_next, xdot, current_control, y, t, dt);
x = x_next;
if (t % 10 == 0)
{
std::cout << "T: " << std::fixed << std::setprecision(3) << t * dt;
// << "s Free Energy: " << fe_stat.real_sys.freeEnergyMean
// << " +- " << fe_stat.real_sys.freeEnergyVariance << std::endl;
std::cout << " X: " << x.transpose() << std::endl;
}
// Slide the control sequence
controller.slideControlSequence(1);
cumulative_cost += cost.computeRunningCost(y, current_control, t, &crash);
}
std::cout << "Total Cost: " << cumulative_cost << std::endl;
return 0;
}
================================================
FILE: include/mppi/controllers/ColoredMPPI/colored_mppi_controller.cu
================================================
#include <mppi/controllers/ColoredMPPI/colored_mppi_controller.cuh>
#include <mppi/core/mppi_common.cuh>
#include <algorithm>
#include <iostream>
#include <mppi/sampling_distributions/colored_noise/colored_noise.cuh>
#define ColoredMPPI_TEMPLATE \
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, class SAMPLING_T, \
class PARAMS_T>
#define ColoredMPPI ColoredMPPIController<DYN_T, COST_T, FB_T, MAX_TIMESTEPS, NUM_ROLLOUTS, SAMPLING_T, PARAMS_T>
ColoredMPPI_TEMPLATE ColoredMPPI::ColoredMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller,
SAMPLING_T* sampler, float dt, int max_iter, float lambda,
float alpha, int num_timesteps,
const Eigen::Ref<const control_trajectory>& init_control_traj,
cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, dt, max_iter, lambda, alpha, num_timesteps, init_control_traj,
stream)
{
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// Copy the noise std_dev to the device
// this->copyControlStdDevToDevice();
chooseAppropriateKernel();
}
ColoredMPPI_TEMPLATE ColoredMPPI::ColoredMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller,
SAMPLING_T* sampler, PARAMS_T& params, cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, params, stream)
{
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// // Copy the noise std_dev to the device
// this->copyControlStdDevToDevice();
chooseAppropriateKernel();
}
ColoredMPPI_TEMPLATE void ColoredMPPI::chooseAppropriateKernel()
{
cudaDeviceProp deviceProp;
HANDLE_ERROR(cudaGetDeviceProperties(&deviceProp, 0));
unsigned single_kernel_byte_size = mppi::kernels::calcRolloutCombinedKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, this->params_.dynamics_rollout_dim_);
unsigned split_dyn_kernel_byte_size = mppi::kernels::calcRolloutDynamicsKernelSharedMemSize(
this->model_, this->sampler_, this->params_.dynamics_rollout_dim_);
unsigned split_cost_kernel_byte_size =
mppi::kernels::calcRolloutCostKernelSharedMemSize(this->cost_, this->sampler_, this->params_.cost_rollout_dim_);
unsigned vis_single_kernel_byte_size = mppi::kernels::calcVisualizeKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, this->getNumTimesteps(), this->params_.visualize_dim_);
bool too_much_mem_single_kernel = single_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool too_much_mem_vis_kernel = vis_single_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool too_much_mem_split_kernel = split_dyn_kernel_byte_size > deviceProp.sharedMemPerBlock;
too_much_mem_split_kernel = too_much_mem_split_kernel || split_cost_kernel_byte_size > deviceProp.sharedMemPerBlock;
too_much_mem_single_kernel = too_much_mem_single_kernel || too_much_mem_vis_kernel;
if (too_much_mem_split_kernel && too_much_mem_single_kernel)
{
std::string error_msg =
"There is not enough shared memory on the GPU for either rollout kernel option. The combined rollout kernel "
"takes " +
std::to_string(single_kernel_byte_size) + " bytes, the cost rollout kernel takes " +
std::to_string(split_cost_kernel_byte_size) + " bytes, the dynamics rollout kernel takes " +
std::to_string(split_dyn_kernel_byte_size) + " bytes, the combined visualization kernel takes " +
std::to_string(vis_single_kernel_byte_size) + " bytes, and the max is " +
std::to_string(deviceProp.sharedMemPerBlock) +
" bytes. Considering lowering the corresponding thread block sizes.";
throw std::runtime_error(error_msg);
}
else if (too_much_mem_single_kernel)
{
this->setKernelChoice(kernelType::USE_SPLIT_KERNELS);
return;
}
else if (too_much_mem_split_kernel)
{
this->setKernelChoice(kernelType::USE_SINGLE_KERNEL);
return;
}
// Send the nominal control to the device
this->copyNominalControlToDevice(false);
state_array zero_state = this->model_->getZeroState();
// Send zero state to the device
HANDLE_ERROR(cudaMemcpyAsync(this->initial_state_d_, zero_state.data(), DYN_T::STATE_DIM * sizeof(float),
cudaMemcpyHostToDevice, this->stream_));
// Generate noise data
this->sampler_->generateSamples(1, 0, this->gen_, true);
float single_kernel_time_ms = std::numeric_limits<float>::infinity();
float split_kernel_time_ms = std::numeric_limits<float>::infinity();
// Evaluate each kernel that is applicable
auto start_single_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !too_much_mem_single_kernel; i++)
{
mppi::kernels::launchRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->stream_, true);
}
auto end_single_kernel_time = std::chrono::steady_clock::now();
auto start_split_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !too_much_mem_split_kernel; i++)
{
mppi::kernels::launchSplitRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->output_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->params_.cost_rollout_dim_, this->stream_, true);
}
auto end_split_kernel_time = std::chrono::steady_clock::now();
// calc times
if (!too_much_mem_single_kernel)
{
single_kernel_time_ms = mppi::math::timeDiffms(end_single_kernel_time, start_single_kernel_time);
}
if (!too_much_mem_split_kernel)
{
split_kernel_time_ms = mppi::math::timeDiffms(end_split_kernel_time, start_split_kernel_time);
}
std::string kernel_choice = "";
if (split_kernel_time_ms < single_kernel_time_ms)
{
this->setKernelChoice(kernelType::USE_SPLIT_KERNELS);
kernel_choice = "split ";
}
else
{
this->setKernelChoice(kernelType::USE_SINGLE_KERNEL);
kernel_choice = "single";
}
this->logger_->info("Choosing %s kernel based on split taking %f ms and single taking %f ms after %d iterations\n",
kernel_choice.c_str(), split_kernel_time_ms, single_kernel_time_ms,
this->getNumKernelEvaluations());
}
ColoredMPPI_TEMPLATE ColoredMPPI::~ColoredMPPIController()
{
// all implemented in standard controller
}
ColoredMPPI_TEMPLATE void ColoredMPPI::computeControl(const Eigen::Ref<const state_array>& state,
int optimization_stride)
{
this->free_energy_statistics_.real_sys.previousBaseline = this->getBaselineCost();
state_array local_state = state;
if (getLeashActive())
{
this->model_->enforceLeash(state, this->state_.col(leash_jump_), this->params_.state_leash_dist_, local_state);
}
// Send the initial condition to the device
HANDLE_ERROR(cudaMemcpyAsync(this->initial_state_d_, local_state.data(), DYN_T::STATE_DIM * sizeof(float),
cudaMemcpyHostToDevice, this->stream_));
float baseline_prev = 1e8;
for (int opt_iter = 0; opt_iter < this->getNumIters(); opt_iter++)
{
// Send the nominal control to the device
this->copyNominalControlToDevice(false);
// Generate noise data
this->sampler_->generateSamples(optimization_stride, opt_iter, this->gen_, false);
// Launch the rollout kernel
if (this->getKernelChoiceAsEnum() == kernelType::USE_SPLIT_KERNELS)
{
mppi::kernels::launchSplitRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->output_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->params_.cost_rollout_dim_, this->stream_, false);
}
else if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{
mppi::kernels::launchRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->stream_, false);
}
// Copy the costs back to the host
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), this->trajectory_costs_d_,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
this->setBaseline(mppi::kernels::computeBaselineCost(this->trajectory_costs_.data(), NUM_ROLLOUTS));
if (this->getBaselineCost() > baseline_prev + 1)
{
this->logger_->debug("Previous Baseline: %f\n Baseline: %f\n", baseline_prev, this->getBaselineCost());
}
baseline_prev = this->getBaselineCost();
// Launch the norm exponential kernel
if (getGamma() == 0 || getRExp() == 0)
{
mppi::kernels::launchNormExpKernel(NUM_ROLLOUTS, this->getNormExpThreads(), this->trajectory_costs_d_,
1.0 / this->getLambda(), this->getBaselineCost(), this->stream_, false);
}
else
{
mppi::kernels::launchTsallisKernel(NUM_ROLLOUTS, this->getNormExpThreads(), this->trajectory_costs_d_, getGamma(),
getRExp(), this->getBaselineCost(), this->stream_, false);
}
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), this->trajectory_costs_d_,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
// Compute the normalizer
this->setNormalizer(mppi::kernels::computeNormalizer(this->trajectory_costs_.data(), NUM_ROLLOUTS));
mppi::kernels::computeFreeEnergy(this->free_energy_statistics_.real_sys.freeEnergyMean,
this->free_energy_statistics_.real_sys.freeEnergyVariance,
this->free_energy_statistics_.real_sys.freeEnergyModifiedVariance,
this->trajectory_costs_.data(), NUM_ROLLOUTS, this->getBaselineCost(),
this->getLambda());
// Compute the cost weighted average //TODO SUM_STRIDE is BDIM_X, but should it be its own parameter?
this->sampler_->updateDistributionParamsFromDevice(this->trajectory_costs_d_, this->getNormalizerCost(), 0, false);
// Transfer the new control to the host
this->sampler_->setHostOptimalControlSequence(this->control_.data(), 0, true);
}
this->free_energy_statistics_.real_sys.normalizerPercent = this->getNormalizerCost() / NUM_ROLLOUTS;
this->free_energy_statistics_.real_sys.increase =
this->getBaselineCost() - this->free_energy_statistics_.real_sys.previousBaseline;
smoothControlTrajectory();
computeStateTrajectory(local_state);
state_array zero_state = this->model_->getZeroState();
for (int i = 0; i < this->getNumTimesteps(); i++)
{
// this->model_->enforceConstraints(zero_state, this->control_.col(i));
this->control_.col(i)[1] =
fminf(fmaxf(this->control_.col(i)[1], this->model_->control_rngs_[1].x), this->model_->control_rngs_[1].y);
}
// Copy back sampled trajectories
this->copySampledControlFromDevice(false);
if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{ // copy initial state to vis initial state for use with visualizeKernel
HANDLE_ERROR(cudaMemcpyAsync(this->vis_initial_state_d_, this->initial_state_d_, sizeof(float) * DYN_T::STATE_DIM,
cudaMemcpyDeviceToDevice, this->vis_stream_));
}
this->copyTopControlFromDevice(true);
}
ColoredMPPI_TEMPLATE void ColoredMPPI::allocateCUDAMemory()
{
PARENT_CLASS::allocateCUDAMemoryHelper();
}
ColoredMPPI_TEMPLATE void ColoredMPPI::computeStateTrajectory(const Eigen::Ref<const state_array>& x0)
{
this->computeOutputTrajectoryHelper(this->output_, this->state_, x0, this->control_);
}
ColoredMPPI_TEMPLATE void ColoredMPPI::slideControlSequence(int steps)
{
// TODO does the logic of handling control history reasonable?
leash_jump_ = steps;
// Save the control history
this->saveControlHistoryHelper(steps, this->control_, this->control_history_);
this->slideControlSequenceHelper(steps, this->control_);
}
ColoredMPPI_TEMPLATE void ColoredMPPI::smoothControlTrajectory()
{
this->smoothControlTrajectoryHelper(this->control_, this->control_history_);
}
ColoredMPPI_TEMPLATE void ColoredMPPI::calculateSampledStateTrajectories()
{
int num_sampled_trajectories = this->getTotalSampledTrajectories();
// controls already copied in compute control
if (this->getKernelChoiceAsEnum() == kernelType::USE_SPLIT_KERNELS)
{
mppi::kernels::launchVisualizeCostKernel<COST_T, SAMPLING_T>(
this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), num_sampled_trajectories,
this->getLambda(), this->getAlpha(), this->sampled_outputs_d_, this->sampled_crash_status_d_,
this->sampled_costs_d_, this->params_.cost_rollout_dim_, this->stream_, false);
}
else if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{
mppi::kernels::launchVisualizeKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), num_sampled_trajectories,
this->getLambda(), this->getAlpha(), this->vis_initial_state_d_, this->sampled_outputs_d_,
this->sampled_costs_d_, this->sampled_crash_status_d_, this->params_.visualize_dim_, this->stream_, false);
}
// #if true
// mppi::kernels::launchVisualizeCostKernel<COST_T, SAMPLING_T>(
// this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(),
// num_sampled_trajectories, this->getLambda(), this->getAlpha(), this->sampled_outputs_d_,
// this->sampled_crash_status_d_, this->sampled_costs_d_, this->params_.cost_rollout_dim_, this->vis_stream_,
// false);
// #else
// mppi_common::launchVisualizeCostKernel<COST_T, 128, COST_B_Y, 1>(
// this->cost_, this->getDt(), this->getNumTimesteps(), num_sampled_trajectories, this->getLambda(),
// this->getAlpha(), this->sampled_outputs_d_, this->sampled_noise_d_, this->sampled_crash_status_d_,
// this->control_std_dev_d_, this->sampled_costs_d_, this->vis_stream_, false);
// #endif
for (int i = 0; i < num_sampled_trajectories; i++)
{
// set initial state to the first location
// shifted by one since we do not save the initial state
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_trajectories_[i].data(),
this->sampled_outputs_d_ + i * this->getNumTimesteps() * DYN_T::OUTPUT_DIM,
(this->getNumTimesteps() - 1) * DYN_T::OUTPUT_DIM * sizeof(float),
cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(
cudaMemcpyAsync(this->sampled_costs_[i].data(), this->sampled_costs_d_ + (i * (this->getNumTimesteps() + 1)),
(this->getNumTimesteps() + 1) * sizeof(float), cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_crash_status_[i].data(),
this->sampled_crash_status_d_ + (i * this->getNumTimesteps()),
this->getNumTimesteps() * sizeof(float), cudaMemcpyDeviceToHost, this->vis_stream_));
}
HANDLE_ERROR(cudaStreamSynchronize(this->vis_stream_));
}
#undef ColoredMPPI
================================================
FILE: include/mppi/controllers/ColoredMPPI/colored_mppi_controller.cuh
================================================
/**
* Created by jason on 10/30/19.
* Creates the API for interfacing with an MPPI controller
* should define a compute_control based on state as well
* as return timing info
**/
#ifndef MPPIGENERIC_COLORED_MPPI_CONTROLLER_CUH
#define MPPIGENERIC_COLORED_MPPI_CONTROLLER_CUH
#include <mppi/controllers/controller.cuh>
#include <mppi/sampling_distributions/colored_noise/colored_noise.cuh>
#include <vector>
template <int S_DIM, int C_DIM, int MAX_TIMESTEPS>
struct ColoredMPPIParams : public ControllerParams<S_DIM, C_DIM, MAX_TIMESTEPS>
{
float r = 2.0;
float gamma = 0;
Eigen::Matrix<float, S_DIM, 1> state_leash_dist_ = Eigen::Matrix<float, S_DIM, 1>::Zero();
ColoredMPPIParams() = default;
ColoredMPPIParams(const ColoredMPPIParams<S_DIM, C_DIM, MAX_TIMESTEPS>& other)
{
typedef ControllerParams<S_DIM, C_DIM, MAX_TIMESTEPS> BASE;
const BASE& other_item_ref = other;
*(static_cast<BASE*>(this)) = other_item_ref;
// this->colored_noise_exponents_ = other.colored_noise_exponents_;
}
ColoredMPPIParams(ColoredMPPIParams<S_DIM, C_DIM, MAX_TIMESTEPS>& other)
{
typedef ControllerParams<S_DIM, C_DIM, MAX_TIMESTEPS> BASE;
BASE& other_item_ref = other;
*(static_cast<BASE*>(this)) = other_item_ref;
// this->colored_noise_exponents_ = other.colored_noise_exponents_;
}
};
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS,
class SAMPLING_T = ::mppi::sampling_distributions::ColoredNoiseDistribution<typename DYN_T::DYN_PARAMS_T>,
class PARAMS_T = ColoredMPPIParams<DYN_T::STATE_DIM, DYN_T::CONTROL_DIM, MAX_TIMESTEPS>>
class ColoredMPPIController : public Controller<DYN_T, COST_T, FB_T, SAMPLING_T, MAX_TIMESTEPS, NUM_ROLLOUTS, PARAMS_T>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef Controller<DYN_T, COST_T, FB_T, SAMPLING_T, MAX_TIMESTEPS, NUM_ROLLOUTS, PARAMS_T> PARENT_CLASS;
// need control_array = ... so that we can initialize
// Eigen::Matrix with Eigen::Matrix::Zero();
using control_array = typename PARENT_CLASS::control_array;
using control_trajectory = typename PARENT_CLASS::control_trajectory;
using state_trajectory = typename PARENT_CLASS::state_trajectory;
using state_array = typename PARENT_CLASS::state_array;
using sampled_cost_traj = typename PARENT_CLASS::sampled_cost_traj;
using FEEDBACK_GPU = typename PARENT_CLASS::TEMPLATED_FEEDBACK_GPU;
/**
*
* Public member functions
*/
// Constructor
ColoredMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, float dt, int max_iter,
float lambda, float alpha, int num_timesteps = MAX_TIMESTEPS,
const Eigen::Ref<const control_trajectory>& init_control_traj = control_trajectory::Zero(),
cudaStream_t stream = nullptr);
ColoredMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, PARAMS_T& params,
cudaStream_t stream = nullptr);
// Destructor
~ColoredMPPIController();
std::string getControllerName() const override
{
return "Colored MPPI";
};
/**
* computes a new control sequence
* @param state starting position
*/
void computeControl(const Eigen::Ref<const state_array>& state, int optimization_stride = 1) override;
/**
* Slide the control sequence back n steps
*/
void slideControlSequence(int optimization_stride) override;
void setPercentageSampledControlTrajectories(float new_perc)
{
this->setPercentageSampledControlTrajectoriesHelper(new_perc, 1);
}
void setGamma(float gamma)
{
this->params_.gamma = gamma;
}
float getGamma()
{
return this->params_.gamma;
}
void setRExp(float r)
{
this->params_.r = r;
}
float getRExp()
{
return this->params_.r;
}
void setOffsetDecayRate(float decay_rate)
{
this->sampler_->setOffsetDecayRate(decay_rate);
}
float getOffsetDecayRate()
{
return this->sampler_->getOffsetDecayRate();
}
void setColoredNoiseExponents(std::vector<float>& new_exponents)
{
auto sampler_params = this->sampler_->getParams();
for (int i = 0; i < new_exponents.size(); i++)
{
sampler_params.exponents[i] = new_exponents[i];
}
this->sampler_->setParams(sampler_params);
}
float getColoredNoiseExponent(int index)
{
auto sampler_params = this->sampler_->getParams();
return sampler_params.exponents[index];
}
std::vector<float> getColoredNoiseExponents()
{
std::vector<float> exponents;
auto sampler_params = this->sampler_->getParams();
for (int i = 0; i < DYN_T::CONTROL_DIM; i++)
{
exponents.push_back(sampler_params.exponents[i]);
}
return exponents;
}
void setNoiseDecay(float new_noise_decay)
{
auto sampler_params = this->sampler_->getParams();
sampler_params.std_dev_decay = new_noise_decay;
this->sampler_->setParams(sampler_params);
}
void setStateLeashLength(float new_state_leash, int index = 0)
{
this->params_.state_leash_dist_[index] = new_state_leash;
}
float getStateLeashLength(int index)
{
return this->params_.state_leash_dist_[index];
}
bool getLeashActive()
{
return leash_active_;
}
void setLeashActive(bool new_leash_active)
{
leash_active_ = new_leash_active;
}
void calculateSampledStateTrajectories() override;
void chooseAppropriateKernel() override;
protected:
std::vector<float>& getColoredNoiseExponentsLValue()
{
return this->params_.colored_noise_exponents_;
}
void computeStateTrajectory(const Eigen::Ref<const state_array>& x0);
void smoothControlTrajectory();
int leash_jump_ = 1;
bool leash_active_ = false;
float control_std_dev_decay_ = 1.0;
private:
// ======== MUST BE OVERWRITTEN =========
void allocateCUDAMemory();
// ======== END MUST BE OVERWRITTEN =====
};
#if __CUDACC__
#include "colored_mppi_controller.cu"
#endif
#endif // MPPIGENERIC_COLORED_MPPI_CONTROLLER_CUH
================================================
FILE: include/mppi/controllers/MPPI/mppi_controller.cu
================================================
#include <atomic>
#include <mppi/controllers/MPPI/mppi_controller.cuh>
#include <mppi/core/mppi_common.cuh>
#include <algorithm>
#include <iostream>
#include <stdexcept>
#include <string>
#define VANILLA_MPPI_TEMPLATE \
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, class SAMPLING_T, \
class PARAMS_T>
#define VanillaMPPI VanillaMPPIController<DYN_T, COST_T, FB_T, MAX_TIMESTEPS, NUM_ROLLOUTS, SAMPLING_T, PARAMS_T>
VANILLA_MPPI_TEMPLATE
VanillaMPPI::VanillaMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, float dt,
int max_iter, float lambda, float alpha, int num_timesteps,
const Eigen::Ref<const control_trajectory>& init_control_traj, cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, dt, max_iter, lambda, alpha, num_timesteps, init_control_traj,
stream)
{
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// Copy the noise std_dev to the device
// this->copyControlStdDevToDevice();
chooseAppropriateKernel();
}
VANILLA_MPPI_TEMPLATE
VanillaMPPI::VanillaMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler,
PARAMS_T& params, cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, params, stream)
{
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// // Copy the noise std_dev to the device
// this->copyControlStdDevToDevice();
chooseAppropriateKernel();
}
VANILLA_MPPI_TEMPLATE
void VanillaMPPI::chooseAppropriateKernel()
{
cudaDeviceProp deviceProp;
HANDLE_ERROR(cudaGetDeviceProperties(&deviceProp, 0));
unsigned single_kernel_byte_size = mppi::kernels::calcRolloutCombinedKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, this->params_.dynamics_rollout_dim_);
unsigned split_dyn_kernel_byte_size = mppi::kernels::calcRolloutDynamicsKernelSharedMemSize(
this->model_, this->sampler_, this->params_.dynamics_rollout_dim_);
unsigned split_cost_kernel_byte_size =
mppi::kernels::calcRolloutCostKernelSharedMemSize(this->cost_, this->sampler_, this->params_.cost_rollout_dim_);
unsigned vis_single_kernel_byte_size = mppi::kernels::calcVisualizeKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, this->getNumTimesteps(), this->params_.visualize_dim_);
bool too_much_mem_single_kernel = single_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool too_much_mem_vis_kernel = vis_single_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool too_much_mem_split_kernel = split_dyn_kernel_byte_size > deviceProp.sharedMemPerBlock;
too_much_mem_split_kernel = too_much_mem_split_kernel || split_cost_kernel_byte_size > deviceProp.sharedMemPerBlock;
too_much_mem_single_kernel = too_much_mem_single_kernel || too_much_mem_vis_kernel;
if (too_much_mem_split_kernel && too_much_mem_single_kernel)
{
std::string error_msg =
"There is not enough shared memory on the GPU for either rollout kernel option. The combined rollout kernel "
"takes " +
std::to_string(single_kernel_byte_size) + " bytes, the cost rollout kernel takes " +
std::to_string(split_cost_kernel_byte_size) + " bytes, the dynamics rollout kernel takes " +
std::to_string(split_dyn_kernel_byte_size) + " bytes, the combined visualization kernel takes " +
std::to_string(vis_single_kernel_byte_size) + " bytes, and the max is " +
std::to_string(deviceProp.sharedMemPerBlock) +
" bytes. Considering lowering the corresponding thread block sizes.";
throw std::runtime_error(error_msg);
}
else if (too_much_mem_single_kernel)
{
this->setKernelChoice(kernelType::USE_SPLIT_KERNELS);
return;
}
else if (too_much_mem_split_kernel)
{
this->setKernelChoice(kernelType::USE_SINGLE_KERNEL);
return;
}
// Send the nominal control to the device
this->copyNominalControlToDevice(false);
state_array zero_state = this->model_->getZeroState();
// Send zero state to the device
HANDLE_ERROR(cudaMemcpyAsync(this->initial_state_d_, zero_state.data(), DYN_T::STATE_DIM * sizeof(float),
cudaMemcpyHostToDevice, this->stream_));
// Generate noise data
this->sampler_->generateSamples(1, 0, this->gen_, true);
float single_kernel_time_ms = std::numeric_limits<float>::infinity();
float split_kernel_time_ms = std::numeric_limits<float>::infinity();
// Evaluate each kernel that is applicable
auto start_single_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !too_much_mem_single_kernel; i++)
{
mppi::kernels::launchRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->stream_, true);
}
auto end_single_kernel_time = std::chrono::steady_clock::now();
auto start_split_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !too_much_mem_split_kernel; i++)
{
mppi::kernels::launchSplitRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->output_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->params_.cost_rollout_dim_, this->stream_, true);
}
auto end_split_kernel_time = std::chrono::steady_clock::now();
// calc times
if (!too_much_mem_single_kernel)
{
single_kernel_time_ms = mppi::math::timeDiffms(end_single_kernel_time, start_single_kernel_time);
}
if (!too_much_mem_split_kernel)
{
split_kernel_time_ms = mppi::math::timeDiffms(end_split_kernel_time, start_split_kernel_time);
}
std::string kernel_choice = "";
if (split_kernel_time_ms < single_kernel_time_ms)
{
this->setKernelChoice(kernelType::USE_SPLIT_KERNELS);
kernel_choice = "split ";
}
else
{
this->setKernelChoice(kernelType::USE_SINGLE_KERNEL);
kernel_choice = "single";
}
this->logger_->info("Choosing %s kernel based on split taking %f ms and single taking %f ms after %d iterations\n",
kernel_choice.c_str(), split_kernel_time_ms, single_kernel_time_ms,
this->getNumKernelEvaluations());
}
VANILLA_MPPI_TEMPLATE
VanillaMPPI::~VanillaMPPIController()
{
// all implemented in standard controller
}
VANILLA_MPPI_TEMPLATE
void VanillaMPPI::computeControl(const Eigen::Ref<const state_array>& state, int optimization_stride)
{
this->free_energy_statistics_.real_sys.previousBaseline = this->getBaselineCost();
// Send the initial condition to the device
HANDLE_ERROR(cudaMemcpyAsync(this->initial_state_d_, state.data(), DYN_T::STATE_DIM * sizeof(float),
cudaMemcpyHostToDevice, this->stream_));
float baseline_prev = 1e8;
for (int opt_iter = 0; opt_iter < this->getNumIters(); opt_iter++)
{
// Send the nominal control to the device
this->copyNominalControlToDevice(false);
// Generate noise data
this->sampler_->generateSamples(optimization_stride, opt_iter, this->gen_, false);
// Launch the rollout kernel
if (this->getKernelChoiceAsEnum() == kernelType::USE_SPLIT_KERNELS)
{
mppi::kernels::launchSplitRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->output_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->params_.cost_rollout_dim_, this->stream_, false);
}
else if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{
mppi::kernels::launchRolloutKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->trajectory_costs_d_,
this->params_.dynamics_rollout_dim_, this->stream_, false);
}
// Copy the costs back to the host
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), this->trajectory_costs_d_,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
this->setBaseline(mppi::kernels::computeBaselineCost(this->trajectory_costs_.data(), NUM_ROLLOUTS));
if (this->getBaselineCost() > baseline_prev + 1)
{
this->logger_->debug("Previous Baseline: %f\n Baseline: %f\n", baseline_prev, this->getBaselineCost());
}
baseline_prev = this->getBaselineCost();
// Launch the norm exponential kernel
mppi::kernels::launchNormExpKernel(NUM_ROLLOUTS, this->getNormExpThreads(), this->trajectory_costs_d_,
1.0 / this->getLambda(), this->getBaselineCost(), this->stream_, false);
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), this->trajectory_costs_d_,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
// Compute the normalizer
this->setNormalizer(mppi::kernels::computeNormalizer(this->trajectory_costs_.data(), NUM_ROLLOUTS));
mppi::kernels::computeFreeEnergy(this->free_energy_statistics_.real_sys.freeEnergyMean,
this->free_energy_statistics_.real_sys.freeEnergyVariance,
this->free_energy_statistics_.real_sys.freeEnergyModifiedVariance,
this->trajectory_costs_.data(), NUM_ROLLOUTS, this->getBaselineCost(),
this->getLambda());
this->sampler_->updateDistributionParamsFromDevice(this->trajectory_costs_d_, this->getNormalizerCost(), 0, false);
// Transfer the new control to the host
this->sampler_->setHostOptimalControlSequence(this->control_.data(), 0, true);
}
this->free_energy_statistics_.real_sys.normalizerPercent = this->getNormalizerCost() / NUM_ROLLOUTS;
this->free_energy_statistics_.real_sys.increase =
this->getBaselineCost() - this->free_energy_statistics_.real_sys.previousBaseline;
smoothControlTrajectory();
computeStateTrajectory(state);
state_array zero_state = this->model_->getZeroState();
for (int i = 0; i < this->getNumTimesteps(); i++)
{
this->model_->enforceConstraints(zero_state, this->control_.col(i));
}
// Copy back sampled trajectories
this->copySampledControlFromDevice(false);
if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{ // copy initial state to vis initial state for use with visualizeKernel
HANDLE_ERROR(cudaMemcpyAsync(this->vis_initial_state_d_, this->initial_state_d_, sizeof(float) * DYN_T::STATE_DIM,
cudaMemcpyDeviceToDevice, this->vis_stream_));
}
this->copyTopControlFromDevice(true);
}
VANILLA_MPPI_TEMPLATE
void VanillaMPPI::allocateCUDAMemory()
{
PARENT_CLASS::allocateCUDAMemoryHelper();
}
VANILLA_MPPI_TEMPLATE
void VanillaMPPI::computeStateTrajectory(const Eigen::Ref<const state_array>& x0)
{
this->computeOutputTrajectoryHelper(this->output_, this->state_, x0, this->control_);
}
VANILLA_MPPI_TEMPLATE
void VanillaMPPI::smoothControlTrajectory()
{
this->smoothControlTrajectoryHelper(this->control_, this->control_history_);
}
VANILLA_MPPI_TEMPLATE
void VanillaMPPI::calculateSampledStateTrajectories()
{
int num_sampled_trajectories = this->getTotalSampledTrajectories();
// control already copied in compute control, so run kernel
if (this->getKernelChoiceAsEnum() == kernelType::USE_SPLIT_KERNELS)
{
mppi::kernels::launchVisualizeCostKernel<COST_T, SAMPLING_T>(
this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), num_sampled_trajectories,
this->getLambda(), this->getAlpha(), this->sampled_outputs_d_, this->sampled_crash_status_d_,
this->sampled_costs_d_, this->params_.cost_rollout_dim_, this->stream_, false);
}
else if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{
mppi::kernels::launchVisualizeKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), num_sampled_trajectories,
this->getLambda(), this->getAlpha(), this->vis_initial_state_d_, this->sampled_outputs_d_,
this->sampled_costs_d_, this->sampled_crash_status_d_, this->params_.visualize_dim_, this->stream_, false);
}
for (int i = 0; i < num_sampled_trajectories; i++)
{
// set initial state to the first location
// shifted by one since we do not save the initial state
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_trajectories_[i].data(),
this->sampled_outputs_d_ + i * this->getNumTimesteps() * DYN_T::OUTPUT_DIM,
(this->getNumTimesteps() - 1) * DYN_T::OUTPUT_DIM * sizeof(float),
cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(
cudaMemcpyAsync(this->sampled_costs_[i].data(), this->sampled_costs_d_ + (i * (this->getNumTimesteps() + 1)),
(this->getNumTimesteps() + 1) * sizeof(float), cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_crash_status_[i].data(),
this->sampled_crash_status_d_ + (i * this->getNumTimesteps()),
this->getNumTimesteps() * sizeof(int), cudaMemcpyDeviceToHost, this->vis_stream_));
}
HANDLE_ERROR(cudaStreamSynchronize(this->vis_stream_));
}
#undef VANILLA_MPPI_TEMPLATE
#undef VanillaMPPI
================================================
FILE: include/mppi/controllers/MPPI/mppi_controller.cuh
================================================
/**
* Created by jason on 10/30/19.
* Creates the API for interfacing with an MPPI controller
* should define a compute_control based on state as well
* as return timing info
**/
#ifndef MPPIGENERIC_MPPI_CONTROLLER_CUH
#define MPPIGENERIC_MPPI_CONTROLLER_CUH
#include <mppi/controllers/controller.cuh>
#include <mppi/sampling_distributions/gaussian/gaussian.cuh>
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS,
class SAMPLING_T = ::mppi::sampling_distributions::GaussianDistribution<typename DYN_T::DYN_PARAMS_T>,
class PARAMS_T = ControllerParams<DYN_T::STATE_DIM, DYN_T::CONTROL_DIM, MAX_TIMESTEPS>>
class VanillaMPPIController : public Controller<DYN_T, COST_T, FB_T, SAMPLING_T, MAX_TIMESTEPS, NUM_ROLLOUTS, PARAMS_T>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// nAeed control_array = ... so that we can initialize
// Eigen::Matrix with Eigen::Matrix::Zero();
typedef Controller<DYN_T, COST_T, FB_T, SAMPLING_T, MAX_TIMESTEPS, NUM_ROLLOUTS, PARAMS_T> PARENT_CLASS;
using control_array = typename PARENT_CLASS::control_array;
using control_trajectory = typename PARENT_CLASS::control_trajectory;
using state_trajectory = typename PARENT_CLASS::state_trajectory;
using state_array = typename PARENT_CLASS::state_array;
using sampled_cost_traj = typename PARENT_CLASS::sampled_cost_traj;
using FEEDBACK_GPU = typename PARENT_CLASS::TEMPLATED_FEEDBACK_GPU;
/**
*
* Public member functions
*/
// Constructor
VanillaMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, float dt, int max_iter,
float lambda, float alpha, int num_timesteps = MAX_TIMESTEPS,
const Eigen::Ref<const control_trajectory>& init_control_traj = control_trajectory::Zero(),
cudaStream_t stream = nullptr);
VanillaMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, PARAMS_T& params,
cudaStream_t stream = nullptr);
// Destructor
~VanillaMPPIController();
std::string getControllerName() const override
{
return "Vanilla MPPI";
};
/**
* computes a new control sequence
* @param state starting position
*/
void computeControl(const Eigen::Ref<const state_array>& state, int optimization_stride = 1) override;
void setPercentageSampledControlTrajectories(float new_perc)
{
this->setPercentageSampledControlTrajectoriesHelper(new_perc, 1);
}
void calculateSampledStateTrajectories() override;
void chooseAppropriateKernel() override;
protected:
void computeStateTrajectory(const Eigen::Ref<const state_array>& x0);
void smoothControlTrajectory();
private:
// ======== MUST BE OVERWRITTEN =========
void allocateCUDAMemory();
// ======== END MUST BE OVERWRITTEN =====
};
#if __CUDACC__
#include "mppi_controller.cu"
#endif
#endif // MPPIGENERIC_MPPI_CONTROLLER_CUH
================================================
FILE: include/mppi/controllers/Primitives/primitives_controller.cu
================================================
#include <mppi/controllers/Primitives/primitives_controller.cuh>
#include <mppi/core/mppi_common.cuh>
#include <algorithm>
#include <iostream>
#include <mppi/sampling_distributions/piecewise_linear/piecewise_linear_noise.cuh>
#include <mppi/sampling_distributions/colored_noise/colored_noise.cuh>
#define Primitives \
PrimitivesController<DYN_T, COST_T, FB_T, MAX_TIMESTEPS, NUM_ROLLOUTS, BDIM_X, BDIM_Y, COST_B_X, COST_B_Y, PARAMS_T>
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
Primitives::PrimitivesController(DYN_T* model, COST_T* cost, FB_T* fb_controller, float dt, int max_iter, float lambda,
float alpha, const Eigen::Ref<const control_array>& control_std_dev, int num_timesteps,
const Eigen::Ref<const control_trajectory>& init_control_traj, cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, dt, max_iter, lambda, alpha, control_std_dev, num_timesteps,
init_control_traj, stream)
{
// Allocate CUDA memory for the controller
allocateCUDAMemory();
std::vector<float> tmp_vec(DYN_T::CONTROL_DIM, 0.0);
getColoredNoiseExponentsLValue() = std::move(tmp_vec);
getScalePiecewiseNoiseLValue() = std::move(tmp_vec);
// Copy the noise std_dev to the device
this->copyControlStdDevToDevice();
control_mppi_history_ = Eigen::Matrix<float, DYN_T::CONTROL_DIM, 2>::Zero();
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
Primitives::PrimitivesController(DYN_T* model, COST_T* cost, FB_T* fb_controller, PARAMS_T& params, cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, params, stream)
{
// Allocate CUDA memory for the controller
allocateCUDAMemory();
if (this->getColoredNoiseExponentsLValue().size() == 0)
{
std::vector<float> tmp_vec(DYN_T::CONTROL_DIM, 0.0);
getColoredNoiseExponentsLValue() = std::move(tmp_vec);
}
if (this->getScalePiecewiseNoiseLValue().size() == 0)
{
std::vector<float> tmp_vec(DYN_T::CONTROL_DIM, 0.0);
getScalePiecewiseNoiseLValue() = std::move(tmp_vec);
}
// Copy the noise std_dev to the device
this->copyControlStdDevToDevice();
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
Primitives::~PrimitivesController()
{
// all implemented in standard controller
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::computeControl(const Eigen::Ref<const state_array>& state, int optimization_stride)
{
// this->free_energy_statistics_.real_sys.previousBaseline = this->getBaselineCost();
state_array local_state = state;
if (getLeashActive())
{
this->model_->enforceLeash(state, this->state_.col(leash_jump_), this->params_.state_leash_dist_, local_state);
}
// Send the initial condition to the device
HANDLE_ERROR(cudaMemcpyAsync(this->initial_state_d_, local_state.data(), DYN_T::STATE_DIM * sizeof(float),
cudaMemcpyHostToDevice, this->stream_));
/////////////////
// BEGIN INTERMEDIATE PLANNER
// Compute intermediate plan using piecewise linear noise, and choosing the best
int prev_controls_idx = 1;
float primitives_baseline = 0.0;
float baseline_prev = 0.0;
int best_idx = -1;
// Send the nominal control to the device
this->copyNominalControlToDevice(false);
for (int opt_iter = 0; opt_iter < getNumPrimitiveIterations(); opt_iter++)
{
powerlaw_psd_gaussian(getColoredNoiseExponentsLValue(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->control_noise_d_, 0, this->gen_, this->getOffsetDecayRate(), this->stream_);
// Generate piecewise linear noise data, update control_noise_d_
piecewise_linear_noise(this->getNumTimesteps(), NUM_ROLLOUTS, DYN_T::CONTROL_DIM, getPiecewiseSegments(),
optimization_stride, getScalePiecewiseNoiseLValue(), getFracRandomNoiseTrajLValue(),
getScaleAddNominalNoiseLValue(), this->control_d_, this->control_noise_d_,
this->control_std_dev_d_, this->gen_, this->stream_);
// // Set nominal controls to zero because we want to use the noise directly
// this->control_ = control_trajectory::Zero();
// // Send the zero nominal control to the device
// this->copyNominalControlToDevice();
// Launch the rollout kernel
mppi_common::launchFastRolloutKernel<DYN_T, COST_T, NUM_ROLLOUTS, BDIM_X, BDIM_Y, 1, COST_B_X, COST_B_Y>(
this->model_->model_d_, this->cost_->cost_d_, this->getDt(), this->getNumTimesteps(), optimization_stride,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->output_d_, this->control_d_,
this->control_noise_d_, this->control_std_dev_d_, this->trajectory_costs_d_, this->stream_, false);
// Copy the costs back to the host
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), this->trajectory_costs_d_,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
primitives_baseline = mppi_common::computeBaselineCost(this->trajectory_costs_.data(), NUM_ROLLOUTS);
// get previous control cost (at index = 1, since index = 0 is zero control traj)
baseline_prev = this->trajectory_costs_.data()[prev_controls_idx];
if (this->debug_)
{
std::cerr << "Previous Baseline: " << baseline_prev << " Baseline: " << this->getBaselineCost()
<< std::endl;
}
// if baseline is too high and trajectory is unsafe, create and issue a stopping trajectory
// reminder: baseline_ is the average cost along trajectory
if (getStoppingCostThreshold() > 0 && primitives_baseline > getStoppingCostThreshold())
{
std::cerr << "Baseline is too high, issuing stopping trajectory!" << std::endl;
computeStoppingTrajectory(local_state);
primitives_baseline = std::numeric_limits<float>::min();
}
// else if (primitives_baseline > baseline_prev - getHysteresisCostThreshold())
// {
// // baseline is not decreasing enough, use controls from the previous iteration
// if (this->debug_)
// {
// std::cout << "Not enough improvement, use prev controls." << std::endl;
// }
// HANDLE_ERROR(cudaMemcpyAsync(
// this->control_.data(),
// this->control_noise_d_ + prev_controls_idx * this->getNumTimesteps() * DYN_T::CONTROL_DIM,
// sizeof(float) * this->getNumTimesteps() * DYN_T::CONTROL_DIM, cudaMemcpyDeviceToHost, this->stream_));
// primitives_baseline = baseline_prev;
// }
else
{ // otherwise, update the nominal control
// Copy best control from device to the host
best_idx = mppi_common::computeBestIndex(this->trajectory_costs_.data(), NUM_ROLLOUTS);
HANDLE_ERROR(cudaMemcpyAsync(
this->control_.data(), this->control_noise_d_ + best_idx * this->getNumTimesteps() * DYN_T::CONTROL_DIM,
sizeof(float) * this->getNumTimesteps() * DYN_T::CONTROL_DIM, cudaMemcpyDeviceToHost, this->stream_));
}
this->copyNominalControlToDevice(false);
cudaStreamSynchronize(this->stream_);
}
// Copy back sampled trajectories for visualization
if (getVisualizePrimitives())
{
this->copySampledControlFromDevice(false);
this->copyTopControlFromDevice(true);
}
// END INTERMEDIATE PLANNER
////////////////
////////////////
// BEGIN MPPI
for (int opt_iter = 0; opt_iter < this->getNumIters(); opt_iter++)
{
// Send the nominal control to the device
copyMPPIControlToDevice(false);
// Generate noise data
// const int colored_num_timesteps = (this->getNumTimesteps() > optimization_stride) ?
// this->getNumTimesteps() - optimization_stride :
// this->getNumTimesteps();
// const int colored_stride = (this->getNumTimesteps() > optimization_stride) ? optimization_stride : 0;
// if (colored_stride == 0)
// {
// std::cout << "We tripped the fail-safe" << std::endl;
// }
powerlaw_psd_gaussian(getColoredNoiseExponentsLValue(), this->getNumTimesteps(), NUM_ROLLOUTS,
this->control_noise_d_, 0, this->gen_, this->getOffsetDecayRate(), this->stream_);
// curandGenerateNormal(this->gen_, this->control_noise_d_, NUM_ROLLOUTS * this->getNumTimesteps() *
// DYN_T::CONTROL_DIM,
// 0.0, 1.0);
/*
std::vector<float> noise = this->getSampledNoise();
float mean = 0;
for(int k = 0; k < noise.size(); k++) {
mean += (noise[k]/noise.size());
}
float std_dev = 0;
for(int k = 0; k < noise.size(); k++) {
std_dev += powf(noise[k] - mean, 2);
}
std_dev = sqrt(std_dev/noise.size());
printf("CPU 1 side N(%f, %f)\n", mean, std_dev);
*/
// Launch the rollout kernel
mppi_common::launchFastRolloutKernel<DYN_T, COST_T, NUM_ROLLOUTS, BDIM_X, BDIM_Y, 1, COST_B_X, COST_B_Y>(
this->model_->model_d_, this->cost_->cost_d_, this->getDt(), this->getNumTimesteps(), optimization_stride,
this->getLambda(), this->getAlpha(), this->initial_state_d_, this->output_d_, control_mppi_d_,
this->control_noise_d_, this->control_std_dev_d_, this->trajectory_costs_d_, this->stream_, false);
/*
noise = this->getSampledNoise();
mean = 0;
for(int k = 0; k < noise.size(); k++) {
mean += (noise[k]/noise.size());
}
std_dev = 0;
for(int k = 0; k < noise.size(); k++) {
std_dev += powf(noise[k] - mean, 2);
}
std_dev = sqrt(std_dev/noise.size());
printf("CPU 2 side N(%f, %f)\n", mean, std_dev);
*/
// Copy the costs back to the host
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), this->trajectory_costs_d_,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
this->setBaseline(mppi_common::computeBaselineCost(this->trajectory_costs_.data(), NUM_ROLLOUTS));
// if (this->getBaselineCost() > baseline_prev + 1)
// {
// // TODO handle printing
// if (this->debug_)
// {
// std::cout << "Previous Baseline: " << baseline_prev << std::endl;
// std::cout << " Baseline: " << this->getBaselineCost() << std::endl;
// }
// }
// baseline_prev = this->getBaselineCost();
// Launch the norm exponential kernel
if (getGamma() == 0 || getRExp() == 0)
{
mppi_common::launchNormExpKernel(NUM_ROLLOUTS, BDIM_X, this->trajectory_costs_d_, 1.0 / this->getLambda(),
this->getBaselineCost(), this->stream_, false);
}
else
{
mppi_common::launchTsallisKernel(NUM_ROLLOUTS, BDIM_X, this->trajectory_costs_d_, getGamma(), getRExp(),
this->getBaselineCost(), this->stream_, false);
}
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), this->trajectory_costs_d_,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
// Compute the normalizer
this->setNormalizer(mppi_common::computeNormalizer(this->trajectory_costs_.data(), NUM_ROLLOUTS));
mppi_common::computeFreeEnergy(this->free_energy_statistics_.real_sys.freeEnergyMean,
this->free_energy_statistics_.real_sys.freeEnergyVariance,
this->free_energy_statistics_.real_sys.freeEnergyModifiedVariance,
this->trajectory_costs_.data(), NUM_ROLLOUTS, this->getBaselineCost(),
this->getLambda());
// Compute the cost weighted average //TODO SUM_STRIDE is BDIM_X, but should it be its own parameter?
mppi_common::launchWeightedReductionKernel<DYN_T, NUM_ROLLOUTS, BDIM_X>(
this->trajectory_costs_d_, this->control_noise_d_, control_mppi_d_, this->getNormalizerCost(),
this->getNumTimesteps(), this->stream_, false);
/*
noise = this->getSampledNoise();
mean = 0;
for(int k = 0; k < noise.size(); k++) {
mean += (noise[k]/noise.size());
}
std_dev = 0;
for(int k = 0; k < noise.size(); k++) {
std_dev += powf(noise[k] - mean, 2);
}
std_dev = sqrt(std_dev/noise.size());
printf("CPU 3 side N(%f, %f)\n", mean, std_dev);
*/
// Transfer the new control to the host
HANDLE_ERROR(cudaMemcpyAsync(control_mppi_.data(), control_mppi_d_,
sizeof(float) * this->getNumTimesteps() * DYN_T::CONTROL_DIM, cudaMemcpyDeviceToHost,
this->stream_));
cudaStreamSynchronize(this->stream_);
}
this->free_energy_statistics_.real_sys.normalizerPercent = this->getNormalizerCost() / NUM_ROLLOUTS;
this->free_energy_statistics_.real_sys.increase =
this->getBaselineCost() - this->free_energy_statistics_.real_sys.previousBaseline;
// END MPPI
////////////////////////
// decide between using the MPPI control or the primitives control
if (this->debug_)
{
std::cerr << "mppi baseline: " << this->getBaselineCost() << ", primitives baseline: " << primitives_baseline
<< ", prev baseline: " << baseline_prev << std::endl;
}
if ((getNumPrimitiveIterations() == 0 && this->getNumIters() > 0) ||
((getNumPrimitiveIterations() > 0 && this->getNumIters() > 0) &&
(this->getBaselineCost() < primitives_baseline + getHysteresisCostThreshold())))
{
this->control_ = control_mppi_;
this->copyNominalControlToDevice();
if (this->debug_)
{
std::cout << "Using MPPI control" << std::endl;
}
this->free_energy_statistics_.nominal_state_used = 0;
}
else
{
// control_mppi_ = this->control_; // don't do this, we want to save the MPPI control
if (this->debug_)
{
std::cout << "Using primitives control, ";
}
if (best_idx > 0 && best_idx <= int((getFracRandomNoiseTrajLValue())[0] * NUM_ROLLOUTS))
{
if (this->debug_)
{
std::cout << "colored noise added to nominal." << std::endl;
}
this->free_energy_statistics_.nominal_state_used = 1;
}
else if (best_idx <= int((getFracRandomNoiseTrajLValue()[0] + getFracRandomNoiseTrajLValue()[1]) * NUM_ROLLOUTS))
{
if (this->debug_)
{
std::cout << "piecewise noise added to nominal." << std::endl;
}
this->free_energy_statistics_.nominal_state_used = 2;
}
else
{
if (this->debug_)
{
std::cout << "new piecewise trajectory." << std::endl;
}
this->free_energy_statistics_.nominal_state_used = 3;
}
}
smoothControlTrajectory();
computeStateTrajectory(local_state);
state_array zero_state = state_array::Zero();
for (int i = 0; i < this->getNumTimesteps(); i++)
{
this->model_->enforceConstraints(zero_state, this->control_.col(i));
this->model_->enforceConstraints(zero_state, control_mppi_.col(i));
// this->control_.col(i)[1] =
// fminf(fmaxf(this->control_.col(i)[1], this->model_->control_rngs_[1].x), this->model_->control_rngs_[1].y);
}
// Copy back sampled trajectories for visualization
if (!getVisualizePrimitives())
{
this->copySampledControlFromDevice(false);
this->copyTopControlFromDevice(true);
}
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::allocateCUDAMemory()
{
PARENT_CLASS::allocateCUDAMemoryHelper();
HANDLE_ERROR(cudaMalloc((void**)&control_mppi_d_, sizeof(float) * DYN_T::CONTROL_DIM * MAX_TIMESTEPS));
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::copyMPPIControlToDevice(bool synchronize)
{
HANDLE_ERROR(cudaMemcpyAsync(control_mppi_d_, control_mppi_.data(), sizeof(float) * control_mppi_.size(),
cudaMemcpyHostToDevice, this->stream_));
if (synchronize)
{
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
}
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::computeStateTrajectory(const Eigen::Ref<const state_array>& x0)
{
this->computeStateTrajectoryHelper(this->state_, x0, this->control_);
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::computeStoppingTrajectory(const Eigen::Ref<const state_array>& x0)
{
state_array xdot;
state_array state = x0;
state_array xnext;
output_array output;
control_array u_i = control_array::Zero();
this->model_->initializeDynamics(state, u_i, output, 0, this->getDt());
for (int i = 0; i < this->getNumTimesteps() - 1; ++i)
{
this->model_->getStoppingControl(state, u_i);
this->model_->enforceConstraints(state, u_i);
this->control_.col(i) = u_i;
this->model_->step(state, xnext, xdot, u_i, output, i, this->getDt());
state = xnext;
}
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::slideControlSequence(int steps)
{
// TODO does the logic of handling control history reasonable?
leash_jump_ = steps;
// Save the control history
this->saveControlHistoryHelper(steps, this->control_, this->control_history_);
this->saveControlHistoryHelper(steps, control_mppi_, control_mppi_history_);
this->slideControlSequenceHelper(steps, this->control_);
this->slideControlSequenceHelper(steps, control_mppi_);
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::smoothControlTrajectory()
{
this->smoothControlTrajectoryHelper(this->control_, this->control_history_);
this->smoothControlTrajectoryHelper(control_mppi_, control_mppi_history_);
}
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X, int COST_B_Y, class PARAMS_T>
void Primitives::calculateSampledStateTrajectories()
{
int num_sampled_trajectories = this->getTotalSampledTrajectories();
// controls already copied in compute control
mppi_common::launchStateAndCostTrajectoryKernel<DYN_T, COST_T, FEEDBACK_GPU, BDIM_X, BDIM_Y>(
this->model_->model_d_, this->cost_->cost_d_, this->fb_controller_->getDevicePointer(), this->sampled_noise_d_,
this->initial_state_d_, this->sampled_outputs_d_, this->sampled_costs_d_, this->sampled_crash_status_d_,
num_sampled_trajectories, this->getNumTimesteps(), this->getDt(), this->vis_stream_);
for (int i = 0; i < num_sampled_trajectories; i++)
{
// set initial state to the first location
// shifted by one since we do not save the initial state
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_trajectories_[i].data(),
this->sampled_outputs_d_ + i * this->getNumTimesteps() * DYN_T::OUTPUT_DIM,
(this->getNumTimesteps() - 1) * DYN_T::OUTPUT_DIM * sizeof(float),
cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(
cudaMemcpyAsync(this->sampled_costs_[i].data(), this->sampled_costs_d_ + (i * (this->getNumTimesteps() + 1)),
(this->getNumTimesteps() + 1) * sizeof(float), cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_crash_status_[i].data(),
this->sampled_crash_status_d_ + (i * this->getNumTimesteps()),
this->getNumTimesteps() * sizeof(float), cudaMemcpyDeviceToHost, this->vis_stream_));
}
HANDLE_ERROR(cudaStreamSynchronize(this->vis_stream_));
}
#undef Primitives
================================================
FILE: include/mppi/controllers/Primitives/primitives_controller.cuh
================================================
/**
* Created by david fan on 04/11/22.
* Creates the API for interfacing with an MPPI controller
* should define a compute_control based on state as well
* as return timing info
**/
#ifndef MPPIGENERIC_PRIMITIVES_CONTROLLER_CUH
#define MPPIGENERIC_PRIMITIVES_CONTROLLER_CUH
#include <mppi/controllers/controller.cuh>
// this is needed to extend the coloredMPPI params and ensure that
// dynamic reconfigure in mppi_generic_racer_plant.cpp works properly
#include <mppi/controllers/ColoredMPPI/colored_mppi_controller.cuh>
#include <vector>
template <int S_DIM, int C_DIM, int MAX_TIMESTEPS>
struct PrimitivesParams : public ColoredMPPIParams<S_DIM, C_DIM, MAX_TIMESTEPS>
{
int num_primitive_iters_;
int num_piecewise_segments_ = 5;
std::vector<float> scale_piecewise_noise_;
std::vector<float> frac_add_nominal_traj_;
std::vector<float> scale_add_nominal_noise_;
float stopping_cost_threshold_ = 1.0e8;
float hysteresis_cost_threshold_ = 0.0;
bool visualize_primitives_ = false;
PrimitivesParams<S_DIM, C_DIM, MAX_TIMESTEPS>() = default;
PrimitivesParams<S_DIM, C_DIM, MAX_TIMESTEPS>(const PrimitivesParams<S_DIM, C_DIM, MAX_TIMESTEPS>& other)
{
typedef ColoredMPPIParams<S_DIM, C_DIM, MAX_TIMESTEPS> BASE;
const BASE& other_item_ref = other;
*(static_cast<BASE*>(this)) = other_item_ref;
this->scale_piecewise_noise_ = other.scale_piecewise_noise_;
this->frac_add_nominal_traj_ = other.frac_add_nominal_traj_;
this->scale_add_nominal_noise_ = other.scale_add_nominal_noise_;
}
PrimitivesParams<S_DIM, C_DIM, MAX_TIMESTEPS>(PrimitivesParams<S_DIM, C_DIM, MAX_TIMESTEPS>& other)
{
typedef ColoredMPPIParams<S_DIM, C_DIM, MAX_TIMESTEPS> BASE;
BASE& other_item_ref = other;
*(static_cast<BASE*>(this)) = other_item_ref;
this->scale_piecewise_noise_ = other.scale_piecewise_noise_;
this->frac_add_nominal_traj_ = other.frac_add_nominal_traj_;
this->scale_add_nominal_noise_ = other.scale_add_nominal_noise_;
}
};
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, int BDIM_X, int BDIM_Y,
int COST_B_X = 64, int COST_B_Y = 2,
class PARAMS_T = PrimitivesParams<DYN_T::STATE_DIM, DYN_T::CONTROL_DIM, MAX_TIMESTEPS>>
class PrimitivesController
: public Controller<DYN_T, COST_T, FB_T, MAX_TIMESTEPS, NUM_ROLLOUTS, BDIM_X, BDIM_Y, PARAMS_T>
{
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// need control_array = ... so that we can initialize
// Eigen::Matrix with Eigen::Matrix::Zero();
typedef Controller<DYN_T, COST_T, FB_T, MAX_TIMESTEPS, NUM_ROLLOUTS, BDIM_X, BDIM_Y, PARAMS_T> PARENT_CLASS;
using control_array = typename PARENT_CLASS::control_array;
using control_trajectory = typename PARENT_CLASS::control_trajectory;
using state_trajectory = typename PARENT_CLASS::state_trajectory;
using state_array = typename PARENT_CLASS::state_array;
using output_array = typename PARENT_CLASS::output_array;
using sampled_cost_traj = typename PARENT_CLASS::sampled_cost_traj;
using FEEDBACK_GPU = typename PARENT_CLASS::TEMPLATED_FEEDBACK_GPU;
/**
*
* Public member functions
*/
// Constructor
PrimitivesController(DYN_T* model, COST_T* cost, FB_T* fb_controller, float dt, int max_iter, float lambda,
float alpha, const Eigen::Ref<const control_array>& control_std_dev,
int num_timesteps = MAX_TIMESTEPS,
const Eigen::Ref<const control_trajectory>& init_control_traj = control_trajectory::Zero(),
cudaStream_t stream = nullptr);
PrimitivesController(DYN_T* model, COST_T* cost, FB_T* fb_controller, PARAMS_T& params,
cudaStream_t stream = nullptr);
// Destructor
~PrimitivesController();
std::string getControllerName()
{
return "Primitives";
};
/**
* computes a new control sequence
* @param state starting position
*/
void computeControl(const Eigen::Ref<const state_array>& state, int optimization_stride = 1) override;
/**
* Slide the control sequence back n steps
*/
void slideControlSequence(int optimization_stride) override;
void setPercentageSampledControlTrajectories(float new_perc)
{
this->setPercentageSampledControlTrajectoriesHelper(new_perc, 1);
}
void setNumPrimitiveIterations(int new_num_iter)
{
this->params_.num_primitive_iters_ = new_num_iter;
}
int getNumPrimitiveIterations()
{
return this->params_.num_primitive_iters_;
}
void setGamma(float gamma)
{
this->params_.gamma = gamma;
}
float getGamma()
{
return this->params_.gamma;
}
void setRExp(float r)
{
this->params_.r = r;
}
float getRExp()
{
return this->params_.r;
}
void setOffsetDecayRate(float offset_decay_rate)
{
this->params_.offset_decay_rate = offset_decay_rate;
}
float getOffsetDecayRate()
{
return this->params_.offset_decay_rate;
}
void setColoredNoiseExponents(std::vector<float>& new_exponents)
{
this->params_.colored_noise_exponents_ = new_exponents;
}
float getColoredNoiseExponent(int index) const
{
return this->params_.colored_noise_exponents_[index];
}
void setPiecewiseSegments(int segments)
{
this->params_.num_piecewise_segments_ = segments;
}
int getPiecewiseSegments()
{
return this->params_.num_piecewise_segments_;
}
void setScalePiecewiseNoise(std::vector<float>& new_scale)
{
this->params_.scale_piecewise_noise_ = new_scale;
}
std::vector<float> getScalePiecewiseNoise()
{
return this->params_.scale_piecewise_noise_;
}
void setFracRandomNoiseTraj(std::vector<float> frac_add_nominal_traj)
{
this->params_.frac_add_nominal_traj_ = frac_add_nominal_traj;
}
std::vector<float> getFracRandomNoiseTraj()
{
return this->params_.frac_add_nominal_traj_;
}
void setScaleAddNominalNoise(std::vector<float> scale_add_nominal_noise)
{
this->params_.scale_add_nominal_noise_ = scale_add_nominal_noise;
}
std::vector<float> getScaleAddNominalNoise()
{
return this->params_.scale_add_nominal_noise_;
}
void setStateLeashLength(float new_state_leash, int index = 0)
{
this->params_.state_leash_dist_[index] = new_state_leash;
}
float getStateLeashLength(int index)
{
return this->params_.state_leash_dist_[index];
}
bool getLeashActive()
{
return leash_active_;
}
void setLeashActive(bool new_leash_active)
{
leash_active_ = new_leash_active;
}
void setStoppingCostThreshold(float new_stopping_cost_threshold)
{
this->params_.stopping_cost_threshold_ = new_stopping_cost_threshold;
}
float getStoppingCostThreshold()
{
return this->params_.stopping_cost_threshold_;
}
void setHysteresisCostThreshold(float new_hysteresis_cost_threshold)
{
this->params_.hysteresis_cost_threshold_ = new_hysteresis_cost_threshold;
}
float getHysteresisCostThreshold()
{
return this->params_.hysteresis_cost_threshold_;
}
void setVisualizePrimitives(bool visualize_primitives)
{
this->params_.visualize_primitives_ = visualize_primitives;
}
bool getVisualizePrimitives()
{
return this->params_.visualize_primitives_;
}
void calculateSampledStateTrajectories() override;
protected:
std::vector<float>& getScaleAddNominalNoiseLValue()
{
return this->params_.scale_add_nominal_noise_;
}
std::vector<float>& getScalePiecewiseNoiseLValue()
{
return this->params_.scale_piecewise_noise_;
}
std::vector<float>& getFracRandomNoiseTrajLValue()
{
return this->params_.frac_add_nominal_traj_;
}
std::vector<float>& getColoredNoiseExponentsLValue()
{
return this->params_.colored_noise_exponents_;
}
void computeStateTrajectory(const Eigen::Ref<const state_array>& x0);
void copyMPPIControlToDevice(bool synchronize = true);
void computeStoppingTrajectory(const Eigen::Ref<const state_array>& x0);
void smoothControlTrajectory();
int leash_jump_ = 1;
bool leash_active_ = false;
float* control_mppi_d_; // Array of size DYN_T::CONTROL_DIM*NUM_TIMESTEPS
control_trajectory control_mppi_ = control_trajectory::Zero(); // host side mppi control trajectory
Eigen::Matrix<float, DYN_T::CONTROL_DIM, 2> control_mppi_history_;
private:
// ======== MUST BE OVERWRITTEN =========
void allocateCUDAMemory();
// ======== END MUST BE OVERWRITTEN =====
};
#if __CUDACC__
#include "primitives_controller.cu"
#endif
#endif // MPPIGENERIC_PRIMITIVES_CONTROLLER_CUH
================================================
FILE: include/mppi/controllers/R-MPPI/robust_mppi_controller.cu
================================================
#include "robust_mppi_controller.cuh"
#include <Eigen/Eigenvalues>
#include <exception>
#define ROBUST_MPPI_TEMPLATE \
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, class SAMPLING_T, \
class PARAMS_T>
#define RobustMPPI RobustMPPIController<DYN_T, COST_T, FB_T, MAX_TIMESTEPS, NUM_ROLLOUTS, SAMPLING_T, PARAMS_T>
ROBUST_MPPI_TEMPLATE
RobustMPPI::RobustMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, float dt,
int max_iter, float lambda, float alpha, float value_function_threshold,
int num_timesteps, const Eigen::Ref<const control_trajectory>& init_control_traj,
int num_candidate_nominal_states, int optimization_stride, cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, dt, max_iter, lambda, alpha, num_timesteps, init_control_traj,
stream)
{
setValueFunctionThreshold(value_function_threshold);
setOptimizationStride(optimization_stride);
setNumCandidates(num_candidate_nominal_states);
updateNumCandidates(getNumCandidates());
setParams(this->params_);
this->sampler_->setNumDistributions(2);
// Zero the control history
this->control_history_ = Eigen::Matrix<float, DYN_T::CONTROL_DIM, 2>::Zero();
nominal_control_history_ = Eigen::Matrix<float, DYN_T::CONTROL_DIM, 2>::Zero();
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// Copy the noise std_dev to the device
// this->copyControlStdDevToDevice();
// Initialize Feedback
this->fb_controller_->initTrackingController();
// Initialize the nominal control trajectory
nominal_control_trajectory_ = init_control_traj;
this->enable_feedback_ = true;
chooseAppropriateKernel();
}
ROBUST_MPPI_TEMPLATE
RobustMPPI::RobustMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, PARAMS_T& params,
cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, params, stream)
{
updateNumCandidates(getNumCandidates());
setParams(params);
this->sampler_->setNumDistributions(2);
// Zero the control history
this->control_history_ = Eigen::Matrix<float, DYN_T::CONTROL_DIM, 2>::Zero();
nominal_control_history_ = Eigen::Matrix<float, DYN_T::CONTROL_DIM, 2>::Zero();
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// Copy the noise std_dev to the device
// this->copyControlStdDevToDevice();
// Initialize Feedback
this->fb_controller_->initTrackingController();
// Initialize the nominal control trajectory
nominal_control_trajectory_ = this->params_.init_control_traj_;
this->enable_feedback_ = true;
chooseAppropriateKernel();
}
ROBUST_MPPI_TEMPLATE
RobustMPPI::~RobustMPPIController()
{
deallocateNominalStateCandidateMemory();
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::allocateCUDAMemory()
{
PARENT_CLASS::allocateCUDAMemoryHelper(1);
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::chooseAppropriateKernel()
{
// Get properties of current GPU
cudaDeviceProp deviceProp;
HANDLE_ERROR(cudaGetDeviceProperties(&deviceProp, 0));
unsigned single_rollout_kernel_byte_size = mppi::kernels::rmppi::calcRMPPICombinedKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, this->fb_controller_->getHostPointer().get(),
this->params_.dynamics_rollout_dim_);
unsigned rollout_dyn_kernel_byte_size = mppi::kernels::rmppi::calcRMPPIDynKernelSharedMemSize(
this->model_, this->sampler_, this->fb_controller_->getHostPointer().get(), this->params_.dynamics_rollout_dim_);
unsigned rollout_cost_kernel_byte_size = mppi::kernels::rmppi::calcRMPPICostKernelSharedMemSize(
this->cost_, this->sampler_, this->fb_controller_->getHostPointer().get(), this->params_.cost_rollout_dim_);
// Limit kernel choice to those that fit within shared memory constraints
bool rollout_dyn_too_large = rollout_dyn_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool rollout_cost_too_large = rollout_cost_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool rollout_combined_too_large = single_rollout_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool rollout_set = false;
if (rollout_combined_too_large && (rollout_dyn_too_large || rollout_cost_too_large))
{
std::string error_msg =
"There is not enough shared memory on the GPU for either rollout kernel option. The combined rollout kernel "
"takes " +
std::to_string(single_rollout_kernel_byte_size) + " bytes, the cost rollout kernel takes " +
std::to_string(rollout_cost_kernel_byte_size) + " bytes, the dynamics rollout kernel takes " +
std::to_string(rollout_dyn_kernel_byte_size) + " bytes, and the max is " +
std::to_string(deviceProp.sharedMemPerBlock) +
" bytes. Considering lowering the corresponding thread block sizes.";
throw std::runtime_error(error_msg);
}
else if (rollout_dyn_too_large || rollout_cost_too_large)
{
this->setKernelChoice(kernelType::USE_SINGLE_KERNEL);
rollout_set = true;
}
else if (rollout_combined_too_large)
{
this->setKernelChoice(kernelType::USE_SPLIT_KERNELS);
rollout_set = true;
}
/**
* Set up for kernel comparison
*/
state_array zero_state = this->model_->getZeroState();
float* initial_state_nominal_d = this->initial_state_d_;
float* initial_state_real_d = this->initial_state_d_ + DYN_T::STATE_DIM;
// Transfer the initial state to the GPU
HANDLE_ERROR(cudaMemcpyAsync(initial_state_real_d, zero_state.data(), sizeof(float) * DYN_T::STATE_DIM,
cudaMemcpyHostToDevice, this->stream_));
HANDLE_ERROR(cudaMemcpyAsync(initial_state_nominal_d, zero_state.data(), sizeof(float) * DYN_T::STATE_DIM,
cudaMemcpyHostToDevice, this->stream_));
// Copy the importance sampling control to the system
this->sampler_->copyImportanceSamplerToDevice(nominal_control_trajectory_.data(), 0, false);
this->sampler_->copyImportanceSamplerToDevice(nominal_control_trajectory_.data(), 1, false);
// Send feedback gains to the GPU
this->fb_controller_->copyToDevice(false);
// Generate a the control perturbations for exploration
this->sampler_->generateSamples(1, 0, this->gen_, true);
float single_rollout_kernel_time_ms = std::numeric_limits<float>::infinity();
float split_rollout_kernel_time_ms = std::numeric_limits<float>::infinity();
auto start_rollout_single_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !rollout_set; i++)
{
mppi::kernels::rmppi::launchRMPPIRolloutKernel<DYN_T, COST_T, SAMPLING_T, FEEDBACK_GPU>(
this->model_, this->cost_, this->sampler_, this->fb_controller_->getHostPointer().get(), this->getDt(),
this->getNumTimesteps(), NUM_ROLLOUTS, this->getLambda(), this->getAlpha(), getValueFunctionThreshold(),
this->initial_state_d_, this->trajectory_costs_d_, this->params_.dynamics_rollout_dim_, this->stream_, true);
}
auto end_rollout_single_kernel_time = std::chrono::steady_clock::now();
auto start_rollout_split_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !rollout_set; i++)
{
mppi::kernels::rmppi::launchSplitRMPPIRolloutKernel<DYN_T, COST_T, SAMPLING_T, FEEDBACK_GPU>(
this->model_, this->cost_, this->sampler_, this->fb_controller_->getHostPointer().get(), this->getDt(),
this->getNumTimesteps(), NUM_ROLLOUTS, this->getLambda(), this->getAlpha(), getValueFunctionThreshold(),
this->initial_state_d_, this->output_d_, this->trajectory_costs_d_, this->params_.dynamics_rollout_dim_,
this->params_.cost_rollout_dim_, this->stream_, true);
}
auto end_rollout_split_kernel_time = std::chrono::steady_clock::now();
if (!rollout_set)
{
single_rollout_kernel_time_ms =
mppi::math::timeDiffms(end_rollout_single_kernel_time, start_rollout_single_kernel_time);
split_rollout_kernel_time_ms =
mppi::math::timeDiffms(end_rollout_split_kernel_time, start_rollout_split_kernel_time);
}
std::string kernel_choice = "";
if (split_rollout_kernel_time_ms < single_rollout_kernel_time_ms)
{
this->setKernelChoice(kernelType::USE_SPLIT_KERNELS);
kernel_choice = "split ";
}
else
{
this->setKernelChoice(kernelType::USE_SINGLE_KERNEL);
kernel_choice = "single";
}
this->logger_->info(
"Choosing %s rollout kernel based on split taking %f ms and single taking %f ms after %d iterations\n",
kernel_choice.c_str(), split_rollout_kernel_time_ms, single_rollout_kernel_time_ms,
this->getNumKernelEvaluations());
// Do the same for the eval kernel
chooseAppropriateEvalKernel();
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::chooseAppropriateEvalKernel()
{
// Get properties of current GPU
cudaDeviceProp deviceProp;
HANDLE_ERROR(cudaGetDeviceProperties(&deviceProp, 0));
// Get shared mem sizes for various kernels
unsigned single_eval_kernel_byte_size = mppi::kernels::rmppi::calcEvalCombinedKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, getNumEvalRollouts(), getNumEvalSamplesPerCandidate(),
this->params_.eval_dyn_kernel_dim_);
unsigned eval_dyn_kernel_byte_size = mppi::kernels::rmppi::calcEvalDynKernelSharedMemSize(
this->model_, this->sampler_, this->params_.eval_dyn_kernel_dim_);
unsigned eval_cost_kernel_byte_size = mppi::kernels::rmppi::calcEvalCostKernelSharedMemSize(
this->cost_, this->sampler_, getNumEvalRollouts(), getNumEvalSamplesPerCandidate(),
this->params_.eval_cost_kernel_dim_);
// Limit kernel choice to those that fit within shared memory constraints
bool eval_dyn_too_large = eval_dyn_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool eval_cost_too_large = eval_cost_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool eval_combined_too_large = single_eval_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool eval_set = false;
if (eval_combined_too_large && (eval_dyn_too_large || eval_cost_too_large))
{
std::string error_msg =
"There is not enough shared memory on the GPU for either eval kernel option. The combined eval kernel "
"takes " +
std::to_string(single_eval_kernel_byte_size) + " bytes, the cost eval kernel takes " +
std::to_string(eval_cost_kernel_byte_size) + " bytes, the dynamics eval kernel takes " +
std::to_string(eval_dyn_kernel_byte_size) + " bytes, and the max is " +
std::to_string(deviceProp.sharedMemPerBlock) +
" bytes. Considering lowering the corresponding thread block sizes.";
throw std::runtime_error(error_msg);
}
else if (eval_dyn_too_large || eval_cost_too_large)
{
this->setEvalKernelChoice(kernelType::USE_SINGLE_KERNEL);
eval_set = true;
}
else if (eval_combined_too_large)
{
this->setEvalKernelChoice(kernelType::USE_SPLIT_KERNELS);
eval_set = true;
}
// Send the nominal state candidates to the GPU
HANDLE_ERROR(cudaMemcpyAsync(importance_sampling_states_d_, candidate_nominal_states_.data(),
sizeof(float) * DYN_T::STATE_DIM * getNumCandidates(), cudaMemcpyHostToDevice,
this->stream_));
Eigen::MatrixXi temp_importance_sampler_strides = Eigen::MatrixXi::Ones(getNumCandidates(), 1);
// Send the importance sampler strides to the GPU
HANDLE_ERROR(cudaMemcpyAsync(importance_sampling_strides_d_, temp_importance_sampler_strides.data(),
sizeof(int) * getNumCandidates(), cudaMemcpyHostToDevice, this->stream_));
// Send the nominal control to the GPU
copyNominalControlToDevice(false);
// Generate noise for the samples
this->sampler_->generateSamples(1, 0, this->gen_, true);
float single_eval_kernel_time_ms = std::numeric_limits<float>::infinity();
float split_eval_kernel_time_ms = std::numeric_limits<float>::infinity();
auto start_eval_split_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !eval_set; i++)
{
mppi::kernels::rmppi::launchSplitInitEvalKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), getNumEvalRollouts(),
this->getLambda(), this->getAlpha(), getNumEvalSamplesPerCandidate(), importance_sampling_strides_d_,
importance_sampling_states_d_, importance_sampling_outputs_d_, importance_sampling_costs_d_,
this->params_.eval_dyn_kernel_dim_, this->params_.eval_cost_kernel_dim_, this->stream_, true);
}
auto end_eval_split_kernel_time = std::chrono::steady_clock::now();
auto start_eval_single_kernel_time = std::chrono::steady_clock::now();
for (int i = 0; i < this->getNumKernelEvaluations() && !eval_set; i++)
{
mppi::kernels::rmppi::launchInitEvalKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), getNumEvalRollouts(),
this->getLambda(), this->getAlpha(), getNumEvalSamplesPerCandidate(), importance_sampling_strides_d_,
importance_sampling_states_d_, importance_sampling_costs_d_, this->params_.eval_dyn_kernel_dim_, this->stream_,
true);
}
auto end_eval_single_kernel_time = std::chrono::steady_clock::now();
if (!eval_set)
{
single_eval_kernel_time_ms = mppi::math::timeDiffms(end_eval_single_kernel_time, start_eval_single_kernel_time);
split_eval_kernel_time_ms = mppi::math::timeDiffms(end_eval_split_kernel_time, start_eval_split_kernel_time);
}
std::string kernel_choice = "";
if (split_eval_kernel_time_ms < single_eval_kernel_time_ms)
{
this->setEvalKernelChoice(kernelType::USE_SPLIT_KERNELS);
kernel_choice = "split ";
}
else
{
this->setEvalKernelChoice(kernelType::USE_SINGLE_KERNEL);
kernel_choice = "single";
}
this->logger_->info(
"Choosing %s eval kernel based on split taking %f ms and single taking %f ms after %d iterations\n",
kernel_choice.c_str(), split_eval_kernel_time_ms, single_eval_kernel_time_ms, this->getNumKernelEvaluations());
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::setParams(const PARAMS_T& p)
{
bool empty_eval_dyn_size = p.eval_dyn_kernel_dim_.x == 0;
bool changed_sample_size = p.eval_dyn_kernel_dim_.x != this->params_.eval_dyn_kernel_dim_.x;
bool changed_num_candidates = p.num_candidate_nominal_states_ != this->params_.num_candidate_nominal_states_;
PARENT_CLASS::setParams(p);
this->params_.dynamics_rollout_dim_.z = max(2, p.dynamics_rollout_dim_.z);
this->params_.cost_rollout_dim_.z = max(2, p.cost_rollout_dim_.z);
// Set up cost eval kernel dimensions
if (p.eval_cost_kernel_dim_.x == 0)
{
this->params_.eval_cost_kernel_dim_.x = this->getNumTimesteps();
}
this->params_.eval_cost_kernel_dim_.y = max(1, p.eval_cost_kernel_dim_.y);
this->params_.eval_cost_kernel_dim_.z = max(1, p.eval_cost_kernel_dim_.z);
// Set up dynamics eval kernel dimensions
if (empty_eval_dyn_size)
{
this->params_.eval_dyn_kernel_dim_.x = 32;
changed_sample_size = true;
}
this->params_.eval_dyn_kernel_dim_.y = max(1, p.eval_dyn_kernel_dim_.y);
this->params_.eval_dyn_kernel_dim_.z = max(1, p.eval_dyn_kernel_dim_.z);
if (changed_sample_size)
{
updateCandidateMemory();
}
else if (changed_num_candidates)
{
updateNumCandidates(p.num_candidate_nominal_states_);
}
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::getInitNominalStateCandidates(const Eigen::Ref<const state_array>& nominal_x_k,
const Eigen::Ref<const state_array>& nominal_x_kp1,
const Eigen::Ref<const state_array>& real_x_kp1)
{
Eigen::MatrixXf points(DYN_T::STATE_DIM, 3);
points << nominal_x_k, nominal_x_kp1, real_x_kp1;
auto candidates = points * line_search_weights_;
for (int i = 0; i < getNumCandidates(); ++i)
{
candidate_nominal_states_[i] = candidates.col(i);
}
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::resetCandidateCudaMem()
{
deallocateNominalStateCandidateMemory();
#if defined(CUDART_VERSION) && CUDART_VERSION > 11200
HANDLE_ERROR(
cudaMallocAsync((void**)&importance_sampling_costs_d_, sizeof(float) * getNumEvalRollouts(), this->stream_));
HANDLE_ERROR(cudaMallocAsync((void**)&importance_sampling_outputs_d_,
sizeof(float) * getNumEvalRollouts() * this->getNumTimesteps() * DYN_T::OUTPUT_DIM,
this->stream_));
HANDLE_ERROR(cudaMallocAsync((void**)&importance_sampling_states_d_,
sizeof(float) * DYN_T::STATE_DIM * getNumCandidates(), this->stream_));
HANDLE_ERROR(
cudaMallocAsync((void**)&importance_sampling_strides_d_, sizeof(int) * getNumCandidates(), this->stream_));
#else
HANDLE_ERROR(cudaMalloc((void**)&importance_sampling_costs_d_, sizeof(float) * getNumEvalRollouts()));
HANDLE_ERROR(cudaMalloc((void**)&importance_sampling_outputs_d_,
sizeof(float) * getNumEvalRollouts() * this->getNumTimesteps() * DYN_T::OUTPUT_DIM));
HANDLE_ERROR(
cudaMalloc((void**)&importance_sampling_states_d_, sizeof(float) * DYN_T::STATE_DIM * getNumCandidates()));
HANDLE_ERROR(cudaMalloc((void**)&importance_sampling_strides_d_, sizeof(int) * getNumCandidates()));
#endif
// Set flag so that the we know cudamemory is allocated
importance_sampling_cuda_mem_init_ = true;
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::deallocateNominalStateCandidateMemory()
{
if (importance_sampling_cuda_mem_init_)
{
#if defined(CUDART_VERSION) && CUDART_VERSION > 11200
HANDLE_ERROR(cudaFreeAsync(importance_sampling_costs_d_, this->stream_));
HANDLE_ERROR(cudaFreeAsync(importance_sampling_outputs_d_, this->stream_));
HANDLE_ERROR(cudaFreeAsync(importance_sampling_states_d_, this->stream_));
HANDLE_ERROR(cudaFreeAsync(importance_sampling_strides_d_, this->stream_));
#else
HANDLE_ERROR(cudaFree(importance_sampling_costs_d_));
HANDLE_ERROR(cudaFree(importance_sampling_outputs_d_));
HANDLE_ERROR(cudaFree(importance_sampling_states_d_));
HANDLE_ERROR(cudaFree(importance_sampling_strides_d_));
#endif
importance_sampling_costs_d_ = nullptr;
importance_sampling_outputs_d_ = nullptr;
importance_sampling_states_d_ = nullptr;
importance_sampling_strides_d_ = nullptr;
// Set flag so that we know cudamemory has been freed
importance_sampling_cuda_mem_init_ = false;
}
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::copyNominalControlToDevice(bool synchronize)
{
this->sampler_->copyImportanceSamplerToDevice(nominal_control_trajectory_.data(), 0, synchronize);
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::updateNumCandidates(int new_num_candidates)
{
if ((new_num_candidates * getNumEvalSamplesPerCandidate()) > NUM_ROLLOUTS)
{
std::cerr << "ERROR: (number of candidates) * (SAMPLES_PER_CANDIDATE) cannot exceed NUM_ROLLOUTS\n";
std::cerr << "number of candidates: " << new_num_candidates
<< ", SAMPLES_PER_CANDIDATE: " << getNumEvalSamplesPerCandidate() << ", NUM_ROLLOUTS: " << NUM_ROLLOUTS
<< "\n";
std::terminate();
}
// New number must be odd and greater than 3
if (new_num_candidates < 3)
{
std::cerr << "ERROR: number of candidates must be greater or equal to 3\n";
std::cerr << "number of candidates: " << new_num_candidates << "\n";
std::terminate();
}
if (new_num_candidates % 2 == 0)
{
std::cerr << "ERROR: number of candidates must be odd\n";
std::cerr << "number of candidates: " << new_num_candidates << "\n";
std::terminate();
}
// Set the new value of the number of candidates
setNumCandidates(new_num_candidates);
updateCandidateMemory();
// Recompute the line search weights based on the number of candidates
computeLineSearchWeights();
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::updateCandidateMemory()
{
// Resize the vector holding the candidate nominal states
candidate_nominal_states_.resize(getNumCandidates());
// Resize the matrix holding the importance sampler strides
importance_sampler_strides_.resize(getNumCandidates(), 1);
// Resize the trajectory costs matrix
candidate_trajectory_costs_.resize(getNumEvalRollouts(), 1);
candidate_trajectory_costs_.setZero();
// Resize the free energy costs matrix
candidate_free_energy_.resize(getNumCandidates(), 1);
candidate_free_energy_.setZero();
// Deallocate and reallocate cuda memory
resetCandidateCudaMem();
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::computeLineSearchWeights()
{
line_search_weights_.resize(3, getNumCandidates());
// For a given setup, this never changes.... why recompute every time?
int num_candid_over_2 = getNumCandidates() / 2;
for (int i = 0; i < num_candid_over_2 + 1; i++)
{
line_search_weights_(0, i) = 1 - i / float(num_candid_over_2);
line_search_weights_(1, i) = i / float(num_candid_over_2);
line_search_weights_(2, i) = 0.0;
}
for (int i = 1; i < num_candid_over_2 + 1; i++)
{
line_search_weights_(0, num_candid_over_2 + i) = 0.0;
line_search_weights_(1, num_candid_over_2 + i) = 1 - i / float(num_candid_over_2);
line_search_weights_(2, num_candid_over_2 + i) = i / float(num_candid_over_2);
}
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::computeImportanceSamplerStride(int stride)
{
Eigen::MatrixXf stride_vec(1, 3);
stride_vec << 0, stride, stride;
// Perform matrix multiplication, convert to array so that we can round the floats to the nearest
// integer. Then cast the resultant float array to an int array. Then set equal to our int matrix.
importance_sampler_strides_ = (stride_vec * line_search_weights_).array().round().template cast<int>();
// importance_sampler_strides_.array() += stride;
}
ROBUST_MPPI_TEMPLATE
float RobustMPPI::computeCandidateBaseline()
{
float baseline = candidate_trajectory_costs_(0);
for (int i = 1; i < getNumEvalRollouts(); i++)
{ // TODO What is the reasoning behind only using the first condition to get the baseline?
if (candidate_trajectory_costs_(i) < baseline)
{
baseline = candidate_trajectory_costs_(i);
}
}
return baseline;
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::computeBestIndex()
{
candidate_free_energy_.setZero();
float baseline = computeCandidateBaseline();
for (int i = 0; i < getNumCandidates(); i++)
{
for (int j = 0; j < getNumEvalSamplesPerCandidate(); j++)
{
candidate_free_energy_(i) += expf(
-1.0 / this->getLambda() * (candidate_trajectory_costs_(i * getNumEvalSamplesPerCandidate() + j) - baseline));
}
candidate_free_energy_(i) /= (1.0 * getNumEvalSamplesPerCandidate());
candidate_free_energy_(i) = -this->getLambda() * logf(candidate_free_energy_(i)) + baseline;
if (candidate_free_energy_(i) < getValueFunctionThreshold())
{
best_index_ = i;
}
}
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::updateImportanceSamplingControl(const Eigen::Ref<const state_array>& state, int stride)
{
// (Controller Frequency)*(Optimization Time) corresponds to how many timesteps occurred in the last optimization
real_stride_ = stride;
computeNominalStateAndStride(state, stride); // Launches the init eval kernel
// Save the nominal control history for the importance sampler
this->saveControlHistoryHelper(nominal_stride_, nominal_control_trajectory_, nominal_control_history_);
// Save the real control history for the optimal control
this->saveControlHistoryHelper(real_stride_, this->control_, this->control_history_);
// Slide the control sequence for the nominal control trajectory
this->slideControlSequenceHelper(nominal_stride_, nominal_control_trajectory_);
// Compute the nominal trajectory because we have slid the control sequence and updated the nominal state
this->computeStateTrajectoryHelper(nominal_state_trajectory_, nominal_state_, nominal_control_trajectory_);
// Compute the feedback gains and save them to an array
computeNominalFeedbackGains(state);
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::computeNominalStateAndStride(const Eigen::Ref<const state_array>& state, int stride)
{
if (!nominal_state_init_)
{
nominal_state_ = state;
nominal_state_init_ = true;
nominal_stride_ = 0;
}
else
{
getInitNominalStateCandidates(nominal_state_trajectory_.col(0), nominal_state_trajectory_.col(1), state);
computeImportanceSamplerStride(stride);
// Send the nominal state candidates to the GPU
HANDLE_ERROR(cudaMemcpyAsync(importance_sampling_states_d_, candidate_nominal_states_.data(),
sizeof(float) * DYN_T::STATE_DIM * getNumCandidates(), cudaMemcpyHostToDevice,
this->stream_));
// Send the importance sampler strides to the GPU
HANDLE_ERROR(cudaMemcpyAsync(importance_sampling_strides_d_, importance_sampler_strides_.data(),
sizeof(int) * getNumCandidates(), cudaMemcpyHostToDevice, this->stream_));
// Send the nominal control to the GPU
copyNominalControlToDevice(false);
// Generate noise for the samples
this->sampler_->generateSamples(stride, 0, this->gen_, false);
// Launch the init eval kernel
if (this->getEvalKernelChoiceAsEnum() == kernelType::USE_SPLIT_KERNELS)
{
mppi::kernels::rmppi::launchSplitInitEvalKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), getNumEvalRollouts(),
this->getLambda(), this->getAlpha(), getNumEvalSamplesPerCandidate(), importance_sampling_strides_d_,
importance_sampling_states_d_, importance_sampling_outputs_d_, importance_sampling_costs_d_,
this->params_.eval_dyn_kernel_dim_, this->params_.eval_cost_kernel_dim_, this->stream_, false);
}
else if (this->getEvalKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{
mppi::kernels::rmppi::launchInitEvalKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), getNumEvalRollouts(),
this->getLambda(), this->getAlpha(), getNumEvalSamplesPerCandidate(), importance_sampling_strides_d_,
importance_sampling_states_d_, importance_sampling_costs_d_, this->params_.eval_dyn_kernel_dim_,
this->stream_, false);
}
HANDLE_ERROR(cudaMemcpyAsync(candidate_trajectory_costs_.data(), importance_sampling_costs_d_,
sizeof(float) * getNumEvalRollouts(), cudaMemcpyDeviceToHost, this->stream_));
cudaStreamSynchronize(this->stream_);
// Compute the best nominal state candidate from the rollouts
computeBestIndex();
// best_index_ = 8;
this->free_energy_statistics_.nominal_state_used = best_index_;
nominal_stride_ = importance_sampler_strides_(best_index_);
nominal_state_ = candidate_nominal_states_[best_index_];
}
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::computeNominalFeedbackGains(const Eigen::Ref<const state_array>& state)
{
this->computeFeedbackHelper(state, nominal_state_trajectory_, nominal_control_trajectory_);
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::computeControl(const Eigen::Ref<const state_array>& state, int optimization_stride)
{
// Handy dandy pointers to nominal data
float* trajectory_costs_nominal_d = this->trajectory_costs_d_;
float* trajectory_costs_real_d = this->trajectory_costs_d_ + NUM_ROLLOUTS;
float* initial_state_nominal_d = this->initial_state_d_;
float* initial_state_real_d = this->initial_state_d_ + DYN_T::STATE_DIM;
this->free_energy_statistics_.nominal_sys.previousBaseline = this->getBaselineCost(0);
this->free_energy_statistics_.real_sys.previousBaseline = this->getBaselineCost(1);
// Transfer the feedback gains to the GPU
this->fb_controller_->copyToDevice(false);
// Transfer the real initial state to the GPU
HANDLE_ERROR(cudaMemcpyAsync(initial_state_real_d, state.data(), sizeof(float) * DYN_T::STATE_DIM,
cudaMemcpyHostToDevice, this->stream_));
// Transfer the nominal state to the GPU: recall that the device GPU has the augmented state [nominal state, real
// state]
HANDLE_ERROR(cudaMemcpyAsync(initial_state_nominal_d, nominal_state_.data(), sizeof(float) * DYN_T::STATE_DIM,
cudaMemcpyHostToDevice, this->stream_));
for (int opt_iter = 0; opt_iter < this->getNumIters(); opt_iter++)
{
// Copy the importance sampling control to the system
this->sampler_->copyImportanceSamplerToDevice(nominal_control_trajectory_.data(), 0, false);
this->sampler_->copyImportanceSamplerToDevice(nominal_control_trajectory_.data(), 1, false);
// Generate a the control perturbations for exploration
this->sampler_->generateSamples(optimization_stride, opt_iter, this->gen_, false);
// Launch the rollout kernel
if (this->getKernelChoiceAsEnum() == kernelType::USE_SPLIT_KERNELS)
{
mppi::kernels::rmppi::launchSplitRMPPIRolloutKernel<DYN_T, COST_T, SAMPLING_T, FEEDBACK_GPU>(
this->model_, this->cost_, this->sampler_, this->fb_controller_->getHostPointer().get(), this->getDt(),
this->getNumTimesteps(), NUM_ROLLOUTS, this->getLambda(), this->getAlpha(), getValueFunctionThreshold(),
this->initial_state_d_, this->output_d_, this->trajectory_costs_d_, this->params_.dynamics_rollout_dim_,
this->params_.cost_rollout_dim_, this->stream_, false);
}
else
{
mppi::kernels::rmppi::launchRMPPIRolloutKernel<DYN_T, COST_T, SAMPLING_T, FEEDBACK_GPU>(
this->model_, this->cost_, this->sampler_, this->fb_controller_->getHostPointer().get(), this->getDt(),
this->getNumTimesteps(), NUM_ROLLOUTS, this->getLambda(), this->getAlpha(), getValueFunctionThreshold(),
this->initial_state_d_, this->trajectory_costs_d_, this->params_.dynamics_rollout_dim_, this->stream_, false);
}
// Return the costs -> nominal, real costs
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), trajectory_costs_real_d, NUM_ROLLOUTS * sizeof(float),
cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaMemcpyAsync(trajectory_costs_nominal_.data(), trajectory_costs_nominal_d,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
// Launch the norm exponential kernels for the nominal costs and the real costs
this->setBaseline(mppi::kernels::computeBaselineCost(trajectory_costs_nominal_.data(), NUM_ROLLOUTS), 0);
this->setBaseline(mppi::kernels::computeBaselineCost(this->trajectory_costs_.data(), NUM_ROLLOUTS), 1);
// In this case this->gamma = 1 / lambda
mppi::kernels::launchNormExpKernel(NUM_ROLLOUTS, this->getNormExpThreads(), trajectory_costs_nominal_d,
1.0 / this->getLambda(), this->getBaselineCost(0), this->stream_, false);
mppi::kernels::launchNormExpKernel(NUM_ROLLOUTS, this->getNormExpThreads(), trajectory_costs_real_d,
1.0 / this->getLambda(), this->getBaselineCost(1), this->stream_, false);
HANDLE_ERROR(cudaMemcpyAsync(this->trajectory_costs_.data(), trajectory_costs_real_d, NUM_ROLLOUTS * sizeof(float),
cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaMemcpyAsync(trajectory_costs_nominal_.data(), trajectory_costs_nominal_d,
NUM_ROLLOUTS * sizeof(float), cudaMemcpyDeviceToHost, this->stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->stream_));
// Launch the weighted reduction kernel for the nominal costs and the real costs
this->setNormalizer(mppi::kernels::computeNormalizer(trajectory_costs_nominal_.data(), NUM_ROLLOUTS), 0);
this->setNormalizer(mppi::kernels::computeNormalizer(this->trajectory_costs_.data(), NUM_ROLLOUTS), 1);
// Compute real free energy
mppi::kernels::computeFreeEnergy(this->free_energy_statistics_.real_sys.freeEnergyMean,
this->free_energy_statistics_.real_sys.freeEnergyVariance,
this->free_energy_statistics_.real_sys.freeEnergyModifiedVariance,
this->trajectory_costs_.data(), NUM_ROLLOUTS, this->getBaselineCost(1),
this->getLambda());
// Compute Nominal State free Energy
mppi::kernels::computeFreeEnergy(this->free_energy_statistics_.nominal_sys.freeEnergyMean,
this->free_energy_statistics_.nominal_sys.freeEnergyVariance,
this->free_energy_statistics_.nominal_sys.freeEnergyModifiedVariance,
this->trajectory_costs_nominal_.data(), NUM_ROLLOUTS, this->getBaselineCost(0),
this->getLambda());
// Calculate new optimal trajectories
this->sampler_->updateDistributionParamsFromDevice(trajectory_costs_nominal_d, this->getNormalizerCost(0), 0,
false);
this->sampler_->updateDistributionParamsFromDevice(trajectory_costs_real_d, this->getNormalizerCost(1), 1, false);
// Transfer the new control to the host
this->sampler_->setHostOptimalControlSequence(nominal_control_trajectory_.data(), 0, false);
this->sampler_->setHostOptimalControlSequence(this->control_.data(), 1, true);
}
// Smooth the control
this->smoothControlTrajectoryHelper(this->control_, this->control_history_);
this->smoothControlTrajectoryHelper(nominal_control_trajectory_, nominal_control_history_);
// Compute the nominal trajectory because we updated the nominal control!
this->computeStateTrajectoryHelper(nominal_state_trajectory_, nominal_state_, nominal_control_trajectory_);
this->free_energy_statistics_.real_sys.normalizerPercent = this->getNormalizerCost(1) / NUM_ROLLOUTS;
this->free_energy_statistics_.real_sys.increase =
this->getBaselineCost(1) - this->free_energy_statistics_.real_sys.previousBaseline;
this->free_energy_statistics_.nominal_sys.normalizerPercent = this->getNormalizerCost(0) / NUM_ROLLOUTS;
this->free_energy_statistics_.nominal_sys.increase =
this->getBaselineCost(0) - this->free_energy_statistics_.nominal_sys.previousBaseline;
// Copy back sampled trajectories
this->copySampledControlFromDevice(false);
if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{ // copy initial state to vis initial state for use with visualizeKernel
HANDLE_ERROR(cudaMemcpyAsync(this->vis_initial_state_d_, this->initial_state_d_,
sizeof(float) * DYN_T::STATE_DIM * 2, cudaMemcpyDeviceToDevice, this->vis_stream_));
}
this->copyTopControlFromDevice(true);
}
ROBUST_MPPI_TEMPLATE
float RobustMPPI::computeDF()
{
return (this->getFeedbackPropagatedStateSeq().col(0) - this->getFeedbackPropagatedStateSeq().col(1)).norm() +
(this->getTargetStateSeq().col(0) - this->getFeedbackPropagatedStateSeq().col(0)).norm();
}
ROBUST_MPPI_TEMPLATE
void RobustMPPI::calculateSampledStateTrajectories()
{
int num_sampled_trajectories = this->getTotalSampledTrajectories();
// control already copied in compute control, so run kernel
if (this->getKernelChoiceAsEnum() == kernelType::USE_SPLIT_KERNELS)
{
mppi::kernels::launchVisualizeCostKernel<COST_T, SAMPLING_T>(
this->cost_->cost_d_, this->sampler_->sampling_d_, this->getDt(), this->getNumTimesteps(),
num_sampled_trajectories, this->getLambda(), this->getAlpha(), this->sampled_outputs_d_,
this->sampled_crash_status_d_, this->sampled_costs_d_, this->params_.cost_rollout_dim_, this->stream_, false);
}
else if (this->getKernelChoiceAsEnum() == kernelType::USE_SINGLE_KERNEL)
{
mppi::kernels::launchVisualizeKernel<DYN_T, COST_T, SAMPLING_T>(
this->model_, this->cost_, this->sampler_, this->getDt(), this->getNumTimesteps(), num_sampled_trajectories,
this->getLambda(), this->getAlpha(), this->vis_initial_state_d_, this->sampled_outputs_d_,
this->sampled_costs_d_, this->sampled_crash_status_d_, this->params_.visualize_dim_, this->stream_, false);
}
// copy back results
for (int i = 0; i < num_sampled_trajectories * 2; i++)
{
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_trajectories_[i].data(),
this->sampled_outputs_d_ + i * this->getNumTimesteps() * DYN_T::OUTPUT_DIM,
this->getNumTimesteps() * DYN_T::OUTPUT_DIM * sizeof(float), cudaMemcpyDeviceToHost,
this->vis_stream_));
}
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_costs_.data(), this->sampled_costs_d_,
this->getNumTimesteps() * 2 * sizeof(float), cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(cudaMemcpyAsync(this->sampled_crash_status_.data(), this->sampled_crash_status_d_,
this->getNumTimesteps() * 2 * sizeof(float), cudaMemcpyDeviceToHost, this->vis_stream_));
HANDLE_ERROR(cudaStreamSynchronize(this->vis_stream_));
}
================================================
FILE: include/mppi/controllers/R-MPPI/robust_mppi_controller.cuh
================================================
/*
* Software License Agreement (BSD License)
* Copyright (c) 2013, Georgia Institute of Technology
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/**********************************************
* @file mppi_controller.cuh
* @author Grady Williams <gradyrw@gmail.com>
* @date May 24, 2017
* @copyright 2017 Georgia Institute of Technology
* @brief Class definition for the MPPI controller.
***********************************************/
#ifndef MPPI_GEN_RMPPI_CONTROLLER_CUH_
#define MPPI_GEN_RMPPI_CONTROLLER_CUH_
#include <curand.h>
#include <mppi/controllers/controller.cuh>
#include <mppi/sampling_distributions/gaussian/gaussian.cuh>
#include <mppi/core/rmppi_kernels.cuh>
// Needed for list of candidate states
#include <mppi/ddp/util.h>
template <int S_DIM, int C_DIM, int MAX_TIMESTEPS>
struct RobustMPPIParams : public ControllerParams<S_DIM, C_DIM, MAX_TIMESTEPS>
{
float value_function_threshold_ = 1000.0;
int optimization_stride_ = 1;
int num_candidate_nominal_states_ = 9;
dim3 eval_cost_kernel_dim_;
dim3 eval_dyn_kernel_dim_;
};
// template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS = 2560, int BDIM_X = 64,
// int BDIM_Y = 1, class PARAMS_T = RobustMPPIParams<DYN_T::STATE_DIM, DYN_T::CONTROL_DIM, MAX_TIMESTEPS>,
// int SAMPLES_PER_CONDITION_MULTIPLIER = 1>
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS = 2560,
class SAMPLING_T = ::mppi::sampling_distributions::GaussianDistribution<typename DYN_T::DYN_PARAMS_T>,
class PARAMS_T = RobustMPPIParams<DYN_T::STATE_DIM, DYN_T::CONTROL_DIM, MAX_TIMESTEPS>>
class RobustMPPIController : public Controller<DYN_T, COST_T, FB_T, SAMPLING_T, MAX_TIMESTEPS, NUM_ROLLOUTS, PARAMS_T>
{
public:
/**
* Set up useful types
*/
typedef Controller<DYN_T, COST_T, FB_T, SAMPLING_T, MAX_TIMESTEPS, NUM_ROLLOUTS, PARAMS_T> PARENT_CLASS;
using control_array = typename PARENT_CLASS::control_array;
using control_trajectory = typename PARENT_CLASS::control_trajectory;
using state_trajectory = typename PARENT_CLASS::state_trajectory;
using state_array = typename PARENT_CLASS::state_array;
using sampled_cost_traj = typename PARENT_CLASS::sampled_cost_traj;
using FEEDBACK_STATE = typename PARENT_CLASS::TEMPLATED_FEEDBACK_STATE;
using FEEDBACK_PARAMS = typename PARENT_CLASS::TEMPLATED_FEEDBACK_PARAMS;
using FEEDBACK_GPU = typename PARENT_CLASS::TEMPLATED_FEEDBACK_GPU;
using NominalCandidateVector = typename util::NamedEigenAlignedVector<state_array>;
static const int STATE_DIM = DYN_T::STATE_DIM;
static const int CONTROL_DIM = DYN_T::CONTROL_DIM;
// Number of samples per condition must be a multiple of the blockDIM
// static const int SAMPLES_PER_CANDIDATE = SAMPLES_PER_CANDIDATE_MULTIPLIER;
int getNumEvalSamplesPerCandidate() const
{
return this->params_.eval_dyn_kernel_dim_.x;
}
// float value_function_threshold_ = 1000.0;
state_array nominal_state_ = state_array::Zero();
/**
* @brief Constructor for mppi controller class
* @param model A basis function model of the system dynamics.
* @param cost An MppiCost object.
* @param dt The time increment. horizon = num_timesteps*dt
* @param max_iter number of times to repeat control sequence calculations
* @param gamma
* @param num_timesteps The number of timesteps to look ahead for.
* TODO Finish this description
*/
RobustMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, float dt, int max_iter,
float lambda, float alpha, float value_function_threshold, int num_timesteps = MAX_TIMESTEPS,
const Eigen::Ref<const control_trajectory>& init_control_traj = control_trajectory::Zero(),
int num_candidate_nominal_states = 9, int optimization_stride = 1,
cudaStream_t stream = nullptr);
RobustMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, PARAMS_T& params,
cudaStream_t stream = nullptr);
/**
* @brief Destructor for mppi controller class.
*/
~RobustMPPIController();
std::string getControllerName()
{
return "Robust MPPI";
};
// Initializes the num_candidates, candidate_nominal_states, linesearch_weights,
// and allocates the associated CUDA memory
void updateNumCandidates(int new_num_candidates);
// Update the importance sampler prior to calling computeControl
void updateImportanceSamplingControl(const Eigen::Ref<const state_array>& state, int stride);
/**
* @brief Compute the control given the current state of the system.
* @param state The current state of the autorally system.
*/
void computeControl(const Eigen::Ref<const state_array>& state, int optimization_stride = 1);
control_trajectory getControlSeq() const override
{
return this->control_;
};
state_trajectory getTargetStateSeq() const override
{
return nominal_state_trajectory_;
};
state_array getNominalState() const
{
return nominal_state_;
}
int getBestIndex() const
{
return best_index_;
};
float getValueFunctionThreshold() const
{
return this->params_.value_function_threshold_;
}
int getOptimizationStride() const
{
return this->params_.optimization_stride_;
}
int getNumCandidates() const
{
return this->params_.num_candidate_nominal_states_;
}
int getNumEvalRollouts() const
{
return getNumCandidates() * getNumEvalSamplesPerCandidate();
}
int getEvalKernelChoiceAsInt() const
{
return static_cast<int>(use_eval_kernel_);
}
kernelType getEvalKernelChoiceAsEnum() const
{
return use_eval_kernel_;
}
// Does nothing. This reason is because the control sliding happens during the importance sampler update.
// The control applied to the real system (during the MPPI rollouts) is the nominal control (which slides
// during the importance sampler update), plus the feedback term. Inside the runControlIteration function
// slideControl sequence is called prior to optimization, after the importance sampler update.
void slideControlSequence(int steps) override{};
// Feedback gain computation is done after the importance sampling update. The nominal trajectory computed
// during the importance sampling update does not change after the optimization, thus the feedback gains will
// not change either. In the current implementation of runControlIteration, the compute feedback gains is called
// after the computation of the optimal control.
void computeFeedback(const Eigen::Ref<const state_array>& state) override{};
Eigen::MatrixXf getCandidateFreeEnergy()
{
return candidate_free_energy_;
};
float computeDF();
void setPercentageSampledControlTrajectories(float new_perc)
{
this->setPercentageSampledControlTrajectoriesHelper(new_perc, 2);
}
void setValueFunctionThreshold(float value_function_threshold)
{
this->params_.value_function_threshold_ = value_function_threshold;
}
void setOptimizationStride(float optimization_stride)
{
this->params_.optimization_stride_ = optimization_stride;
}
void setNumCandidates(int num_candidate_nominal_states)
{
this->params_.num_candidate_nominal_states_ = num_candidate_nominal_states;
}
void setNumSamplesPerCandidate(const int& num_samples)
{
this->params_.eval_dyn_kernel_dim_.x = num_samples;
updateCandidateMemory();
}
void setEvalKernelChoice(const int& kernel_type)
{
use_eval_kernel_ = static_cast<kernelType>(kernel_type);
}
void setEvalKernelChoice(const kernelType& kernel_type)
{
use_eval_kernel_ = kernel_type;
}
void setParams(const PARAMS_T& p);
void calculateSampledStateTrajectories() override;
void chooseAppropriateKernel() override;
void chooseAppropriateEvalKernel();
protected:
bool importance_sampling_cuda_mem_init_ = false;
// int num_candidate_nominal_states_;
int best_index_ = 0; // Selected nominal state candidate
// int optimization_stride_; // Number of timesteps to apply the optimal control (== 1 for true MPC)
int nominal_stride_ = 0; // Stride for the chosen nominal state of the importance sampler
int real_stride_ = 0; // Stride for the optimal controller sliding
bool nominal_state_init_ = false;
// Free energy variables
float nominal_free_energy_mean_ = 0;
float nominal_free_energy_variance_ = 0;
float nominal_free_energy_modified_variance_ = 0;
// Storage classes
control_trajectory nominal_control_trajectory_ = control_trajectory::Zero();
state_trajectory nominal_state_trajectory_ = state_trajectory::Zero();
sampled_cost_traj trajectory_costs_nominal_ = sampled_cost_traj::Zero();
// Make the control history size flexible, related to issue #30
Eigen::Matrix<float, DYN_T::CONTROL_DIM, 2> nominal_control_history_; // History used for nominal_state IS
NominalCandidateVector candidate_nominal_states_ = { state_array::Zero() };
Eigen::MatrixXf line_search_weights_; // At minimum there must be 3 candidates
Eigen::MatrixXi importance_sampler_strides_; // Time index where control trajectory starts for each nominal state
// candidate
Eigen::MatrixXf candidate_trajectory_costs_;
Eigen::MatrixXf candidate_free_energy_;
void allocateCUDAMemory();
void copyNominalControlToDevice(bool synchronize = true);
void deallocateNominalStateCandidateMemory();
void updateCandidateMemory();
void resetCandidateCudaMem();
void getInitNominalStateCandidates(const Eigen::Ref<const state_array>& nominal_x_k,
const Eigen::Ref<const state_array>& nominal_x_kp1,
const Eigen::Ref<const state_array>& real_x_kp1);
// compute the line search weights
void computeLineSearchWeights();
void computeNominalStateAndStride(const Eigen::Ref<const state_array>& state, int stride);
// compute the importance sampler strides
void computeImportanceSamplerStride(int stride);
// Compute the baseline of the candidates
float computeCandidateBaseline();
// Get the best index based on the candidate free energy
void computeBestIndex();
// Computes and saves the feedback gains used in the rollout kernel and tracking.
virtual void computeNominalFeedbackGains(const Eigen::Ref<const state_array>& state);
// CUDA Memory
float* importance_sampling_costs_d_ = nullptr;
float* importance_sampling_outputs_d_ = nullptr;
float* importance_sampling_states_d_ = nullptr;
int* importance_sampling_strides_d_ = nullptr;
float* feedback_gain_array_d_ = nullptr;
kernelType use_eval_kernel_ = kernelType::USE_SPLIT_KERNELS;
};
#if __CUDACC__
#include "robust_mppi_controller.cu"
#endif
#endif /* MPPI_GEN_RMPPI_CONTROLLER_CUH_ */
================================================
FILE: include/mppi/controllers/Tube-MPPI/tube_mppi_controller.cu
================================================
#include "tube_mppi_controller.cuh"
#include <mppi/core/mppi_common.cuh>
#define TUBE_MPPI_TEMPLATE \
template <class DYN_T, class COST_T, class FB_T, int MAX_TIMESTEPS, int NUM_ROLLOUTS, class SAMPLING_T, \
class PARAMS_T>
#define TubeMPPI TubeMPPIController<DYN_T, COST_T, FB_T, MAX_TIMESTEPS, NUM_ROLLOUTS, SAMPLING_T, PARAMS_T>
TUBE_MPPI_TEMPLATE
TubeMPPI::TubeMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, float dt,
int max_iter, float lambda, float alpha, int num_timesteps,
const Eigen::Ref<const control_trajectory>& init_control_traj, cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, dt, max_iter, lambda, alpha, num_timesteps, init_control_traj,
stream)
{
nominal_control_trajectory_ = init_control_traj;
// call rollout kernel with z = 2 since we have a nominal state
this->params_.dynamics_rollout_dim_.z = max(2, this->params_.dynamics_rollout_dim_.z);
this->params_.cost_rollout_dim_.z = max(2, this->params_.cost_rollout_dim_.z);
this->sampler_->setNumDistributions(2);
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// Initialize Feedback
this->fb_controller_->initTrackingController();
this->enable_feedback_ = true;
chooseAppropriateKernel();
}
TUBE_MPPI_TEMPLATE
TubeMPPI::TubeMPPIController(DYN_T* model, COST_T* cost, FB_T* fb_controller, SAMPLING_T* sampler, PARAMS_T& params,
cudaStream_t stream)
: PARENT_CLASS(model, cost, fb_controller, sampler, params, stream)
{
nominal_control_trajectory_ = this->params_.init_control_traj_;
// call rollout kernel with z = 2 since we have a nominal state
this->params_.dynamics_rollout_dim_.z = max(2, this->params_.dynamics_rollout_dim_.z);
this->params_.cost_rollout_dim_.z = max(2, this->params_.cost_rollout_dim_.z);
this->sampler_->setNumDistributions(2);
// Allocate CUDA memory for the controller
allocateCUDAMemory();
// Initialize Feedback
this->fb_controller_->initTrackingController();
this->enable_feedback_ = true;
chooseAppropriateKernel();
}
TUBE_MPPI_TEMPLATE
void TubeMPPI::chooseAppropriateKernel()
{
cudaDeviceProp deviceProp;
HANDLE_ERROR(cudaGetDeviceProperties(&deviceProp, 0));
unsigned single_kernel_byte_size = mppi::kernels::calcRolloutCombinedKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, this->params_.dynamics_rollout_dim_);
unsigned split_dyn_kernel_byte_size = mppi::kernels::calcRolloutDynamicsKernelSharedMemSize(
this->model_, this->sampler_, this->params_.dynamics_rollout_dim_);
unsigned split_cost_kernel_byte_size =
mppi::kernels::calcRolloutCostKernelSharedMemSize(this->cost_, this->sampler_, this->params_.cost_rollout_dim_);
unsigned vis_single_kernel_byte_size = mppi::kernels::calcVisualizeKernelSharedMemSize(
this->model_, this->cost_, this->sampler_, this->getNumTimesteps(), this->params_.visualize_dim_);
bool too_much_mem_single_kernel = single_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool too_much_mem_vis_kernel = vis_single_kernel_byte_size > deviceProp.sharedMemPerBlock;
bool too_much_mem_split_kernel = split_dyn_kernel_byte_size > deviceProp.sharedMemPerBlock;
too_much_mem_split_kernel = too_much_mem_split_kernel || split_cost_kernel_byte_size > deviceProp.sharedMemPerBlock;
too_much_mem_single_kernel = too_much_
gitextract_ky3rcz46/
├── .clang-format
├── .gitattributes
├── .gitignore
├── .gitmodules
├── CMakeLists.txt
├── LICENSE
├── README.md
├── VERSION
├── cmake/
│ ├── Config.cmake.in
│ ├── MPPIGenericToolsConfig.cmake
│ └── Modules/
│ ├── CMakeLists.txt.gtest.in
│ └── cmake_uninstall.cmake.in
├── doc/
│ └── feedback.md
├── examples/
│ ├── CMakeLists.txt
│ ├── cartpole_example.cu
│ ├── double_integrator_CORL2020.cu
│ └── double_integrator_example.cu
├── include/
│ └── mppi/
│ ├── controllers/
│ │ ├── ColoredMPPI/
│ │ │ ├── colored_mppi_controller.cu
│ │ │ └── colored_mppi_controller.cuh
│ │ ├── MPPI/
│ │ │ ├── mppi_controller.cu
│ │ │ └── mppi_controller.cuh
│ │ ├── Primitives/
│ │ │ ├── primitives_controller.cu
│ │ │ └── primitives_controller.cuh
│ │ ├── R-MPPI/
│ │ │ ├── robust_mppi_controller.cu
│ │ │ └── robust_mppi_controller.cuh
│ │ ├── Tube-MPPI/
│ │ │ ├── tube_mppi_controller.cu
│ │ │ └── tube_mppi_controller.cuh
│ │ ├── controller.cu
│ │ └── controller.cuh
│ ├── core/
│ │ ├── base_plant.hpp
│ │ ├── buffer.hpp
│ │ ├── buffered_plant.hpp
│ │ ├── mppi_common.cu
│ │ ├── mppi_common.cuh
│ │ ├── rmppi_kernels.cu
│ │ └── rmppi_kernels.cuh
│ ├── cost_functions/
│ │ ├── autorally/
│ │ │ ├── ar_robust_cost.cu
│ │ │ ├── ar_robust_cost.cuh
│ │ │ ├── ar_standard_cost.cu
│ │ │ └── ar_standard_cost.cuh
│ │ ├── cartpole/
│ │ │ ├── cartpole_quadratic_cost.cu
│ │ │ └── cartpole_quadratic_cost.cuh
│ │ ├── cost.cu
│ │ ├── cost.cuh
│ │ ├── double_integrator/
│ │ │ ├── double_integrator_circle_cost.cu
│ │ │ ├── double_integrator_circle_cost.cuh
│ │ │ ├── double_integrator_robust_cost.cu
│ │ │ └── double_integrator_robust_cost.cuh
│ │ ├── quadratic_cost/
│ │ │ ├── quadratic_cost.cu
│ │ │ └── quadratic_cost.cuh
│ │ └── quadrotor/
│ │ ├── quadrotor_map_cost.cu
│ │ ├── quadrotor_map_cost.cuh
│ │ ├── quadrotor_quadratic_cost.cu
│ │ └── quadrotor_quadratic_cost.cuh
│ ├── ddp/
│ │ ├── boxqp.h
│ │ ├── ddp.h
│ │ ├── ddp_costs.h
│ │ ├── ddp_dynamics.h
│ │ ├── ddp_model_wrapper.h
│ │ ├── ddp_tracking_costs.h
│ │ ├── result.h
│ │ └── util.h
│ ├── dynamics/
│ │ ├── autorally/
│ │ │ ├── ar_nn_model.cu
│ │ │ └── ar_nn_model.cuh
│ │ ├── bicycle_slip/
│ │ │ ├── bicycle_slip_parametric.cu
│ │ │ └── bicycle_slip_parametric.cuh
│ │ ├── cartpole/
│ │ │ ├── cartpole_dynamics.cu
│ │ │ └── cartpole_dynamics.cuh
│ │ ├── double_integrator/
│ │ │ ├── di_dynamics.cu
│ │ │ └── di_dynamics.cuh
│ │ ├── dubins/
│ │ │ ├── dubins.cu
│ │ │ └── dubins.cuh
│ │ ├── dynamics.cu
│ │ ├── dynamics.cuh
│ │ ├── linear/
│ │ │ ├── linear.cu
│ │ │ └── linear.cuh
│ │ ├── quadrotor/
│ │ │ ├── quadrotor_dynamics.cu
│ │ │ └── quadrotor_dynamics.cuh
│ │ ├── racer_dubins/
│ │ │ ├── racer_dubins.cu
│ │ │ ├── racer_dubins.cuh
│ │ │ ├── racer_dubins_elevation.cu
│ │ │ ├── racer_dubins_elevation.cuh
│ │ │ ├── racer_dubins_elevation_lstm_steering.cu
│ │ │ ├── racer_dubins_elevation_lstm_steering.cuh
│ │ │ ├── racer_dubins_elevation_lstm_unc.cu
│ │ │ ├── racer_dubins_elevation_lstm_unc.cuh
│ │ │ ├── racer_dubins_elevation_suspension_lstm.cu
│ │ │ └── racer_dubins_elevation_suspension_lstm.cuh
│ │ └── racer_suspension/
│ │ ├── racer_suspension.cu
│ │ └── racer_suspension.cuh
│ ├── feedback_controllers/
│ │ ├── CCM/
│ │ │ └── ccm.h
│ │ ├── DDP/
│ │ │ ├── ddp.cu
│ │ │ └── ddp.cuh
│ │ ├── feedback.cu
│ │ └── feedback.cuh
│ ├── instantiations/
│ │ ├── autorally_mppi/
│ │ │ └── autorally_mppi.cuh
│ │ ├── cartpole_mppi/
│ │ │ └── cartpole_mppi.cuh
│ │ ├── double_integrator_mppi/
│ │ │ └── double_integrator_mppi.cuh
│ │ └── quadrotor_mppi/
│ │ └── quadrotor_mppi.cuh
│ ├── sampling_distributions/
│ │ ├── colored_noise/
│ │ │ ├── colored_noise.cu
│ │ │ └── colored_noise.cuh
│ │ ├── gaussian/
│ │ │ ├── gaussian.cu
│ │ │ └── gaussian.cuh
│ │ ├── nln/
│ │ │ ├── nln.cu
│ │ │ └── nln.cuh
│ │ ├── piecewise_linear/
│ │ │ └── piecewise_linear_noise.cuh
│ │ ├── sampling_distribution.cu
│ │ ├── sampling_distribution.cuh
│ │ └── smooth-MPPI/
│ │ ├── smooth-MPPI.cu
│ │ └── smooth-MPPI.cuh
│ ├── shaping_functions/
│ │ ├── CEM/
│ │ │ ├── cem_shaping_function.cu
│ │ │ └── cem_shaping_function.cuh
│ │ ├── shaping_function.cu
│ │ └── shaping_function.cuh
│ ├── utils/
│ │ ├── activation_functions.cuh
│ │ ├── angle_utils.cuh
│ │ ├── cuda_math_utils.cuh
│ │ ├── eigen_type_conversions.h
│ │ ├── file_utils.h
│ │ ├── gpu_err_chk.cuh
│ │ ├── logger.hpp
│ │ ├── managed.cuh
│ │ ├── math_utils.h
│ │ ├── matrix_mult_utils.cuh
│ │ ├── nn_helpers/
│ │ │ ├── fnn_helper.cu
│ │ │ ├── fnn_helper.cuh
│ │ │ ├── lstm_helper.cu
│ │ │ ├── lstm_helper.cuh
│ │ │ ├── lstm_lstm_helper.cu
│ │ │ ├── lstm_lstm_helper.cuh
│ │ │ └── meta_math.h
│ │ ├── numerical_integration.h
│ │ ├── parallel_utils.cuh
│ │ ├── risk_utils.cu
│ │ ├── risk_utils.cuh
│ │ ├── test_helper.h
│ │ ├── texture_helpers/
│ │ │ ├── texture_helper.cu
│ │ │ ├── texture_helper.cuh
│ │ │ ├── three_d_texture_helper.cu
│ │ │ ├── three_d_texture_helper.cuh
│ │ │ ├── two_d_texture_helper.cu
│ │ │ └── two_d_texture_helper.cuh
│ │ └── type_printing.h
│ └── version.h.in
├── resources/
│ ├── PF_1000_cell_init.npz
│ ├── PF_1000_hidden_init.npz
│ ├── PF_1000_lstm.npz
│ ├── PF_1000_output.npz
│ ├── ackerman_test.npz
│ ├── autorally_nnet_09_12_2018.npz
│ ├── bicycle_slip_hybrid.npz
│ ├── bicycle_slip_hybrid_test.npz
│ ├── bicycle_slip_kinematic_test.npz
│ ├── bicycle_slip_test.npz
│ ├── body_loss_bicycle_slip_kinematic_2023_03_10-12_54_39.npz
│ ├── ccrf_track.npz
│ ├── cell_init.npz
│ ├── hidden_init.npz
│ ├── lstm.npz
│ ├── lstm_lstm_ackerman.npz
│ ├── lstm_lstm_bicycle_slip_kinematic.npz
│ ├── lstm_lstm_steering.npz
│ ├── lstm_lstm_steering_accel.npz
│ ├── lstm_lstm_test.npz
│ ├── network_rde_2024_03_22-19_00_26.npz
│ ├── network_rde_test.npz
│ ├── output.npz
│ ├── sim_PF_200_cell_init.npz
│ ├── sim_PF_200_hidden_init.npz
│ ├── sim_PF_200_lstm.npz
│ └── sim_PF_200_output.npz
├── scripts/
│ ├── autorally/
│ │ ├── lstm_converter.py
│ │ └── test/
│ │ ├── generateTestMaps.py
│ │ └── generateTestNetwork.py
│ ├── colored_noise.py
│ ├── double_integrator/
│ │ ├── generate_free_energy_video.py
│ │ ├── generate_trajectory_video.py
│ │ ├── plot_DI_test_trajectories.py
│ │ ├── plot_DI_test_trajectories_tube_only.py
│ │ └── plot_stuff.py
│ └── run_autoformatter.sh
├── src/
│ ├── CMakeLists.txt
│ └── controllers/
│ ├── CMakeLists.txt
│ ├── autorally/
│ │ ├── CMakeLists.txt
│ │ └── autorally_mppi.cu
│ ├── cartpole/
│ │ ├── CMakeLists.txt
│ │ └── cartpole_mppi.cu
│ ├── double_integrator/
│ │ ├── CMakeLists.txt
│ │ └── double_integrator_mppi.cu
│ └── quadrotor/
│ ├── CMakeLists.txt
│ └── quadrotor_mppi.cu
└── tests/
├── CMakeLists.txt
├── controllers/
│ ├── CMakeLists.txt
│ ├── controller_generic_tests.cu
│ ├── controller_kernel_testing.cu
│ ├── rmppi_test.cu
│ ├── tube_mppi_test.cu
│ └── vanilla_mppi_test.cu
├── cost_functions/
│ ├── CMakeLists.txt
│ ├── autorally_robust_cost_test.cu
│ ├── autorally_standard_cost_test.cu
│ ├── cartpole_quadratic_cost_test.cu
│ ├── general_cost_test.cu
│ ├── general_quadratic_costs.cu
│ ├── quadrotor_map_cost_test.cu
│ └── quadrotor_quadratic_cost_test.cu
├── dynamics/
│ ├── CMakeLists.txt
│ ├── angle_utils_test.cu
│ ├── ar_dynamics_nn_test.cu
│ ├── bicycle_slip_parametric_model_test.cu
│ ├── cartpole_dynamics_tests.cu
│ ├── dubins_dynamics_tests.cu
│ ├── dynamics_generic_tests.cu
│ ├── linear_dynamics_tests.cu
│ ├── quadrotor_dynamics_tests.cu
│ ├── racer_dubins_elevation_lstm_steering_model_test.cu
│ ├── racer_dubins_elevation_lstm_uncertainty_model_test.cu
│ ├── racer_dubins_elevation_model_test.cu
│ ├── racer_dubins_elevation_suspension_test.cu
│ ├── racer_dubins_model_test.cu
│ └── racer_suspension_model_test.cu
├── feedback_controllers/
│ ├── CMakeLists.txt
│ ├── ddp_test.cu
│ └── generic_feedback_controller_test.cu
├── include/
│ ├── kernel_tests/
│ │ ├── core/
│ │ │ ├── normexp_kernel_test.cu
│ │ │ ├── normexp_kernel_test.cuh
│ │ │ ├── rmppi_kernel_test.cu
│ │ │ ├── rmppi_kernel_test.cuh
│ │ │ ├── rollout_kernel_test.cu
│ │ │ ├── rollout_kernel_test.cuh
│ │ │ ├── weightedreduction_kernel_test.cu
│ │ │ └── weightedreduction_kernel_test.cuh
│ │ ├── cost_functions/
│ │ │ ├── autorally/
│ │ │ │ ├── ar_robust_cost_kernel_test.cu
│ │ │ │ ├── ar_robust_cost_kernel_test.cuh
│ │ │ │ ├── ar_standard_cost_kernel_test.cu
│ │ │ │ └── ar_standard_cost_kernel_test.cuh
│ │ │ ├── cartpole/
│ │ │ │ ├── cartpole_quadratic_cost_kernel_test.cu
│ │ │ │ └── cartpole_quadratic_cost_kernel_test.cuh
│ │ │ ├── cost_generic_kernel_tests.cu
│ │ │ └── cost_generic_kernel_tests.cuh
│ │ ├── dynamics/
│ │ │ ├── autorally/
│ │ │ │ ├── ar_nn_dynamics_kernel_test.cu
│ │ │ │ └── ar_nn_dynamics_kernel_test.cuh
│ │ │ ├── cartpole/
│ │ │ │ ├── cartpole_dynamics_kernel_test.cu
│ │ │ │ └── cartpole_dynamics_kernel_test.cuh
│ │ │ ├── dynamics_generic_kernel_tests.cu
│ │ │ └── dynamics_generic_kernel_tests.cuh
│ │ ├── shaping_functions/
│ │ │ ├── shaping_function_kernels_tests.cu
│ │ │ └── shaping_function_kernels_tests.cuh
│ │ └── utils/
│ │ ├── network_helper_kernel_test.cuh
│ │ └── texture_test_kernels.cuh
│ └── mppi_test/
│ └── mock_classes/
│ ├── mock_classes.h
│ ├── mock_controller.h
│ ├── mock_costs.h
│ ├── mock_dynamics.h
│ ├── mock_feedback.h
│ └── mock_sampler.h
├── integration/
│ ├── CMakeLists.txt
│ └── integrator_tests.cu
├── math_utils/
│ ├── CMakeLists.txt
│ ├── cuda_math_utils_tests.cu
│ └── math_utils_test.cu
├── misc/
│ ├── CMakeLists.txt
│ ├── di_dynamics_kernel_tests.cu
│ ├── di_dynamics_kernel_tests.cuh
│ └── miscellaneous_tests.cu
├── mppi_core/
│ ├── CCM_tests.cu
│ ├── CMakeLists.txt
│ ├── base_plant_tester.cu
│ ├── buffered_plant_tester.cu
│ ├── normexp_kernel_tests.cu
│ ├── rmppi_kernel_tests.cu
│ ├── rollout_kernel_tests.cu
│ ├── viz_kernels_test.cu
│ └── weightedreduction_kernel_tests.cu
├── nn_helpers/
│ ├── CMakeLists.txt
│ ├── activation_functions_tests.cu
│ ├── fnn_helper_test.cu
│ ├── lstm_helper_test.cu
│ └── lstm_lstm_helper_test.cu
├── sampling_distributions/
│ ├── CMakeLists.txt
│ ├── colored_noise_tests.cu
│ ├── gaussian_noise_tests.cu
│ └── generic_sampling_distribution_tests.cu
├── shaping_functions/
│ ├── CMakeLists.txt
│ ├── cem_shaping_function_test.cu
│ └── shaping_function_test.cu
├── templated_headers/
│ ├── autorally_test_map.h.in
│ ├── autorally_test_network.h.in
│ ├── racer_test_networks.h.in
│ └── test_networks.h.in
├── test_main.cpp
└── texture_helpers/
├── CMakeLists.txt
├── texture_helper_test.cu
├── three_d_texture_helper_test.cu
└── two_d_texture_helper_test.cu
SYMBOL INDEX (208 symbols across 29 files)
FILE: include/mppi/core/base_plant.hpp
class BasePlant (line 25) | class BasePlant
method BasePlant (line 129) | BasePlant(std::shared_ptr<CONTROLLER_T> controller, int hz, int optimi...
method getStateTime (line 186) | virtual double getStateTime()
method s_traj (line 193) | s_traj getStateTraj()
method c_traj (line 197) | c_traj getControlTraj()
method FB_STATE_T (line 201) | FB_STATE_T getFeedbackState()
method s_array (line 210) | virtual s_array getState()
method setState (line 216) | virtual void setState(const Eigen::Ref<const s_array>& state)
method setControl (line 220) | virtual void setControl(const Eigen::Ref<const c_array>& u)
method setDebugMode (line 224) | virtual void setDebugMode(bool mode)
method resetStateTime (line 229) | void resetStateTime()
method getAvgOptimizationTime (line 234) | double getAvgOptimizationTime() const
method getTargetOptimizationStride (line 239) | int getTargetOptimizationStride()
method getLastOptimizationStride (line 243) | int getLastOptimizationStride()
method setTargetOptimizationStride (line 247) | void setTargetOptimizationStride(int new_val)
method getHz (line 252) | int getHz() const
method setHz (line 257) | void setHz(int hz)
method getVisualizationHz (line 262) | int getVisualizationHz() const
method setVisualizationHz (line 267) | void setVisualizationHz(int hz)
method buffer_trajectory (line 272) | virtual buffer_trajectory getSmoothedBuffer(double time)
method setSolution (line 277) | virtual void setSolution(const Eigen::Ref<const s_traj>& state_seq, co...
method updateState (line 294) | virtual void updateState(Eigen::Ref<s_array> state, double time)
method hasNewDynamicsParams (line 329) | virtual bool hasNewDynamicsParams()
method hasNewCostParams (line 333) | virtual bool hasNewCostParams()
method hasNewControllerParams (line 337) | virtual bool hasNewControllerParams()
method hasNewSamplerParams (line 341) | virtual bool hasNewSamplerParams()
method DYN_PARAMS_T (line 346) | virtual DYN_PARAMS_T getNewDynamicsParams(bool set_flag = false)
method COST_PARAMS_T (line 351) | virtual COST_PARAMS_T getNewCostParams(bool set_flag = false)
method CONTROLLER_PARAMS_T (line 356) | virtual CONTROLLER_PARAMS_T getNewControllerParams(bool set_flag = false)
method SAMPLER_PARAMS_T (line 361) | virtual SAMPLER_PARAMS_T getNewSamplerParams(bool set_flag = false)
method setDynamicsParams (line 367) | virtual void setDynamicsParams(const DYN_PARAMS_T& params)
method setCostParams (line 373) | virtual void setCostParams(const COST_PARAMS_T& params)
method setControllerParams (line 379) | virtual void setControllerParams(const CONTROLLER_PARAMS_T& params)
method setSamplerParams (line 385) | virtual void setSamplerParams(const SAMPLER_PARAMS_T& params)
method setLogger (line 392) | virtual void setLogger(const mppi::util::MPPILoggerPtr& logger)
method setLogLevel (line 398) | virtual void setLogLevel(const mppi::util::LOG_LEVEL& level)
method getLogger (line 404) | virtual mppi::util::MPPILoggerPtr getLogger()
method getLogger (line 409) | virtual mppi::util::MPPILoggerPtr getLogger() const
method updateParameters (line 420) | virtual bool updateParameters()
method runControlIteration (line 466) | void runControlIteration(std::atomic<bool>* is_alive)
method runControlLoop (line 599) | void runControlLoop(std::atomic<bool>* is_alive)
FILE: include/mppi/core/buffer.hpp
type BufferMessage (line 18) | struct BufferMessage
method BufferMessage (line 24) | BufferMessage(double time, T data)
class Buffer (line 32) | class Buffer
method updateExtraValue (line 38) | void updateExtraValue(const std::string& name, float value, double time)
method updateControls (line 48) | void updateControls(c_array& control, double time)
method insertionSort (line 55) | static void insertionSort(std::list<BufferMessage<T>>& list, double ti...
method cleanList (line 74) | static void cleanList(std::list<BufferMessage<T>>& list, double time, ...
method interp (line 89) | static Eigen::Quaternionf interp(std::list<BufferMessage<Eigen::Quater...
method T (line 121) | static T interp(std::list<BufferMessage<T>>& list, double time)
method updateOdometry (line 152) | void updateOdometry(Eigen::Vector3f& pos, Eigen::Quaternionf& quat, Ei...
method getInterpState (line 164) | std::map<std::string, float> getInterpState(double time)
method buffer_trajectory (line 209) | buffer_trajectory getSmoothedBuffer(double latest_time, double buffer_...
method cleanBuffers (line 252) | void cleanBuffers(double time, double horizon)
method clearBuffers (line 266) | void clearBuffers()
method getPrevPositionList (line 277) | std::list<BufferMessage<Eigen::Vector3f>> getPrevPositionList()
method getPrevQuaternionList (line 282) | std::list<BufferMessage<Eigen::Quaternionf>> getPrevQuaternionList()
method getPrevVelocityList (line 287) | std::list<BufferMessage<Eigen::Vector3f>> getPrevVelocityList()
method getPrevOmegaList (line 292) | std::list<BufferMessage<Eigen::Vector3f>> getPrevOmegaList()
method getPrevControlList (line 297) | std::list<BufferMessage<c_array>> getPrevControlList()
method getPrevExtraList (line 302) | std::map<std::string, std::list<BufferMessage<float>>> getPrevExtraList()
method getLatestOdomTime (line 307) | double getLatestOdomTime()
method getOldestOdomTime (line 316) | double getOldestOdomTime()
FILE: include/mppi/core/buffered_plant.hpp
class BufferedPlant (line 12) | class BufferedPlant : public BasePlant<CONTROLLER_T>
method BufferedPlant (line 20) | BufferedPlant(std::shared_ptr<CONTROLLER_T> controller, int hz, int op...
method updateExtraValue (line 25) | void updateExtraValue(const std::string& name, float value, double time)
method updateControls (line 30) | void updateControls(c_array& control, double time)
method updateOdometry (line 35) | void updateOdometry(Eigen::Vector3f& pos, Eigen::Quaternionf& quat, Ei...
method getInterpState (line 50) | std::map<std::string, float> getInterpState(double time)
method updateParameters (line 55) | bool updateParameters()
method buffer_trajectory (line 63) | buffer_trajectory getSmoothedBuffer(double latest_time)
method cleanBuffers (line 68) | void cleanBuffers(double time)
method clearBuffers (line 73) | void clearBuffers()
method getBufferDt (line 78) | double getBufferDt() const
method setBufferDt (line 83) | void setBufferDt(const double buff_dt)
FILE: include/mppi/ddp/boxqp.h
function namespace (line 10) | namespace util
FILE: include/mppi/ddp/ddp_costs.h
function Scalar (line 37) | const Scalar& target(int idx) const
function Scalar (line 84) | const Scalar& target(int idx) const
FILE: include/mppi/ddp/ddp_dynamics.h
function namespace (line 9) | namespace internal
function EIGEN_MAKE_ALIGNED_OPERATOR_NEW (line 59) | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FILE: include/mppi/ddp/ddp_model_wrapper.h
function State (line 68) | State f(const Eigen::Ref<const State>& x, const Eigen::Ref<const Control...
function Jacobian (line 83) | Jacobian df(const Eigen::Ref<const State>& x, const Eigen::Ref<const Con...
FILE: include/mppi/ddp/ddp_tracking_costs.h
function Scalar (line 37) | Scalar c(const Eigen::Ref<const State>& x, const Eigen::Ref<const Contro...
function Gradient (line 45) | Gradient dc(const Eigen::Ref<const State>& x, const Eigen::Ref<const Con...
function Hessian (line 50) | Hessian d2c(const Eigen::Ref<const State>& x, const Eigen::Ref<const Con...
function setTargets (line 55) | void setTargets(const float* traj_target, const float* control_target, i...
function Scalar (line 109) | Scalar c(const Eigen::Ref<const State>& x)
function Gradient (line 114) | Gradient dc(const Eigen::Ref<const State>& x)
function Hessian (line 119) | Hessian d2c(const Eigen::Ref<const State>& x)
FILE: include/mppi/ddp/util.h
function namespace (line 13) | namespace util
FILE: include/mppi/feedback_controllers/CCM/ccm.h
function namespace (line 13) | namespace ccm
FILE: include/mppi/utils/eigen_type_conversions.h
function float3 (line 10) | float3 EigenToCuda(const Eigen::Vector3f& v)
FILE: include/mppi/utils/file_utils.h
function fileExists (line 11) | inline bool fileExists(const std::string& name)
FILE: include/mppi/utils/logger.hpp
type mppi (line 11) | namespace mppi
type util (line 13) | namespace util
type LOG_LEVEL (line 15) | enum class LOG_LEVEL : int
class MPPILogger (line 36) | class MPPILogger
method MPPILogger (line 39) | MPPILogger() = default;
method MPPILogger (line 40) | MPPILogger(const MPPILogger& other) = default;
method MPPILogger (line 41) | MPPILogger(MPPILogger&& other) = default;
method MPPILogger (line 43) | MPPILogger& operator=(const MPPILogger& other) = default;
method MPPILogger (line 44) | MPPILogger& operator=(MPPILogger&& other) = default;
method MPPILogger (line 46) | explicit MPPILogger(LOG_LEVEL level)
method setLogLevel (line 56) | void setLogLevel(const LOG_LEVEL& level)
method setOutputStream (line 66) | void setOutputStream(std::FILE* const output)
method LOG_LEVEL (line 76) | LOG_LEVEL getLogLevel() const
method debug (line 99) | void debug(const char* fmt, Args const&... args)
method info (line 113) | void info(const char* fmt, Args const&... args)
method warning (line 127) | void warning(const char* fmt, Args const&... args)
method error (line 141) | void error(const char* fmt, Args const&... args)
method debug_impl (line 151) | virtual void debug_impl(const std::string& message)
method info_impl (line 159) | virtual void info_impl(const std::string& message)
method warning_impl (line 167) | virtual void warning_impl(const std::string& message)
method error_impl (line 175) | virtual void error_impl(const std::string& message)
method surround_fprintf (line 191) | virtual void surround_fprintf(std::FILE* fstream, const char* pref...
method format_string (line 206) | std::string format_string(const char* fmt, Args const&... args)
FILE: include/mppi/utils/math_utils.h
function namespace (line 39) | namespace mppi
function QuatInv (line 188) | void QuatInv(const float q[4], float q_inv[4])
function __device__ (line 206) | inline __device__ void QuatSubtract(float q_1[4], float q_2[4], float q_...
function __device__ (line 216) | inline __device__ void Euler2QuatNWU(const double& r, const double& p, c...
function Euler2QuatNWU (line 237) | void Euler2QuatNWU(const float& r, const float& p, const float& y, float...
function Quat2EulerNWU (line 263) | void Quat2EulerNWU(const float q[4], float& r, float& p, float& y)
function Quat2DCM (line 272) | void Quat2DCM(const float q[4], float M[3][3])
function QuatSubtract (line 285) | void QuatSubtract(const Eigen::Quaternionf& q_1, const Eigen::Quaternion...
function QuatInv (line 301) | void QuatInv(const Eigen::Quaternionf& q, Eigen::Quaternionf& q_f)
function RotatePointByQuat (line 309) | void RotatePointByQuat(const float q[4], const float3& point, float3& ou...
function RotatePointByQuat (line 325) | void RotatePointByQuat(const Eigen::Quaternionf& q, const float3& point,...
function RotatePointByQuat (line 342) | void RotatePointByQuat(const Eigen::Quaternionf& q, const Eigen::Ref<Eig...
function RotatePointByQuat (line 351) | void RotatePointByQuat(const float q[4], float3& point)
function RotatePointByQuat (line 359) | void RotatePointByQuat(const Eigen::Quaternionf& q, float3& point)
function RotatePointByQuat (line 367) | void RotatePointByQuat(const Eigen::Quaternionf& q, Eigen::Ref<Eigen::Ve...
function Euler2QuatNWU (line 429) | void Euler2QuatNWU(const float& r, const float& p, const float& y, Eigen...
function Euler2DCM_NWU (line 457) | void Euler2DCM_NWU(const float& r, const float& p, const float& y, float...
function Euler2DCM_NWU (line 487) | void Euler2DCM_NWU(const float& r, const float& p, const float& y,
function __device__ (line 519) | __device__ Quat2EulerNWU(const Eigen::Quaternionf& q, float& r, float& p...
function Quat2DCM (line 529) | inline void Quat2DCM(const Eigen::Quaternionf& q, Eigen::Ref<Eigen::Matr...
function __device__ (line 534) | inline __device__ void omega2edot(const float p, const float q, const fl...
function omega2edot (line 543) | inline void omega2edot(const float p, const float q, const float r, cons...
function bodyOffsetToWorldPoseQuat (line 551) | void bodyOffsetToWorldPoseQuat(const float3& offset, const float3& body_...
function bodyOffsetToWorldPoseQuat (line 563) | void bodyOffsetToWorldPoseQuat(const float3& offset, const float3& body_...
function bodyOffsetToWorldPoseQuat (line 575) | void bodyOffsetToWorldPoseQuat(const Eigen::Ref<Eigen::Vector3f>& offset,
function bodyOffsetToWorldPoseEuler (line 585) | void bodyOffsetToWorldPoseEuler(const float3& offset, const float3& body...
function bodyOffsetToWorldPoseEuler (line 599) | void bodyOffsetToWorldPoseEuler(const float3& offset, const float3& body...
function bodyOffsetToWorldPoseEuler (line 613) | void bodyOffsetToWorldPoseEuler(const Eigen::Ref<Eigen::Vector3f>& offset,
function bodyOffsetToWorldPoseDCM (line 626) | void bodyOffsetToWorldPoseDCM(const float3& offset, const float3& body_p...
function bodyOffsetToWorldPoseDCM (line 637) | void bodyOffsetToWorldPoseDCM(const float3& offset, const float3& body_p...
function bodyOffsetToWorldPoseDCM (line 648) | void bodyOffsetToWorldPoseDCM(const Eigen::Ref<Eigen::Vector3f>& offset,
function __host__ (line 666) | inline __host__ double timeDiffms(const std::chrono::steady_clock::time_...
function normalCDF (line 672) | double normalCDF(double x)
function __host__ (line 677) | inline __host__ std::vector<double> calculateCk(size_t steps)
function clamp (line 739) | float clamp(float value, float min, float max)
function sign (line 744) | float sign(float value)
FILE: include/mppi/utils/nn_helpers/meta_math.h
function input_dim (line 14) | int input_dim(int first, Args... args)
function output_dim (line 20) | int output_dim(int last)
function output_dim (line 26) | int output_dim(int first, Args... args)
function param_counter (line 32) | int param_counter(int first)
function param_counter (line 38) | int param_counter(int first, int next)
function param_counter (line 44) | int param_counter(int first, int next, Args... args)
function layer_counter (line 50) | int layer_counter(int first)
function layer_counter (line 56) | int layer_counter(int first, Args... args)
function neuron_counter (line 62) | int neuron_counter(int first)
function neuron_counter (line 68) | int neuron_counter(int first, Args... args)
FILE: include/mppi/utils/test_helper.h
function array_assert_float_eq (line 13) | inline void array_assert_float_eq(const std::vector<float>& known, const...
function array_assert_float_eq (line 23) | inline void array_assert_float_eq(const float known, const std::vector<f...
function array_assert_float_eq (line 33) | void array_assert_float_eq(const std::array<float, size>& known, const s...
function array_expect_float_eq (line 54) | void array_expect_float_eq(const std::array<float, size>& known, const s...
function array_expect_near (line 64) | void array_expect_near(const std::array<float, size>& known, const std::...
function array_assert_float_eq (line 74) | void array_assert_float_eq(const float known, const std::array<float, si...
function eigen_assert_float_eq (line 84) | void eigen_assert_float_eq(const Eigen::Ref<const EIGEN_MAT>& known, con...
function eigen_assert_float_near (line 93) | void eigen_assert_float_near(const Eigen::Ref<const EIGEN_MAT>& known,
function array_assert_float_near (line 103) | void array_assert_float_near(const std::array<float, size>& known, const...
FILE: include/mppi/utils/type_printing.h
function string (line 25) | string type_name()
FILE: scripts/autorally/test/generateTestMaps.py
function genLoadTrackDataTestMap (line 8) | def genLoadTrackDataTestMap(args):
FILE: scripts/autorally/test/generateTestNetwork.py
function genLoadNetworkDataTest (line 8) | def genLoadNetworkDataTest(args):
function genComputationNetworkTest (line 27) | def genComputationNetworkTest(args):
FILE: scripts/colored_noise.py
function powerlaw_psd_gaussian (line 9) | def powerlaw_psd_gaussian(exponent, size, fmin=0):
FILE: scripts/double_integrator/generate_free_energy_video.py
function init (line 49) | def init():
function update (line 55) | def update(frame):
function index_data (line 74) | def index_data(time, nominal_fe, real_fe, begin_time, end_time):
function main (line 86) | def main(args):
FILE: scripts/double_integrator/generate_trajectory_video.py
function init (line 57) | def init():
function update (line 63) | def update(frame):
function main (line 83) | def main(args):
FILE: scripts/double_integrator/plot_DI_test_trajectories.py
function plot_trajectories (line 18) | def plot_trajectories(axis, axis_name, trajectory_array):
function plot_trajectories2 (line 40) | def plot_trajectories2(axis, axis_name, trajectory_array):
function main (line 61) | def main(args):
FILE: scripts/double_integrator/plot_DI_test_trajectories_tube_only.py
function callback (line 25) | def callback(event):
function plot_trajectories (line 63) | def plot_trajectories(axis, axis_name, trajectory_array):
function plot_boundaries (line 73) | def plot_boundaries(axis):
function main (line 78) | def main(args):
FILE: scripts/double_integrator/plot_stuff.py
function plot_fe_vs_time (line 18) | def plot_fe_vs_time(fe_array, crash_cost=1.0, title="", savefig=0):
function plot_fe_bounded (line 29) | def plot_fe_bounded(fe_array, fe_bound, title="", savefig=0):
function plot_fe_vs_space (line 41) | def plot_fe_vs_space(fe_array, state_array):
function plot_trajectory (line 44) | def plot_trajectory(trajectory, title="", savefig=0):
function plot_nominal_trajectory (line 57) | def plot_nominal_trajectory(trajectory, nominal_trajectory, title="", sa...
function plot_nominal_state_used_vs_time (line 80) | def plot_nominal_state_used_vs_time(ns_array, title=""):
function main (line 88) | def main(args):
FILE: tests/include/mppi_test/mock_classes/mock_costs.h
type mockCostParams (line 13) | typedef struct
FILE: tests/include/mppi_test/mock_classes/mock_dynamics.h
function DynamicsParams (line 13) | struct mockDynamicsParams : public DynamicsParams
type Eigen (line 28) | typedef Eigen::Matrix<float, STATE_DIM, 1> state_array;
type Eigen (line 29) | typedef Eigen::Matrix<float, OUTPUT_DIM, 1> output_array;
type Eigen (line 30) | typedef Eigen::Matrix<float, STATE_DIM, STATE_DIM> dfdx;
type Eigen (line 31) | typedef Eigen::Matrix<float, STATE_DIM, CONTROL_DIM> dfdu;
type Eigen (line 32) | typedef Eigen::Matrix<float, CONTROL_DIM, STATE_DIM> feedback_matrix;
type Eigen (line 33) | typedef Eigen::Matrix<float, STATE_DIM, STATE_DIM
type std (line 35) | typedef std::map<std::string, Eigen::VectorXf> buffer_trajectory;
FILE: tests/include/mppi_test/mock_classes/mock_feedback.h
type mockGPUFeedbackParams (line 11) | struct mockGPUFeedbackParams
FILE: tests/test_main.cpp
function main (line 7) | int main(int argc, char** argv)
Condensed preview — 295 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (2,459K chars).
[
{
"path": ".clang-format",
"chars": 1907,
"preview": "---\n# roscpp style file taken from https://github.com/PickNikRobotics/roscpp_code_format.git\n\nBasedOnStyle: Google\nAcces"
},
{
"path": ".gitattributes",
"chars": 42,
"preview": "*.npz filter=lfs diff=lfs merge=lfs -text\n"
},
{
"path": ".gitignore",
"chars": 495,
"preview": "# Prerequisites\n*.d\n\n# Compiled Object files\n*.slo\n*.lo\n*.o\n*.obj\n\n# Precompiled Headers\n*.gch\n*.pch\n\n# Compiled Dynamic"
},
{
"path": ".gitmodules",
"chars": 99,
"preview": "[submodule \"submodules/cnpy\"]\n path = submodules/cnpy\n url = https://github.com/ACDSLab/cnpy.git\n"
},
{
"path": "CMakeLists.txt",
"chars": 10394,
"preview": "if (CMAKE_VERSION VERSION_LESS 3.12)\n cmake_minimum_required(VERSION 3.8)\nelse()\n cmake_minimum_required(VERSION 3.8.."
},
{
"path": "LICENSE",
"chars": 1320,
"preview": "BSD 2-Clause License\n\nCopyright (c) 2020, Georgia Institute of Technology\n\nRedistribution and use in source and binary f"
},
{
"path": "README.md",
"chars": 2161,
"preview": "# MPPI-Generic\nMPPI-Generic is a C++/CUDA header-only library implementation of Model Predictive Path Integral Control ("
},
{
"path": "VERSION",
"chars": 6,
"preview": "0.9.0\n"
},
{
"path": "cmake/Config.cmake.in",
"chars": 860,
"preview": "# - Config file for the MPPI-Generic package\n# It defines the following variables\n# MPPI_INCLUDE_DIRS - include directo"
},
{
"path": "cmake/MPPIGenericToolsConfig.cmake",
"chars": 3959,
"preview": "if(NOT CMAKE_BUILD_TYPE)\n set(CMAKE_BUILD_TYPE Release CACHE STRING \"Build type options: Release, RelWithDebInfo, Debug"
},
{
"path": "cmake/Modules/CMakeLists.txt.gtest.in",
"chars": 417,
"preview": "cmake_minimum_required(VERSION 2.8.2)\nproject(googletest-download NONE)\n\ninclude(ExternalProject)\nExternalProject_Add(go"
},
{
"path": "cmake/Modules/cmake_uninstall.cmake.in",
"chars": 8952,
"preview": "# ============================================================================\n# Copyright (c) 2011-2012 University of P"
},
{
"path": "doc/feedback.md",
"chars": 3828,
"preview": "# Overview\r\nThe overall structure for the Feedback Controller is a bit confusing but I will try to lay out the basic ide"
},
{
"path": "examples/CMakeLists.txt",
"chars": 795,
"preview": "# add_executable(cartpole_example cartpole_example.cpp)\n# target_include_directories(cartpole_example PUBLIC ${CMAKE_CUD"
},
{
"path": "examples/cartpole_example.cu",
"chars": 3553,
"preview": "#include <mppi/instantiations/cartpole_mppi/cartpole_mppi.cuh>\n#include <iostream>\n#include <chrono>\n\nusing SAMPLER_T = "
},
{
"path": "examples/double_integrator_CORL2020.cu",
"chars": 31763,
"preview": "#include <mppi/dynamics/double_integrator/di_dynamics.cuh>\n#include <mppi/cost_functions/double_integrator/double_integr"
},
{
"path": "examples/double_integrator_example.cu",
"chars": 3347,
"preview": "#include <mppi/dynamics/double_integrator/di_dynamics.cuh>\n#include <mppi/cost_functions/quadratic_cost/quadratic_cost.c"
},
{
"path": "include/mppi/controllers/ColoredMPPI/colored_mppi_controller.cu",
"chars": 16377,
"preview": "#include <mppi/controllers/ColoredMPPI/colored_mppi_controller.cuh>\n#include <mppi/core/mppi_common.cuh>\n#include <algor"
},
{
"path": "include/mppi/controllers/ColoredMPPI/colored_mppi_controller.cuh",
"chars": 6031,
"preview": "/**\n * Created by jason on 10/30/19.\n * Creates the API for interfacing with an MPPI controller\n * should define a compu"
},
{
"path": "include/mppi/controllers/MPPI/mppi_controller.cu",
"chars": 14285,
"preview": "#include <atomic>\n#include <mppi/controllers/MPPI/mppi_controller.cuh>\n#include <mppi/core/mppi_common.cuh>\n#include <al"
},
{
"path": "include/mppi/controllers/MPPI/mppi_controller.cuh",
"chars": 2959,
"preview": "/**\n * Created by jason on 10/30/19.\n * Creates the API for interfacing with an MPPI controller\n * should define a compu"
},
{
"path": "include/mppi/controllers/Primitives/primitives_controller.cu",
"chars": 20846,
"preview": "#include <mppi/controllers/Primitives/primitives_controller.cuh>\n#include <mppi/core/mppi_common.cuh>\n#include <algorith"
},
{
"path": "include/mppi/controllers/Primitives/primitives_controller.cuh",
"chars": 8587,
"preview": "/**\n * Created by david fan on 04/11/22.\n * Creates the API for interfacing with an MPPI controller\n * should define a c"
},
{
"path": "include/mppi/controllers/R-MPPI/robust_mppi_controller.cu",
"chars": 37593,
"preview": "#include \"robust_mppi_controller.cuh\"\n#include <Eigen/Eigenvalues>\n#include <exception>\n\n#define ROBUST_MPPI_TEMPLATE "
},
{
"path": "include/mppi/controllers/R-MPPI/robust_mppi_controller.cuh",
"chars": 12197,
"preview": "/*\n * Software License Agreement (BSD License)\n * Copyright (c) 2013, Georgia Institute of Technology\n * All rights rese"
},
{
"path": "include/mppi/controllers/Tube-MPPI/tube_mppi_controller.cu",
"chars": 18839,
"preview": "#include \"tube_mppi_controller.cuh\"\n#include <mppi/core/mppi_common.cuh>\n\n#define TUBE_MPPI_TEMPLATE "
},
{
"path": "include/mppi/controllers/Tube-MPPI/tube_mppi_controller.cuh",
"chars": 4671,
"preview": "/**\n * Created by Manan Gandhi on 2/14/2020\n * API for interfacing with the TUBE MPPI controller.\n */\n\n#ifndef MPPIGENER"
},
{
"path": "include/mppi/controllers/controller.cu",
"chars": 11374,
"preview": "#include <mppi/controllers/controller.cuh>\n\n#define CONTROLLER_TEMPLATE "
},
{
"path": "include/mppi/controllers/controller.cuh",
"chars": 31395,
"preview": "//\n// Created by jason on 10/30/19.\n//\n\n#ifndef MPPIGENERIC_CONTROLLER_CUH\n#define MPPIGENERIC_CONTROLLER_CUH\n\n#include "
},
{
"path": "include/mppi/core/base_plant.hpp",
"chars": 20277,
"preview": "/**\n * Created by Bogdan on 2/11/20.\n * Creates the API for interfacing with an MPPI controller\n * should define a compu"
},
{
"path": "include/mppi/core/buffer.hpp",
"chars": 8966,
"preview": "/**\n * @file buffer.hpp\n * @brief Buffer of states and controls\n * @author Bogdan Vlahov\n * @version 0.0.1\n * @date 2024"
},
{
"path": "include/mppi/core/buffered_plant.hpp",
"chars": 2633,
"preview": "//\n// Created by jason on 7/20/21.\n//\n\n#ifndef MPPIGENERIC_BUFFERED_PLANT_H\n#define MPPIGENERIC_BUFFERED_PLANT_H\n\n#inclu"
},
{
"path": "include/mppi/core/mppi_common.cu",
"chars": 66797,
"preview": "#include <mppi/core/mppi_common.cuh>\n#include <curand.h>\n#include <mppi/utils/gpu_err_chk.cuh>\n#include <mppi/utils/math"
},
{
"path": "include/mppi/core/mppi_common.cuh",
"chars": 14836,
"preview": "//\n// Created by Manan Gandhi on 12/2/19.\n//\n\n#ifndef MPPIGENERIC_MPPI_COMMON_CUH\n#define MPPIGENERIC_MPPI_COMMON_CUH\n\n#"
},
{
"path": "include/mppi/core/rmppi_kernels.cu",
"chars": 54539,
"preview": "#include <mppi/core/mppi_common.cuh>\n\nnamespace mp1 = mppi::p1;\n\nnamespace mppi\n{\nnamespace kernels\n{\nnamespace rmppi\n{\n"
},
{
"path": "include/mppi/core/rmppi_kernels.cuh",
"chars": 7961,
"preview": "/**\n * Created by Bogdan Vlahov on 3/25/2023\n **/\n#pragma once\n#include <mppi/utils/math_utils.h>\n#include <mppi/dynamic"
},
{
"path": "include/mppi/cost_functions/autorally/ar_robust_cost.cu",
"chars": 4932,
"preview": "\ntemplate <class CLASS_T, class PARAMS_T>\nARRobustCostImpl<CLASS_T, PARAMS_T>::ARRobustCostImpl(cudaStream_t stream)\n :"
},
{
"path": "include/mppi/cost_functions/autorally/ar_robust_cost.cuh",
"chars": 1779,
"preview": "#ifndef AR_ROBUST_COST_CUH_\n#define AR_ROBUST_COST_CUH_\n\n#include <mppi/cost_functions/autorally/ar_standard_cost.cuh>\n\n"
},
{
"path": "include/mppi/cost_functions/autorally/ar_standard_cost.cu",
"chars": 15293,
"preview": "#include <mppi/cost_functions/autorally/ar_standard_cost.cuh>\n\ntemplate <class CLASS_T, class PARAMS_T, class DYN_PARAMS"
},
{
"path": "include/mppi/cost_functions/autorally/ar_standard_cost.cuh",
"chars": 7439,
"preview": "#pragma once\n\n#ifndef AR_STANDARD_COST_CUH_\n#define AR_STANDARD_COST_CUH_\n\n#include <mppi/cost_functions/cost.cuh>\n#incl"
},
{
"path": "include/mppi/cost_functions/cartpole/cartpole_quadratic_cost.cu",
"chars": 3070,
"preview": "#include <mppi/cost_functions/cartpole/cartpole_quadratic_cost.cuh>\n\nCartpoleQuadraticCost::CartpoleQuadraticCost(cudaSt"
},
{
"path": "include/mppi/cost_functions/cartpole/cartpole_quadratic_cost.cuh",
"chars": 1519,
"preview": "#pragma once\n\n#ifndef CARTPOLE_QUADRATIC_COST_CUH_\n#define CARTPOLE_QUADRATIC_COST_CUH_\n\n#include <mppi/cost_functions/c"
},
{
"path": "include/mppi/cost_functions/cost.cu",
"chars": 1513,
"preview": "#include <mppi/cost_functions/cost.cuh>\n\ntemplate <class CLASS_T, class PARAMS_T, class DYN_PARAMS_T>\nvoid Cost<CLASS_T,"
},
{
"path": "include/mppi/cost_functions/cost.cuh",
"chars": 6933,
"preview": "#pragma once\n/*\nHeader file for costs\n*/\n\n#ifndef COSTS_CUH_\n#define COSTS_CUH_\n\n#include <Eigen/Dense>\n#include <stdio."
},
{
"path": "include/mppi/cost_functions/double_integrator/double_integrator_circle_cost.cu",
"chars": 2609,
"preview": "#include <mppi/cost_functions/double_integrator/double_integrator_circle_cost.cuh>\n\nDoubleIntegratorCircleCost::DoubleIn"
},
{
"path": "include/mppi/cost_functions/double_integrator/double_integrator_circle_cost.cuh",
"chars": 1362,
"preview": "#pragma once\n#ifndef DOUBLE_INTEGRATOR_CIRCLE_COST_CUH_\n#define DOUBLE_INTEGRATOR_CIRCLE_COST_CUH_\n\n#include <mppi/cost_"
},
{
"path": "include/mppi/cost_functions/double_integrator/double_integrator_robust_cost.cu",
"chars": 3102,
"preview": "#include <mppi/cost_functions/double_integrator/double_integrator_robust_cost.cuh>\n#include <mppi/utils/math_utils.h>\n\nD"
},
{
"path": "include/mppi/cost_functions/double_integrator/double_integrator_robust_cost.cuh",
"chars": 960,
"preview": "#ifndef DOUBLE_INTEGRATOR_ROBUST_COST_CUH_\n#define DOUBLE_INTEGRATOR_ROBUST_COST_CUH_\n\n#include <mppi/cost_functions/dou"
},
{
"path": "include/mppi/cost_functions/quadratic_cost/quadratic_cost.cu",
"chars": 1801,
"preview": "#include <mppi/cost_functions/quadratic_cost/quadratic_cost.cuh>\n\ntemplate <class CLASS_T, class DYN_T, class PARAMS_T>\n"
},
{
"path": "include/mppi/cost_functions/quadratic_cost/quadratic_cost.cuh",
"chars": 3761,
"preview": "#pragma once\n/*\n * Created on Wed Dec 16 2020 by Bogdan\n */\n\n#ifndef MPPI_COST_FUNCTIONS_QUADRATIC_COST_CUH_\n#define MPP"
},
{
"path": "include/mppi/cost_functions/quadrotor/quadrotor_map_cost.cu",
"chars": 22834,
"preview": "#include <cnpy.h>\n#include <mppi/utils/file_utils.h>\n#include <mppi/cost_functions/quadrotor/quadrotor_map_cost.cuh>\n#in"
},
{
"path": "include/mppi/cost_functions/quadrotor/quadrotor_map_cost.cuh",
"chars": 6973,
"preview": "/*\n * Created on Wed Jul 22 2020 by Bogdan\n */\n#ifndef MPPI_COST_FUNCTIONS_QUADROTOR_MAP_COST_CUH_\n#define MPPI_COST_FUN"
},
{
"path": "include/mppi/cost_functions/quadrotor/quadrotor_quadratic_cost.cu",
"chars": 3857,
"preview": "#include <mppi/cost_functions/quadrotor/quadrotor_quadratic_cost.cuh>\n\nQuadrotorQuadraticCost::QuadrotorQuadraticCost(cu"
},
{
"path": "include/mppi/cost_functions/quadrotor/quadrotor_quadratic_cost.cuh",
"chars": 3047,
"preview": "/*\n * Created on Thu Jun 11 2020 by Bogdan\n */\n#ifndef MPPI_COST_FUNCTIONS_QUADROTOR_QUADRATIC_COST_CUH_\n#define MPPI_CO"
},
{
"path": "include/mppi/ddp/boxqp.h",
"chars": 8842,
"preview": "#ifndef TRAJOPT_BOXQP_HPP\n#define TRAJOPT_BOXQP_HPP\n\n#include \"util.h\"\n#include <Eigen/Cholesky>\n#include <Eigen/Dense>\n"
},
{
"path": "include/mppi/ddp/ddp.h",
"chars": 8020,
"preview": "#ifndef TRAJOPT_DDP_HPP\n#define TRAJOPT_DDP_HPP\n\n#include \"boxqp.h\"\n#include \"result.h\"\n#include \"util.h\"\n#include <Eige"
},
{
"path": "include/mppi/ddp/ddp_costs.h",
"chars": 2710,
"preview": "#ifndef TRAJOPT_COSTS_HPP\n#define TRAJOPT_COSTS_HPP\n\n#include <Eigen/Dense>\n\ntemplate <class Dynamics>\nstruct CostFuncti"
},
{
"path": "include/mppi/ddp/ddp_dynamics.h",
"chars": 3540,
"preview": "#ifndef TRAJOPT_DYNAMICS_HPP\n#define TRAJOPT_DYNAMICS_HPP\n\n#include \"util.h\"\n#include <Eigen/Dense>\n#include <unsupporte"
},
{
"path": "include/mppi/ddp/ddp_model_wrapper.h",
"chars": 3588,
"preview": "#ifndef DDP_MODEL_WRAPPER_H\n#define DDP_MODEL_WRAPPER_H\n\n#include \"ddp_dynamics.h\"\n#include <type_traits>\n\ntemplate <typ"
},
{
"path": "include/mppi/ddp/ddp_tracking_costs.h",
"chars": 3929,
"preview": "#ifndef DDP_TRACKING_COSTS_\n#define DDP_TRACKING_COSTS_\n\n#include \"ddp_dynamics.h\"\n#include \"ddp_costs.h\"\n\ntemplate <typ"
},
{
"path": "include/mppi/ddp/result.h",
"chars": 3525,
"preview": "#ifndef TRAJOPT_RESULT_HPP\n#define TRAJOPT_RESULT_HPP\n\n#include <Eigen/Dense>\n\n/**\n * @tparam Dynamics\n * @brief Repr"
},
{
"path": "include/mppi/ddp/util.h",
"chars": 3984,
"preview": "#ifndef TRAJOPT_UTIL_HPP\n#define TRAJOPT_UTIL_HPP\n\n#include <Eigen/StdVector>\n#include <cmath>\n#include <cstdarg>\n#inclu"
},
{
"path": "include/mppi/dynamics/autorally/ar_nn_model.cu",
"chars": 7029,
"preview": "\n#include \"ar_nn_model.cuh\"\n\ntemplate <int S_DIM, int C_DIM, int K_DIM>\nNeuralNetModel<S_DIM, C_DIM, K_DIM>::NeuralNetMo"
},
{
"path": "include/mppi/dynamics/autorally/ar_nn_model.cuh",
"chars": 4530,
"preview": "#ifndef AR_NN_DYNAMICS_CUH_\n#define AR_NN_DYNAMICS_CUH_\n\n#include <mppi/dynamics/dynamics.cuh>\n#include <mppi/utils/file"
},
{
"path": "include/mppi/dynamics/bicycle_slip/bicycle_slip_parametric.cu",
"chars": 24786,
"preview": "//\n// Created by jason on 12/12/22.\n//\n\n#include \"bicycle_slip_parametric.cuh\"\n\ntemplate <class CLASS_T, class PARAMS_T>"
},
{
"path": "include/mppi/dynamics/bicycle_slip/bicycle_slip_parametric.cuh",
"chars": 6300,
"preview": "//\n// Created by jason on 12/12/22.\n//\n\n#ifndef MPPIGENERIC_BICYCLE_SLIP_PARAMTERIC_CUH\n#define MPPIGENERIC_BICYCLE_SLIP"
},
{
"path": "include/mppi/dynamics/cartpole/cartpole_dynamics.cu",
"chars": 5608,
"preview": "#include <mppi/dynamics/cartpole/cartpole_dynamics.cuh>\n#include <mppi/utils/math_utils.h>\n\nCartpoleDynamics::CartpoleDy"
},
{
"path": "include/mppi/dynamics/cartpole/cartpole_dynamics.cuh",
"chars": 2965,
"preview": "#ifndef CARTPOLE_CUH_\n#define CARTPOLE_CUH_\n\n#include <mppi/dynamics/dynamics.cuh>\n\nstruct CartpoleDynamicsParams : publ"
},
{
"path": "include/mppi/dynamics/double_integrator/di_dynamics.cu",
"chars": 2841,
"preview": "#include <mppi/dynamics/double_integrator/di_dynamics.cuh>\n\nDoubleIntegratorDynamics::DoubleIntegratorDynamics(float sys"
},
{
"path": "include/mppi/dynamics/double_integrator/di_dynamics.cuh",
"chars": 2077,
"preview": "#pragma once\n\n#ifndef DOUBLE_INTEGRATOR_CUH_\n#define DOUBLE_INTEGRATOR_CUH_\n\n#include <mppi/dynamics/dynamics.cuh>\n#incl"
},
{
"path": "include/mppi/dynamics/dubins/dubins.cu",
"chars": 2913,
"preview": "#include <mppi/dynamics/dubins/dubins.cuh>\n\nDubinsDynamics::DubinsDynamics(cudaStream_t stream) : Dynamics<DubinsDynamic"
},
{
"path": "include/mppi/dynamics/dubins/dubins.cuh",
"chars": 2076,
"preview": "#pragma once\n\n#ifndef DUBINS_CUH_\n#define DUBINS_CUH_\n\n#include <mppi/dynamics/dynamics.cuh>\n#include <mppi/utils/angle_"
},
{
"path": "include/mppi/dynamics/dynamics.cu",
"chars": 6415,
"preview": "#include <mppi/dynamics/dynamics.cuh>\n\ntemplate <class CLASS_T, class PARAMS_T>\nvoid Dynamics<CLASS_T, PARAMS_T>::params"
},
{
"path": "include/mppi/dynamics/dynamics.cuh",
"chars": 15399,
"preview": "#pragma once\n\n#ifndef DYNAMICS_CUH_\n#define DYNAMICS_CUH_\n\n/*\nHeader file for dynamics\n*/\n\n#include <Eigen/Dense>\n#inclu"
},
{
"path": "include/mppi/dynamics/linear/linear.cu",
"chars": 3215,
"preview": "/**\n * @file linear.cu\n * @brief Linear Dynamics\n * @author Bogdan Vlahov\n * @version\n * @date 2024-08-16\n */\n#include <"
},
{
"path": "include/mppi/dynamics/linear/linear.cuh",
"chars": 3884,
"preview": "/**\n * @file linear.cuh\n * @brief Linear Dynamics\n * @author Bogdan Vlahov\n * @version\n * @date 2024-08-16\n */\n#pragma o"
},
{
"path": "include/mppi/dynamics/quadrotor/quadrotor_dynamics.cu",
"chars": 7574,
"preview": "#include <mppi/dynamics/quadrotor/quadrotor_dynamics.cuh>\n#include <mppi/utils/math_utils.h>\n\nQuadrotorDynamics::Quadrot"
},
{
"path": "include/mppi/dynamics/quadrotor/quadrotor_dynamics.cuh",
"chars": 3292,
"preview": "/*\n * Created on Tue Jun 02 2020 by Bogdan Vlahov\n *\n */\n#ifndef QUADROTOR_DYNAMICS_CUH_\n#define QUADROTOR_DYNAMICS_CUH_"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins.cu",
"chars": 19286,
"preview": "#include <mppi/dynamics/racer_dubins/racer_dubins.cuh>\n#include <mppi/utils/math_utils.h>\n\ntemplate <class CLASS_T, clas"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins.cuh",
"chars": 6607,
"preview": "#ifndef MPPIGENERIC_RACER_DUBINS_CUH\n#define MPPIGENERIC_RACER_DUBINS_CUH\n\n#include <mppi/dynamics/dynamics.cuh>\n#includ"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation.cu",
"chars": 41311,
"preview": "#include <mppi/dynamics/racer_dubins/racer_dubins_elevation.cuh>\n#include <mppi/utils/math_utils.h>\n\nnamespace mm = mppi"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation.cuh",
"chars": 8529,
"preview": "#ifndef MPPIGENERIC_RACER_DUBINS_ELEVATION_CUH\n#define MPPIGENERIC_RACER_DUBINS_ELEVATION_CUH\n\n#include <mppi/dynamics/d"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation_lstm_steering.cu",
"chars": 12487,
"preview": "//\n// Created by jason on 8/31/22.\n//\n\n#include \"racer_dubins_elevation_lstm_steering.cuh\"\n\n#define TEMPLATE_TYPE templa"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation_lstm_steering.cuh",
"chars": 5092,
"preview": "//\n// Created by jason on 8/31/22.\n//\n\n#include \"racer_dubins_elevation.cuh\"\n#include <mppi/utils/nn_helpers/lstm_lstm_h"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation_lstm_unc.cu",
"chars": 28792,
"preview": "#include \"racer_dubins_elevation_lstm_unc.cuh\"\n\n// TODO fix the clamping of the brake value\n// TODO look into replacing "
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation_lstm_unc.cuh",
"chars": 4106,
"preview": "#include \"racer_dubins_elevation_suspension_lstm.cuh\"\n\n#ifndef MPPIGENERIC_RACER_DUBINS_ELEVATION_LSTM_UNC_CUH\n#define M"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation_suspension_lstm.cu",
"chars": 25971,
"preview": "//\n// Created by Bogdan on 02/21/2024\n//\n\n#include <eigen3/Eigen/src/Geometry/Quaternion.h>\n#include \"racer_dubins_eleva"
},
{
"path": "include/mppi/dynamics/racer_dubins/racer_dubins_elevation_suspension_lstm.cuh",
"chars": 6379,
"preview": "//\n// Created by Bogdan on 02/21/2024\n//\n#pragma once\n\n#include \"racer_dubins_elevation_lstm_steering.cuh\"\n\n#ifndef W_IN"
},
{
"path": "include/mppi/dynamics/racer_suspension/racer_suspension.cu",
"chars": 18632,
"preview": "#include <mppi/dynamics/racer_suspension/racer_suspension.cuh>\n#include <mppi/utils/eigen_type_conversions.h>\n#include <"
},
{
"path": "include/mppi/dynamics/racer_suspension/racer_suspension.cuh",
"chars": 7683,
"preview": "#ifndef MPPIGENERIC_RACER_SUSPENSION_CUH\n#define MPPIGENERIC_RACER_SUSPENSION_CUH\n\n#include <mppi/dynamics/dynamics.cuh>"
},
{
"path": "include/mppi/feedback_controllers/CCM/ccm.h",
"chars": 8055,
"preview": "/*\n * Created on Tue Jun 30 2020 by Bogdan\n */\n#ifndef FEEDBACK_CONTROLLERS_CCM_CCM_H_\n#define FEEDBACK_CONTROLLERS_CCM_"
},
{
"path": "include/mppi/feedback_controllers/DDP/ddp.cu",
"chars": 4993,
"preview": "#include <mppi/feedback_controllers/DDP/ddp.cuh>\n\ntemplate <class GPU_FB_T, class DYN_T, int NUM_TIMESTEPS>\nDeviceDDPImp"
},
{
"path": "include/mppi/feedback_controllers/DDP/ddp.cuh",
"chars": 5780,
"preview": "/*\n * Created on Sun Sep 28 2020 by Bogdan\n */\n\n#ifndef FEEDBACK_CONTROLLERS_DDP_CUH_\n#define FEEDBACK_CONTROLLERS_DDP_C"
},
{
"path": "include/mppi/feedback_controllers/feedback.cu",
"chars": 1359,
"preview": "#include <mppi/feedback_controllers/feedback.cuh>\n\n// ===================== GPUFeedbackController ======================"
},
{
"path": "include/mppi/feedback_controllers/feedback.cuh",
"chars": 9288,
"preview": "/*\n * Created on Sun Sep 6 2020 by Bogdan\n */\n\n#ifndef FEEDBACK_BASE_CUH_\n#define FEEDBACK_BASE_CUH_\n\n#include <Eigen/Co"
},
{
"path": "include/mppi/instantiations/autorally_mppi/autorally_mppi.cuh",
"chars": 1094,
"preview": "#ifndef MPPIGENERIC_CONTROLLERERS_AUTORALLY_MPPI_CUH\n#define MPPIGENERIC_CONTROLLERERS_AUTORALLY_MPPI_CUH\n\n#include <mpp"
},
{
"path": "include/mppi/instantiations/cartpole_mppi/cartpole_mppi.cuh",
"chars": 453,
"preview": "#ifndef MPPIGENERIC_CONTROLLERERS_CARTPOLE_MPPI_CUH\n#define MPPIGENERIC_CONTROLLERERS_CARTPOLE_MPPI_CUH\n\n#include <mppi/"
},
{
"path": "include/mppi/instantiations/double_integrator_mppi/double_integrator_mppi.cuh",
"chars": 625,
"preview": "#ifndef MPPIGENERIC_CONTROLLERERS_DOUBLE_INTEGRATOR_CUH\n#define MPPIGENERIC_CONTROLLERERS_DOUBLE_INTEGRATOR_CUH\n\n#includ"
},
{
"path": "include/mppi/instantiations/quadrotor_mppi/quadrotor_mppi.cuh",
"chars": 589,
"preview": "#ifndef MPPI_GENERIC_CONTROLLERS_QUADROTOR_MPPI_CUH_\n#define MPPI_GENERIC_CONTROLLERS_QUADROTOR_MPPI_CUH_\n\n#include <mpp"
},
{
"path": "include/mppi/sampling_distributions/colored_noise/colored_noise.cu",
"chars": 18550,
"preview": "/**\n * Created by Bogdan Vlahov on 3/25/2023\n **/\n\n#define COLORED_TEMPLATE template <class CLASS_T, template <int> clas"
},
{
"path": "include/mppi/sampling_distributions/colored_noise/colored_noise.cuh",
"chars": 4912,
"preview": "#pragma once\n/**\n * Created by Bogdan, Dec 16, 2021\n * based off of https://github.com/felixpatzelt/colorednoise/blob/ma"
},
{
"path": "include/mppi/sampling_distributions/gaussian/gaussian.cu",
"chars": 29424,
"preview": "/**\n * Created by Bogdan Vlahov on 3/24/2023\n **/\n\n#include <mppi/sampling_distributions/gaussian/gaussian.cuh>\n#include"
},
{
"path": "include/mppi/sampling_distributions/gaussian/gaussian.cuh",
"chars": 8429,
"preview": "#pragma once\n/**\n * Created by Bogdan Vlahov on 3/24/2023\n **/\n\n#include <mppi/sampling_distributions/sampling_distribut"
},
{
"path": "include/mppi/sampling_distributions/nln/nln.cu",
"chars": 7035,
"preview": "\n/**\n * Created by Bogdan Vlahov on Jan 7, 2024\n **/\n\n#define NLN_TEMPLATE template <class CLASS_T, template <int> class"
},
{
"path": "include/mppi/sampling_distributions/nln/nln.cuh",
"chars": 2373,
"preview": "#pragma once\n/**\n * Created by Bogdan, Jan 7, 2024\n * based off of https://github.com/IhabMohamed/log-MPPI_ros\n */\n\n#inc"
},
{
"path": "include/mppi/sampling_distributions/piecewise_linear/piecewise_linear_noise.cuh",
"chars": 8438,
"preview": "#pragma once\n/**\n * Created by David, Apr 11, 2021\n */\n\n#include <curand.h>\n#include <Eigen/Dense>\n#include <mppi/utils/"
},
{
"path": "include/mppi/sampling_distributions/sampling_distribution.cu",
"chars": 13030,
"preview": "/*\nCreated by Bogdan Vlahov on 3/22/2023\n*/\n#include <mppi/sampling_distributions/sampling_distribution.cuh>\n\ntemplate <"
},
{
"path": "include/mppi/sampling_distributions/sampling_distribution.cuh",
"chars": 19347,
"preview": "#pragma once\n/*\nCreated by Bogdan Vlahov on 3/22/2023\n*/\n\n#include <mppi/dynamics/dynamics.cuh>\n#include <mppi/utils/man"
},
{
"path": "include/mppi/sampling_distributions/smooth-MPPI/smooth-MPPI.cu",
"chars": 12124,
"preview": "/**\n * Created by Bogdan Vlahov on Jan 8, 2024\n **/\n\n#define SMOOTH_MPPI_TEMPLATE template <class CLASS_T, template <int"
},
{
"path": "include/mppi/sampling_distributions/smooth-MPPI/smooth-MPPI.cuh",
"chars": 3610,
"preview": "#pragma once\n/**\n * Created by Bogdan, Jan 8, 2024\n */\n\n#include <mppi/sampling_distributions/gaussian/gaussian.cuh>\n\nna"
},
{
"path": "include/mppi/shaping_functions/CEM/cem_shaping_function.cu",
"chars": 2056,
"preview": "template <class CLASS_T, class PARAMS_T, int NUM_ROLLOUTS, int BDIM_X>\n__host__ __device__ float CEMShapingFunctionImpl<"
},
{
"path": "include/mppi/shaping_functions/CEM/cem_shaping_function.cuh",
"chars": 1456,
"preview": "#ifndef MPPIGENERIC_CEM_SHAPING_FUNCTION_CUH\n#define MPPIGENERIC_CEM_SHAPING_FUNCTION_CUH\n\n#include <mppi/core/mppi_comm"
},
{
"path": "include/mppi/shaping_functions/shaping_function.cu",
"chars": 4825,
"preview": "#include \"shaping_function.cuh\"\n#include <mppi/utils/math_utils.h>\nnamespace mppi_common\n{\ntemplate <class CLASS_T, int "
},
{
"path": "include/mppi/shaping_functions/shaping_function.cuh",
"chars": 1856,
"preview": "#ifndef MPPIGENERIC_SHAPING_FUNCTION_CUH\n#define MPPIGENERIC_SHAPING_FUNCTION_CUH\n\n#include <mppi/core/mppi_common.cuh>\n"
},
{
"path": "include/mppi/utils/activation_functions.cuh",
"chars": 2370,
"preview": "#ifndef ACTIVATION_FUNCTIONS_CUH_\n#define ACTIVATION_FUNCTIONS_CUH_\n\n#include \"math_utils.h\"\nnamespace mppi\n{\nnamespace "
},
{
"path": "include/mppi/utils/angle_utils.cuh",
"chars": 1824,
"preview": "#ifndef ANGLES_CUH_\n#define ANGLES_CUH_\n\n#include <cuda_runtime.h>\n#include <cmath>\n\nnamespace angle_utils\n{\n/**\n *\n * @"
},
{
"path": "include/mppi/utils/cuda_math_utils.cuh",
"chars": 9970,
"preview": "//\n// Created by jason on 4/25/22.\n//\n\n#ifndef MPPIGENERIC_CUDA_MATH_UTILS_CUH\n#define MPPIGENERIC_CUDA_MATH_UTILS_CUH\n\n"
},
{
"path": "include/mppi/utils/eigen_type_conversions.h",
"chars": 283,
"preview": "#pragma once\n#include <cuda_runtime.h>\n#include <Eigen/Dense>\n\n__device__ __host__ Eigen::Vector3f cudaToEigen(const flo"
},
{
"path": "include/mppi/utils/file_utils.h",
"chars": 282,
"preview": "//\n// Created by jgibson37 on 1/9/20.\n//\n\n#ifndef MPPIGENERIC_FILE_UTILS_H\n#define MPPIGENERIC_FILE_UTILS_H\n\n#include <s"
},
{
"path": "include/mppi/utils/gpu_err_chk.cuh",
"chars": 7004,
"preview": "#ifndef CUDA_TESTING_GPU_ERR_CHK_CUH\n#define CUDA_TESTING_GPU_ERR_CHK_CUH\n\n#include <cuda_runtime.h>\n#include <cufft.h>\n"
},
{
"path": "include/mppi/utils/logger.hpp",
"chars": 5672,
"preview": "#pragma once\n/**\n * Created by Bogdan on 11/01/2023\n */\n\n#include <cstdio>\n#include <memory>\n#include <string>\n#include "
},
{
"path": "include/mppi/utils/managed.cuh",
"chars": 4905,
"preview": "/*\n * Software License Agreement (BSD License)\n * Copyright (c) 2013, Georgia Institute of Technology\n * All rights rese"
},
{
"path": "include/mppi/utils/math_utils.h",
"chars": 25452,
"preview": "/*\n * Created on Mon Jun 01 2020 by Bogdan\n *\n */\n#ifndef MATH_UTILS_H_\n#define MATH_UTILS_H_\n\n// Needed for sampling wi"
},
{
"path": "include/mppi/utils/matrix_mult_utils.cuh",
"chars": 10373,
"preview": "//\n// Created by Bogdan on 8/20/23\n//\n#pragma once\n#include <mppi/utils/parallel_utils.cuh>\n\nnamespace mppi\n{\nnamespace "
},
{
"path": "include/mppi/utils/nn_helpers/fnn_helper.cu",
"chars": 14945,
"preview": "//\n// Created by jason on 8/18/22.\n//\n\n#include \"fnn_helper.cuh\"\ntemplate <bool USE_SHARED>\nFNNHelper<USE_SHARED>::FNNHe"
},
{
"path": "include/mppi/utils/nn_helpers/fnn_helper.cuh",
"chars": 4139,
"preview": "#ifndef MPPIGENERIC_FNN_HELPER_CUH\n#define MPPIGENERIC_FNN_HELPER_CUH\n\n#include \"meta_math.h\"\n#include <mppi/utils/math_"
},
{
"path": "include/mppi/utils/nn_helpers/lstm_helper.cu",
"chars": 22520,
"preview": "//\n// Created by jason on 8/20/22.\n//\n\n#include \"lstm_helper.cuh\"\n\ntemplate <bool USE_SHARED>\nLSTMHelper<USE_SHARED>::LS"
},
{
"path": "include/mppi/utils/nn_helpers/lstm_helper.cuh",
"chars": 6062,
"preview": "#ifndef MPPIGENERIC_LSTM_HELPER_CUH\n#define MPPIGENERIC_LSTM_HELPER_CUH\n\n#include \"fnn_helper.cuh\"\n\nstruct LSTMConfig {\n"
},
{
"path": "include/mppi/utils/nn_helpers/lstm_lstm_helper.cu",
"chars": 5337,
"preview": "#include \"lstm_lstm_helper.cuh\"\n\ntemplate <bool USE_SHARED>\nLSTMLSTMHelper<USE_SHARED>::LSTMLSTMHelper(int init_input_di"
},
{
"path": "include/mppi/utils/nn_helpers/lstm_lstm_helper.cuh",
"chars": 3507,
"preview": "//\n// Created by jason on 8/23/22.\n//\n\n#ifndef MPPIGENERIC_LSTM_LSTM_HELPER_CUH\n#define MPPIGENERIC_LSTM_LSTM_HELPER_CUH"
},
{
"path": "include/mppi/utils/nn_helpers/meta_math.h",
"chars": 1553,
"preview": "/**********************************************\n * @file meta_math.h\n * @author Grady Williams <gradyrw@gmail.com>\n * @d"
},
{
"path": "include/mppi/utils/numerical_integration.h",
"chars": 1130,
"preview": "//\n// Created by mgandhi3 on 7/16/21.\n//\n\n#ifndef MPPIGENERIC_NUMERICAL_INTEGRATION_H\n#define MPPIGENERIC_NUMERICAL_INTE"
},
{
"path": "include/mppi/utils/parallel_utils.cuh",
"chars": 8350,
"preview": "//\n// Created by Bogdan on 8/20/23.\n//\n#pragma once\n#include <mppi/utils/cuda_math_utils.cuh>\n\nnamespace mppi\n{\nnamespac"
},
{
"path": "include/mppi/utils/risk_utils.cu",
"chars": 1601,
"preview": "#include <mppi/utils/risk_utils.cuh>\n\n// #include <thrust/device_vector.h>\n// #include <thrust/sort.h>\n\n#include <algori"
},
{
"path": "include/mppi/utils/risk_utils.cuh",
"chars": 2463,
"preview": "#pragma once\n\nnamespace mppi\n{\nclass RiskMeasure\n{\npublic:\n enum FUNC_TYPE : int\n {\n MAX = 0,\n MIN,\n VAR,\n "
},
{
"path": "include/mppi/utils/test_helper.h",
"chars": 3659,
"preview": "//\n// Created by mgandhi3 on 1/13/20.\n//\n\n#ifndef MPPIGENERIC_KERNEL_TESTS_TEST_HELPER_H\n#define MPPIGENERIC_KERNEL_TEST"
},
{
"path": "include/mppi/utils/texture_helpers/texture_helper.cu",
"chars": 13778,
"preview": "#include \"texture_helper.cuh\"\n\ntemplate <class TEX_T, class DATA_T>\nTextureHelper<TEX_T, DATA_T>::TextureHelper(int numb"
},
{
"path": "include/mppi/utils/texture_helpers/texture_helper.cuh",
"chars": 6393,
"preview": "//\n// Created by jason on 1/5/22.\n//\n\n#ifndef MPPIGENERIC_TEXTURE_HELPER_CUH\n#define MPPIGENERIC_TEXTURE_HELPER_CUH\n\n#in"
},
{
"path": "include/mppi/utils/texture_helpers/three_d_texture_helper.cu",
"chars": 12618,
"preview": "#include \"three_d_texture_helper.cuh\"\n\ntemplate <class DATA_T>\nThreeDTextureHelper<DATA_T>::ThreeDTextureHelper(int numb"
},
{
"path": "include/mppi/utils/texture_helpers/three_d_texture_helper.cuh",
"chars": 1631,
"preview": "//\n// Created by jason on 1/10/22.\n//\n\n#ifndef MPPIGENERIC_THREE_D_TEXTURE_HELPER_CUH\n#define MPPIGENERIC_THREE_D_TEXTUR"
},
{
"path": "include/mppi/utils/texture_helpers/two_d_texture_helper.cu",
"chars": 8044,
"preview": "//\n// Created by jason on 1/5/22.\n//\n\n#include \"two_d_texture_helper.cuh\"\n\ntemplate <class DATA_T>\nvoid TwoDTextureHelpe"
},
{
"path": "include/mppi/utils/texture_helpers/two_d_texture_helper.cuh",
"chars": 1643,
"preview": "//\n// Created by jason on 1/5/22.\n//\n\n#ifndef MPPIGENERIC_TWO_D_TEXTURE_HELPER_CUH\n#define MPPIGENERIC_TWO_D_TEXTURE_HEL"
},
{
"path": "include/mppi/utils/type_printing.h",
"chars": 1164,
"preview": "/**\n * C++ 11 solution to print out variable type\n * Copied from:\n * https://stackoverflow.com/questions/81870/is-it-pos"
},
{
"path": "include/mppi/version.h.in",
"chars": 435,
"preview": "/**\n * @file version.h\n * @brief Templated File for MPPI-Generic Version\n * @author Bogdan Vlahov\n * @version 0.0.1\n * @"
},
{
"path": "resources/PF_1000_cell_init.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:ec9c111b7d37c3074b4b4a19adf63585a1d5b333ddda4af216a562693ad492d1\ns"
},
{
"path": "resources/PF_1000_hidden_init.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:5df4063baca04f0079b94e8c997047722e56d8f12a5de8229274acc6a1340b5d\ns"
},
{
"path": "resources/PF_1000_lstm.npz",
"chars": 130,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:375d75e3276e9fa020d0c33c4c7b2a8fc7af5ae93021942488ffc2234262b7f2\ns"
},
{
"path": "resources/PF_1000_output.npz",
"chars": 129,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:6b26b5ec02d2f88b314995819e624d52c0444764a804ac3f334ed74fe4155911\ns"
},
{
"path": "resources/ackerman_test.npz",
"chars": 133,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:6a7a4299fc0f03eff7e7caba21247f70f892f2b4705076ff485a9bea2064442f\ns"
},
{
"path": "resources/autorally_nnet_09_12_2018.npz",
"chars": 130,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:fc67d5a365e513c65ecb5c38c93a2f227dade9ec5c092e3c63878f95e8865694\ns"
},
{
"path": "resources/bicycle_slip_hybrid.npz",
"chars": 132,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:bcd2fbb7262ef3b35637027c15ad7904b02e265ccb9ed8d1b54e73c884f8eb15\ns"
},
{
"path": "resources/bicycle_slip_hybrid_test.npz",
"chars": 132,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:75f53e00e525d0b2faad589c6776e33e09f338ce69fbb06e46b624683580f0e9\ns"
},
{
"path": "resources/bicycle_slip_kinematic_test.npz",
"chars": 133,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:594e9b4fa900438043b7019923b05a730e7c5add894a2e85c3f0a22c0876a3f3\ns"
},
{
"path": "resources/bicycle_slip_test.npz",
"chars": 133,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:c3acbdeff79819386ccb0e02fd84845388a784ad34b8e6023a1dc0d7e1bff2fc\ns"
},
{
"path": "resources/body_loss_bicycle_slip_kinematic_2023_03_10-12_54_39.npz",
"chars": 132,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:74dabe9f4d367a6112132efc36a2dbaf5d004da8fa0ef12a863108cf8ed578e3\ns"
},
{
"path": "resources/ccrf_track.npz",
"chars": 132,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:546f8e88874311082a65fde8d3ebf707bab0f8369746d5ff9b7f725f2d9f1528\ns"
},
{
"path": "resources/cell_init.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:479e450b1dcca2791941b1d50ce285ce7345f5a7ffb5c1133f9bc52887e6ca08\ns"
},
{
"path": "resources/hidden_init.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:93965332fd41d8e37d47dd79bbb4755f4a5a6f719a7a55165407722b230eb2d9\ns"
},
{
"path": "resources/lstm.npz",
"chars": 130,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:3135fec388eab4fefec41948a9f0cbff0d11c14657992f26350ed1e1013691f7\ns"
},
{
"path": "resources/lstm_lstm_ackerman.npz",
"chars": 132,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:3ecf222b541fe8767dd835792dc6f06e7878d58e1f216877c082898969ff5302\ns"
},
{
"path": "resources/lstm_lstm_bicycle_slip_kinematic.npz",
"chars": 132,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:7de7bce16b34fb3f89cc1b53cbd391053eb757b476230b3055438b9510a58714\ns"
},
{
"path": "resources/lstm_lstm_steering.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:83aef869ea23e01cb0a33abcae179ad1baa83d24bc2c60665c59b17950f71e73\ns"
},
{
"path": "resources/lstm_lstm_steering_accel.npz",
"chars": 132,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:c65b093c690ea938e7ae6e49e64dba7f3b042b67027149a46a020d95852dc8a0\ns"
},
{
"path": "resources/lstm_lstm_test.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:3c0a405fc6db14efb3bc736cfc9930dd7bd66faf42f4d7f3941a0ccd185d8388\ns"
},
{
"path": "resources/network_rde_2024_03_22-19_00_26.npz",
"chars": 133,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:77a37ae6d963904c20892c9e4f5f4180f4b5641b0396ad66cc6273d41c474c35\ns"
},
{
"path": "resources/network_rde_test.npz",
"chars": 133,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:e50bf425bcf93de7ba80b0d3cc49eb8ec8ae3bd6255c149efa8e2472614849c0\ns"
},
{
"path": "resources/output.npz",
"chars": 129,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:87285415580e5dc351d3c315270750c88af9b158f93b24f4f1c0407cd77452d5\ns"
},
{
"path": "resources/sim_PF_200_cell_init.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:dcd107ddbf3189ae984f05c0f40f88edfdbc834898840548e782e6698b59825a\ns"
},
{
"path": "resources/sim_PF_200_hidden_init.npz",
"chars": 131,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:fa86275cfccfec3dd7483fed63422b081f28f9e31f8db8acb5d0a978fb306f13\ns"
},
{
"path": "resources/sim_PF_200_lstm.npz",
"chars": 130,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:41952b66ccaf1084665ec69925d7a1ff6f6ddaec8423b556b194e1cde3b4c7d0\ns"
},
{
"path": "resources/sim_PF_200_output.npz",
"chars": 129,
"preview": "version https://git-lfs.github.com/spec/v1\noid sha256:ae29fa9fc96be8ac44efb3451f7be332d52d2dffd3ea5a34fff97cc49e4669d0\ns"
},
{
"path": "scripts/autorally/lstm_converter.py",
"chars": 1310,
"preview": "# reads in an LSTM npz and saves it out so that we can read it with cnpy\nimport torch\nimport numpy as np\nimport pickle\nf"
},
{
"path": "scripts/autorally/test/generateTestMaps.py",
"chars": 5413,
"preview": "#!/usr/bin/env python3\n\nimport numpy as np\nfrom PIL import Image\nimport math\nimport argparse\n\ndef genLoadTrackDataTestMa"
},
{
"path": "scripts/autorally/test/generateTestNetwork.py",
"chars": 1757,
"preview": "#!/usr/bin/env python3\n\nimport numpy as np\nfrom PIL import Image\nimport pickle\nimport argparse\n\ndef genLoadNetworkDataTe"
},
{
"path": "scripts/colored_noise.py",
"chars": 3411,
"preview": "\"\"\"Generate colored noise.\"\"\"\n\"\"\"Source: https://github.com/felixpatzelt/colorednoise/blob/master/colorednoise.py\"\"\"\nfro"
},
{
"path": "scripts/double_integrator/generate_free_energy_video.py",
"chars": 6811,
"preview": "import matplotlib.pyplot as plt\nfrom matplotlib.animation import FuncAnimation\nimport matplotlib.animation as animation\n"
},
{
"path": "scripts/double_integrator/generate_trajectory_video.py",
"chars": 4114,
"preview": "import matplotlib.pyplot as plt\nfrom matplotlib.animation import FuncAnimation\nimport matplotlib.animation as animation\n"
},
{
"path": "scripts/double_integrator/plot_DI_test_trajectories.py",
"chars": 5364,
"preview": "import numpy as np\nimport matplotlib.pyplot as plt\nimport argparse\n\ntrack_radius_outer = 2 + .125\ntrack_radius_inner = 2"
},
{
"path": "scripts/double_integrator/plot_DI_test_trajectories_tube_only.py",
"chars": 4560,
"preview": "import numpy as np\nimport matplotlib.pyplot as plt\nimport argparse\nfrom matplotlib.figure import Figure\nfrom matplotlib."
},
{
"path": "scripts/double_integrator/plot_stuff.py",
"chars": 10311,
"preview": "import numpy as np\nfrom matplotlib import rc\nrc('font', **{'family': 'serif', 'serif': ['Computer Modern'], 'size': 16})"
},
{
"path": "scripts/run_autoformatter.sh",
"chars": 975,
"preview": "#! /usr/bin/env bash\nif [ \"$1\" == \"\" ]; then\n dir=\"./\"\nelse\n dir=\"$1\"\nfi\n\n# Format cuda header files\necho \"Formatting "
},
{
"path": "src/CMakeLists.txt",
"chars": 29,
"preview": "add_subdirectory(controllers)"
},
{
"path": "src/controllers/CMakeLists.txt",
"chars": 268,
"preview": "# Needed for methods to be built into libraries in Release mode\nset(CMAKE_CUDA_FLAGS \"${CMAKE_CUDA_FLAGS} -Xcompiler=-fn"
},
{
"path": "src/controllers/autorally/CMakeLists.txt",
"chars": 561,
"preview": "set(LIBRARY_NAME autorally_mppi)\nSET (LIB_MAJOR 0)\nSET (LIB_MINOR 0)\nSET (LIB_RELEASE 1)\n\nadd_library(${LIBRARY_NAME} SH"
},
{
"path": "src/controllers/autorally/autorally_mppi.cu",
"chars": 579,
"preview": "#include <mppi/instantiations/autorally_mppi/autorally_mppi.cuh>\n\n/*\n * This file contains the instantiations of the con"
},
{
"path": "src/controllers/cartpole/CMakeLists.txt",
"chars": 559,
"preview": "set(LIBRARY_NAME cartpole_mppi)\nSET (LIB_MAJOR 0)\nSET (LIB_MINOR 0)\nSET (LIB_RELEASE 1)\n\nadd_library(${LIBRARY_NAME} SHA"
},
{
"path": "src/controllers/cartpole/cartpole_mppi.cu",
"chars": 2632,
"preview": "#include <mppi/instantiations/cartpole_mppi/cartpole_mppi.cuh>\n\n/*\n * This file contains the instantiations of the contr"
},
{
"path": "src/controllers/double_integrator/CMakeLists.txt",
"chars": 577,
"preview": "set(LIBRARY_NAME double_integrator_mppi)\nSET (LIB_MAJOR 0)\nSET (LIB_MINOR 0)\nSET (LIB_RELEASE 1)\n\nadd_library(${LIBRARY_"
},
{
"path": "src/controllers/double_integrator/double_integrator_mppi.cu",
"chars": 1386,
"preview": "#include <mppi/instantiations/double_integrator_mppi/double_integrator_mppi.cuh>\n\n/*\n * This file contains the instantia"
},
{
"path": "src/controllers/quadrotor/CMakeLists.txt",
"chars": 561,
"preview": "set(LIBRARY_NAME quadrotor_mppi)\nSET (LIB_MAJOR 0)\nSET (LIB_MINOR 0)\nSET (LIB_RELEASE 1)\n\nadd_library(${LIBRARY_NAME} SH"
},
{
"path": "src/controllers/quadrotor/quadrotor_mppi.cu",
"chars": 510,
"preview": "#include <mppi/instantiations/quadrotor_mppi/quadrotor_mppi.cuh>\n\ntypedef mppi::sampling_distributions::GaussianDistribu"
},
{
"path": "tests/CMakeLists.txt",
"chars": 2075,
"preview": "# Add project includes and cuda includes for g++\n\n#include test utils\ninclude_directories(include)\n\n# Create header file"
},
{
"path": "tests/controllers/CMakeLists.txt",
"chars": 551,
"preview": "set(GTEST_LIBRARIES gtest gmock gtest_main)\nfile(GLOB TARGET_SRCS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cu)\n\nforeach(T_"
},
{
"path": "tests/controllers/controller_generic_tests.cu",
"chars": 14488,
"preview": "#include <gmock/gmock.h>\n#include <gtest/gtest.h>\n#include <mppi/controllers/controller.cuh>\n#include <mppi/feedback_con"
},
{
"path": "tests/controllers/controller_kernel_testing.cu",
"chars": 8866,
"preview": "#include <mppi/controllers/MPPI/mppi_controller.cuh>\n#include <mppi/controllers/ColoredMPPI/colored_mppi_controller.cuh>"
},
{
"path": "tests/controllers/rmppi_test.cu",
"chars": 35000,
"preview": "#include <gtest/gtest.h>\n#include <mppi/controllers/R-MPPI/robust_mppi_controller.cuh>\n#include <mppi/cost_functions/dou"
},
{
"path": "tests/controllers/tube_mppi_test.cu",
"chars": 18567,
"preview": "#include <gtest/gtest.h>\n#include <mppi/dynamics/double_integrator/di_dynamics.cuh>\n#include <mppi/cost_functions/double"
},
{
"path": "tests/controllers/vanilla_mppi_test.cu",
"chars": 11329,
"preview": "#include <gtest/gtest.h>\n#include <mppi/instantiations/cartpole_mppi/cartpole_mppi.cuh>\n#include <mppi/instantiations/qu"
},
{
"path": "tests/cost_functions/CMakeLists.txt",
"chars": 561,
"preview": "set(GTEST_LIBRARIES gtest gmock gtest_main)\nfile(GLOB TARGET_SRCS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.cu)\n\nforeach(T_"
},
{
"path": "tests/cost_functions/autorally_robust_cost_test.cu",
"chars": 7443,
"preview": "//\n// Created by jgibson37 on 2/7/20.\n//\n\n#include <gtest/gtest.h>\n#include <mppi/cost_functions/autorally/ar_robust_cos"
}
]
// ... and 95 more files (download for full content)
About this extraction
This page contains the full source code of the ACDSLab/MPPI-Generic GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 295 files (2.3 MB), approximately 608.2k tokens, and a symbol index with 208 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.