diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 4a70e721..ed31bb47 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -28,17 +28,16 @@ jobs: - name: Configure shell: pwsh run: | - cmake -S . -B build ` - -A x64 ` + cmake --preset windows-x64 ` -DVCPKG_TARGET_TRIPLET=x64-windows ` -DCMAKE_TOOLCHAIN_FILE="${{ github.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake" - name: Build shell: pwsh run: | - cmake --build build --config Release --verbose + cmake --build --preset windows-x64-release --verbose - name: Run tests shell: pwsh run: | - ctest --test-dir build --output-on-failure + ctest --preset windows-x64-release --output-on-failure diff --git a/.gitignore b/.gitignore index a58b3aa9..6cdcb229 100644 --- a/.gitignore +++ b/.gitignore @@ -476,3 +476,6 @@ vcpkg_downloads/ !CMakeLists.txt !**/imgui.ini +/build-ninja +/CMakeFiles +/_deps diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f8a85cf..f3be3664 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,20 +1,15 @@ # CMakeLists.txt for DSFE project (root CMakeLists.txt) -cmake_minimum_required(VERSION 3.10) - -if (POLICY CMP0141) - cmake_policy(SET CMP0141 NEW) - set(CMAKE_MSVC_DEBUG_INFORMATION_FORMAT "$,$>,$<$:EditAndContinue>,$<$:ProgramDatabase>>") -endif() - +cmake_minimum_required(VERSION 3.17) project(DSFE LANGUAGES C CXX) set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_MSVC_RUNTIME_LIBRARY "MultiThreaded$<$:Debug>DLL") -# Set common output directories for all targets -set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) +set(OUTPUT_BASE ${CMAKE_BINARY_DIR}) +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${OUTPUT_BASE}/bin) +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${OUTPUT_BASE}/lib) +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${OUTPUT_BASE}/lib) include(FetchContent) @@ -26,56 +21,15 @@ FetchContent_Declare( ) FetchContent_MakeAvailable(eigen) -FetchContent_Declare( - glfw - GIT_REPOSITORY https://github.com/glfw/glfw.git - GIT_TAG 3.4 -) -FetchContent_MakeAvailable(glfw) - -# Fetch GLM from GitHub -FetchContent_Declare( - glm - GIT_REPOSITORY https://github.com/g-truc/glm.git - GIT_TAG 1.0.3 -) -FetchContent_MakeAvailable(glm) - -FetchContent_Declare( - assimp - GIT_REPOSITORY https://github.com/assimp/assimp.git - GIT_TAG v6.0.4 -) - -set(ASSIMP_BUILD_TESTS OFF CACHE BOOL "" FORCE) -set(ASSIMP_BUILD_ASSIMP_TOOLS OFF CACHE BOOL "" FORCE) - -FetchContent_MakeAvailable(assimp) - FetchContent_Declare( googletest GIT_REPOSITORY https://github.com/google/googletest.git GIT_TAG v1.14.0 GIT_SHALLOW TRUE ) +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) -FetchContent_Declare( - imgui - GIT_REPOSITORY https://github.com/ocornut/imgui.git - GIT_TAG docking - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(imgui) - -FetchContent_Declare( - implot - GIT_REPOSITORY https://github.com/epezent/implot.git - GIT_TAG v0.17 - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(implot) - FetchContent_Declare( nlohmann_json GIT_REPOSITORY https://github.com/nlohmann/json.git diff --git a/CMakePresets.json b/CMakePresets.json index 6c6c0187..e97807c3 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -1,81 +1,145 @@ { - "version": 3, + "version": 8, "configurePresets": [ { - "name": "windows-base", + "name": "base", "hidden": true, - "generator": "Ninja", "binaryDir": "${sourceDir}/out/build/${presetName}", - "installDir": "${sourceDir}/out/install/${presetName}", "cacheVariables": { - "CMAKE_C_COMPILER": "cl.exe", - "CMAKE_CXX_COMPILER": "cl.exe" - }, + "CMAKE_EXPORT_COMPILE_COMMANDS": "ON", + "DSFE_ENABLE_GUI": "OFF" + } + }, + { + "name": "base-gui", + "hidden": true, + "binaryDir": "${sourceDir}/out/build/${presetName}", + "cacheVariables": { + "CMAKE_EXPORT_COMPILE_COMMANDS": "ON", + "DSFE_ENABLE_GUI": "ON" + } + }, + { + "name": "windows-x64", + "displayName": "Windows x64", + "inherits": "base", "condition": { "type": "equals", "lhs": "${hostSystemName}", "rhs": "Windows" - } - }, - { - "name": "x64-debug", - "displayName": "x64 Debug", - "inherits": "windows-base", + }, + "generator": "Visual Studio 18 2026", "architecture": { "value": "x64", "strategy": "external" - }, - "cacheVariables": { - "CMAKE_BUILD_TYPE": "Debug" } }, { - "name": "x64-release", - "displayName": "x64 Release", - "inherits": "x64-debug", + "name": "windows-x64-gui", + "displayName": "Windows x64 (GUI)", + "inherits": "base-gui", + "condition": { + "type": "equals", + "lhs": "${hostSystemName}", + "rhs": "Windows" + }, + "generator": "Visual Studio 18 2026", "cacheVariables": { - "CMAKE_BUILD_TYPE": "Release" - } - }, - { - "name": "x86-debug", - "displayName": "x86 Debug", - "inherits": "windows-base", + "CMAKE_PREFIX_PATH": "C:/Qt/6.11.1/msvc2022_64" + }, "architecture": { - "value": "x86", + "value": "x64", "strategy": "external" - }, - "cacheVariables": { - "CMAKE_BUILD_TYPE": "Debug" } }, { - "name": "x86-release", - "displayName": "x86 Release", - "inherits": "x86-debug", + "name": "linux-x64-debug", + "displayName": "Linux x64 Debug", + "inherits": "base", + "condition": { + "type": "equals", + "lhs": "${hostSystemName}", + "rhs": "Linux" + }, + "generator": "Ninja", "cacheVariables": { - "CMAKE_BUILD_TYPE": "Release" + "CMAKE_BUILD_TYPE": "Debug", + "CMAKE_PREFIX_PATH": "/usr/lib/x86_64-linux-gnu/cmake", + "HDF5_DIR": "", + "CMAKE_IGNORE_PATH": "/mnt/c/Program Files/HDF_Group/HDF5/2.0.0" + }, + "architecture": { + "value": "x64", + "strategy": "external" } }, { - "name": "linux-debug", - "displayName": "Linux Debug", - "generator": "Ninja", - "binaryDir": "${sourceDir}/out/build/${presetName}", - "installDir": "${sourceDir}/out/install/${presetName}", - "cacheVariables": { - "CMAKE_BUILD_TYPE": "Debug" - }, + "name": "linux-x64-release", + "displayName": "Linux x64 Release", + "inherits": "base", "condition": { "type": "equals", "lhs": "${hostSystemName}", "rhs": "Linux" }, - "vendor": { - "microsoft.com/VisualStudioRemoteSettings/CMake/1.0": { - "sourceDir": "$env{HOME}/.vs/$ms{projectDirName}" - } + "generator": "Ninja", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Release", + "CMAKE_PREFIX_PATH": "/usr/lib/x86_64-linux-gnu/cmake", + "HDF5_DIR": "", + "CMAKE_IGNORE_PATH": "/mnt/c/Program Files/HDF_Group/HDF5/2.0.0" + }, + "architecture": { + "value": "x64", + "strategy": "external" } } + ], + "buildPresets": [ + { + "name": "windows-x64-debug", + "configurePreset": "windows-x64", + "configuration": "Debug" + }, + { + "name": "windows-x64-release", + "configurePreset": "windows-x64", + "configuration": "Release" + }, + { + "name": "windows-x64-gui-debug", + "configurePreset": "windows-x64-gui", + "configuration": "Debug" + }, + { + "name": "windows-x64-gui-release", + "configurePreset": "windows-x64-gui", + "configuration": "Release" + }, + { + "name": "linux-x64-debug", + "configurePreset": "linux-x64-debug" + }, + { + "name": "linux-x64-release", + "configurePreset": "linux-x64-release" + } + ], + "testPresets": [ + { + "name": "windows-x64-release", + "configurePreset": "windows-x64", + "output": { "outputOnFailure": true } + }, + { + "name": "windows-x64-gui-release", + "configurePreset": "windows-x64-gui", + "output": { "outputOnFailure": true } + }, + { + "name": "linux-x64-release", + "configurePreset": "linux-x64-release", + "output": { "outputOnFailure": true } + } ] } diff --git a/DSFE_App/CMakeLists.txt b/DSFE_App/CMakeLists.txt index c594df20..d7dcadb1 100644 --- a/DSFE_App/CMakeLists.txt +++ b/DSFE_App/CMakeLists.txt @@ -1,9 +1,7 @@ -# CMakeLists.txt for the DSFE_App project -cmake_minimum_required(VERSION 3.10) -project(DSFE_App LANGUAGES C CXX) - -# Add projects within the DSFE_App directory +# CMakeLists.txt for the DSFE_App add_subdirectory(DSFE_Core) -add_subdirectory(DSFE_GUI) -add_subdirectory(DSFE_Engine) -add_subdirectory(DSFE_Unit_Tests) +option(DSFE_ENABLE_GUI "Enable GUI module" ON) +if(DSFE_ENABLE_GUI) + add_subdirectory(DSFE_GUI) +endif() +add_subdirectory(DSFE_Engine) \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 44714ecb..c9764056 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -1,27 +1,59 @@ # CMakeLists.txt for DSFE_Core library -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.17) project(DSFE_Core LANGUAGES C CXX) add_library(DSFE_Core SHARED) find_package(Threads REQUIRED) -find_package(HDF5 CONFIG REQUIRED) +find_package(HDF5 REQUIRED) +target_compile_features(DSFE_Core PUBLIC cxx_std_23) +if (MSVC) + target_compile_options(DSFE_Core PRIVATE /bigobj) +else() + target_compile_options(DSFE_Core PRIVATE -Wall -Wextra -Wpedantic) +endif() target_compile_definitions(DSFE_Core PRIVATE DSFE_CORE_EXPORTS) -set(CORE_SRC - src/EngineCore.cpp +target_compile_definitions(DSFE_Core PUBLIC + $<$:DSFE_PLATFORM_WINDOWS> + $<$:DSFE_PLATFORM_LINUX> +) + +if (NOT MSVC) + set_target_properties(DSFE_Core PROPERTIES + CXX_VISIBILITY_PRESET hidden + VISIBILITY_INLINES_HIDDEN YES + ) +endif() +set(DEP_CORE_SRC + src/EngineCore.cpp src/Numerics/IntegrationService.cpp +) + +set(SINGLE_BODY_SYS_SRC + src/SingleBodySystem/Body.cpp +) +set(MULTI_BODY_SYS_SRC src/Robots/RobotDynamics.cpp src/Robots/RobotKinematics.cpp src/Robots/RobotLoader.cpp src/Robots/RobotSystem.cpp src/Robots/TrajectoryManager.cpp src/Robots/RobotSimSnapshot.cpp - src/Robots/SpatialDynamics.cpp +) + +set(PLATFORM_SRC + src/Platform/DataManager.cpp + src/Platform/Logger.cpp + src/Platform/Paths.cpp + src/Platform/StudyRunner.cpp + src/Analysis/Telemetry.cpp +) +set(DSL_SRC src/Interpreter/Command.cpp src/Interpreter/CommandContext.cpp src/Interpreter/CommandFactory.cpp @@ -48,22 +80,16 @@ set(CORE_SRC src/Interpreter/Commands/WaitCmd.cpp ) -set(PLATFORM_SRC - src/Platform/DataManager.cpp - src/Platform/Logger.cpp - src/Platform/Paths.cpp - src/Platform/StudyRunner.cpp - - src/Analysis/Telemetry.cpp -) - -set(SCENE_SRC src/Scene/SimulationCore.cpp) +set(SIM_CORE_SRC src/Scene/SimulationCore.cpp) # Add sources for DSFE_Core, including ImGui files target_sources(DSFE_Core PRIVATE - ${CORE_SRC} + ${DEP_CORE_SRC} + ${SINGLE_BODY_SYS_SRC} + ${MULTI_BODY_SYS_SRC} ${PLATFORM_SRC} - ${SCENE_SRC} + ${DSL_SRC} + ${SIM_CORE_SRC} ) # Add Windows-specific source files @@ -75,8 +101,6 @@ endif() target_include_directories(DSFE_Core PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/include - - ${HDF5_INCLUDE_DIRS} ) # Use precompiled headers @@ -84,13 +108,22 @@ target_precompile_headers(DSFE_Core PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pch.h") # Link DSFE_Core against its dependencies target_link_libraries(DSFE_Core - PUBLIC - PxMLib + PUBLIC Eigen3::Eigen - hdf5::hdf5-shared + MathLib + PhysLib Threads::Threads PRIVATE - assimp nlohmann_json::nlohmann_json ) + +if (TARGET hdf5::hdf5-shared) + target_link_libraries(DSFE_Core PRIVATE hdf5::hdf5-shared) + +elseif (TARGET HDF5::HDF5) + target_link_libraries(DSFE_Core PRIVATE HDF5::HDF5) + +else() + target_link_libraries(DSFE_Core PRIVATE ${HDF5_LIBRARIES}) +endif() diff --git a/DSFE_App/DSFE_Core/framework.h b/DSFE_App/DSFE_Core/framework.h index 44b94928..0c34e144 100644 --- a/DSFE_App/DSFE_Core/framework.h +++ b/DSFE_App/DSFE_Core/framework.h @@ -1,7 +1,7 @@ +// DSFE_CORE framework.h #pragma once -// File: framework.h -// GitHub: SaltyJoss -#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers -// Windows Header Files -#define NOMINMAX -#include +#ifdef _WIN32 + #define WIN32_LEAN_AND_MEAN + #define NOMINMAX + #include +#endif diff --git a/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h b/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h index 2db3b7ce..dd80ed2b 100644 --- a/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h +++ b/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h @@ -2,6 +2,9 @@ #pragma once #include "EngineCore.h" + +#include + #include #include @@ -22,24 +25,24 @@ namespace diagnostics { // Struct for joint telemetry data struct DSFE_API JointTelemetry{ // Actual data - double q = 0.0f; // Joint angle in radians - double qd = 0.0f; // Joint angular velocity in radians per second + double q = 0.0; // Joint angle in radians + double qd = 0.0; // Joint angular velocity in radians per second // Additional dynamics data - double torqueNm = 0.0f; // Joint torque (N·m) - double damping = 0.0f; // Joint damping coefficient (kg·m²/s) - double friction = 0.0f; // Joint friction coefficient (Coulomb friction - N·m) - double effort = 0.0f; // Normalized effort (0 to 1) + double torqueNm = 0.0; // Joint torque (N·m) + double damping = 0.0; // Joint damping coefficient (kg·m²/s) + double friction = 0.0; // Joint friction coefficient (Coulomb friction - N·m) + double effort = 0.0; // Normalized effort (0 to 1) // Reference data - double q_ref = 0.0f; // Reference joint angle in radians - double qd_ref = 0.0f; // Reference joint angular velocity in radians per second - double qdd_ref = 0.0f; // Reference joint angular acceleration in radians per second squared + double q_ref = 0.0; // Reference joint angle in radians + double qd_ref = 0.0; // Reference joint angular velocity in radians per second + double qdd_ref = 0.0; // Reference joint angular acceleration in radians per second squared // Trajectory data - double traj_q = 0.0f; // Trajectory joint position - double traj_qd = 0.0f; // Trajectory joint velocity - double traj_qdd = 0.0f; // Trajectory joint acceleration + double traj_q = 0.0; // Trajectory joint position + double traj_qd = 0.0; // Trajectory joint velocity + double traj_qdd = 0.0; // Trajectory joint acceleration bool traj_active = false; // Trajectory active state for a joint // Limit clamping flags @@ -54,8 +57,8 @@ namespace diagnostics { std::vector j; // Vector of joint telemetry data // Summary statistics (precomputed to relieve analysis load) - double err_rms = 0.0f; // RMS error across all joints - double err_max = 0.0f; // Maximum error across all joints + double err_rms = 0.0; // RMS error across all joints + double err_max = 0.0; // Maximum error across all joints int clamp_theta = 0; // Sum of angle clamping events across all joints int clamp_omega = 0; // Sum of velocity clamping events across all joints int clamp_sum = 0; // Sum of clamping events across all joints @@ -77,7 +80,7 @@ namespace diagnostics { // Reset the ring buffer with new capacity void reset(size_t cap) { - _cap = std::max(cap, 1); + _cap = mathlib::max(cap, 1); _buf.clear(); _buf.resize(_cap); clear(); diff --git a/DSFE_App/DSFE_Core/include/EngineCore.h b/DSFE_App/DSFE_Core/include/EngineCore.h index 39c5847a..5ff21c4b 100644 --- a/DSFE_App/DSFE_Core/include/EngineCore.h +++ b/DSFE_App/DSFE_Core/include/EngineCore.h @@ -3,13 +3,15 @@ // Conditional compilation for cross-platform symbol export/import #ifdef _WIN32 -#ifdef DSFE_CORE_EXPORTS -#define DSFE_API __declspec(dllexport) + #ifdef DSFE_CORE_EXPORTS + #define DSFE_API __declspec(dllexport) + #else + #define DSFE_API __declspec(dllimport) + #endif +#elif defined(__GNUC__) || defined(__clang__) + #define DSFE_API __attribute__((visibility("default"))) #else -#define DSFE_API __declspec(dllimport) -#endif -#else -#define DSFE_API + #define DSFE_API #endif // Forward declarations for core components diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Command.h b/DSFE_App/DSFE_Core/include/Interpreter/Command.h index dbf78b89..6b223879 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Command.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Command.h @@ -1,9 +1,6 @@ // DSFE_Core Command.h #pragma once -#pragma warning(disable : 4100) - #include "EngineCore.h" - #include "ICommand.h" #include "MainContext.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h index 5ae247a3..cb4af02f 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h @@ -3,7 +3,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/Command.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h index 693c938c..f335e4e1 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h @@ -4,7 +4,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/Command.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h index 9a27afee..82377fb9 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h @@ -3,7 +3,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/Command.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h index 77b94e37..1801a83c 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h @@ -3,7 +3,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/Command.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h index 3b6e8a70..c67295d6 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h @@ -3,7 +3,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/Command.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h index 6a649b5e..f273026a 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h @@ -3,7 +3,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/Command.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h index 0f8e352e..1b1bebe6 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h @@ -3,7 +3,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/SimFwd.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h index 318e990c..39fd0c5d 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h @@ -3,7 +3,7 @@ #include "EngineCore.h" -#include + #include #include "Interpreter/Command.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h index 3f89ed4c..5c6ef41e 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h @@ -1,8 +1,6 @@ // DSFE_Core TrajClearCmd.h #pragma once - #include "EngineCore.h" - #include "Interpreter/SimFwd.h" #include "Interpreter/Command.h" #include "Interpreter/CommandContext.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h index 7091502f..f18f79e5 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h @@ -1,9 +1,6 @@ // DSFE_Core TrajSetCmd.h #pragma once -#pragma warning(disable : 4251) - #include "EngineCore.h" - #include "Interpreter/SimFwd.h" #include "Interpreter/Command.h" #include "Interpreter/CommandContext.h" diff --git a/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h b/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h index 0740468b..5ac25d7d 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h @@ -1,13 +1,8 @@ // DSFE_Core ProgramData.h #pragma once -#pragma warning(disable : 4251) - #include "EngineCore.h" - -#include #include #include - #include #include @@ -49,7 +44,11 @@ namespace program_data { ImplicitEuler, ImplicitMidpoint, GLRK2, - GLRK3 + GLRK3, + AD_ImplicitEuler, + AD_ImplicitMidpoint, + AD_GLRK2, + AD_GLRK3 }; // Enum for preset colours diff --git a/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h b/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h index 6380da7e..fbc718fe 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h +++ b/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h @@ -2,7 +2,7 @@ #pragma once #include "EngineCore.h" -#include + #include #include #include diff --git a/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl new file mode 100644 index 00000000..63a722e4 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Numerics/AutoDiffStep.inl @@ -0,0 +1,51 @@ +// DSFE_Core AutoDiffStep.inl +#pragma once + +namespace integration { + template + StepOut_T DifferentiableIntegrator::step(eAutoDiffIntegrationMethod m, const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f) { + using Real = typename mathlib::DualTraits::BaseScalar; + if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { + if (f == nullptr) { + D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative."); + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 30, 1e-6), dt, dt }; + } + } + + LOG_INFO_ONCE("Using AutoDiff integration method: %s", IntegratorName(m).c_str()); + + Real dt_r = mathlib::real(dt); + + _state->backend = eIntegrationBackend::AutoDiff; + _state->autoDiff = true; + _state->last_dt_taken = (_state->last_dt_taken != dt && !_state->adaptive) ? dt_r : _state->last_dt_taken; // Unless RK45-or other adaptive methods-it is static. + _state->last_dt_sug = (_state->last_dt_sug != dt && !_state->adaptive) ? dt_r : _state->last_dt_sug; // Unless RK45-or other adaptive methods-it is static. + + auto* state = runtimeState().get(); + + // Though right now all methods hold the same fundamental state in AD, I am employing the same structure for consistency + switch (m) { + case eAutoDiffIntegrationMethod::AD_ImplicitEuler: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 30, 1e-6), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitMidpoint_AD(x, t, dt, std::forward(f), 10, 1e-7), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK2: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK2_AD(x, t, dt, std::forward(f), 80, 1e-10), dt_r, dt_r }; + case eAutoDiffIntegrationMethod::AD_GLRK3: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK3_AD(x, t, dt, std::forward(f), 150, 1e-14), dt_r, dt_r }; + default: + LOG_WARN("Unknown Integrator, defaulting to ImplicitEuler (AutoDiff)."); + _state->name = IntegratorName(eAutoDiffIntegrationMethod::AD_ImplicitEuler); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitEuler_AD(x, t, dt, std::forward(f), 30, 1e-6), dt_r, dt_r }; + } + } +} // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h index 75182e1c..f7efa114 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationMethods.h @@ -1,3 +1,4 @@ +// DSFE_Core IntegrationMethods.h #pragma once namespace integration { @@ -13,4 +14,16 @@ namespace integration { GLRK2 = 8, // Gauss-Legendre Runge-Kutta Method (2 stages, 4th order, implicit) GLRK3 = 9 // Gauss-Legendre Runge-Kutta Method (3 stages, 6th order, implicit) }; + + enum class eAutoDiffIntegrationMethod { + AD_ImplicitEuler = 0, + AD_ImplicitMidpoint = 1, + AD_GLRK2 = 2, + AD_GLRK3 = 3 + }; + + inline bool isStandardMethod(eIntegrationMethod method) { + auto val = static_cast(method); + return val >= static_cast(eIntegrationMethod::Euler) && val <= static_cast(eIntegrationMethod::GLRK3); + } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h index a453c920..979312e3 100644 --- a/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationService.h @@ -1,145 +1,81 @@ +// DSFE_Core IntegrationService.h #pragma once -// File: IntegrationService.h -// GitHub: SaltyJoss -#pragma warning(disable : 4251) - #include "EngineCore.h" -#include -#include -#include +#include #include #include "Numerics/IntegrationMethods.h" +#include "Numerics/IntegratorState.h" #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" namespace integration { // Struct representing the result of a single integration step - struct DSFE_API StepOut { - VecX x_next; // next state vector - double dt_taken = 0.0; // actual step size taken - double dt_sug = 0.0; // suggested next step size + template + struct DSFE_API StepOut_T { + mathlib::VecX_T x_next; // next state vector + typename mathlib::DualTraits::BaseScalar dt_taken = 0.0; // actual step size taken + typename mathlib::DualTraits::BaseScalar dt_sug = 0.0; // suggested next step size }; // Class representing the integration service class DSFE_API IntegrationService { public: - // Constructor + using StepOut = StepOut_T; IntegrationService(); template - StepOut stepODE(eIntegrationMethod m, VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { - if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { - if (f == nullptr) { - D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); - return { _ODE->eulerStep(x, t, dt, std::forward(f)), dt, dt }; - } - } - - switch (m) { - // Explicit methods - // * currently all explicit methods use fixed step size, apart from RK45 as it is an adaptive method - case eIntegrationMethod::Euler: return { _ODE->eulerStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Midpoint: return { _ODE->midpointStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Heun: return { _ODE->heunStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::Ralston: return { _ODE->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK4: return { _ODE->rk4Step(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::RK45: return stepAdaptiveODE(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); - // Implicit methods - // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future - case eIntegrationMethod::ImplicitEuler: return { _ODE->implicit_euler(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::ImplicitMidpoint: return { _ODE->implicit_midpoint(x, t, dt, std::forward(f)), dt, dt }; - case eIntegrationMethod::GLRK2: return { _ODE->GLRK2(x, t, dt, std::forward(f), 50, 1e-10, std::forward(jac)), dt, dt }; - case eIntegrationMethod::GLRK3: return { _ODE->GLRK3(x, t, dt, std::forward(f), 80, -1, std::forward(jac)), dt, dt }; - default: - LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); - return { _ODE->rk4Step(x, t, dt, std::forward(f)), dt, dt }; - } - } - + StepOut step(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac); template - StepOut stepAdaptiveODE(eIntegrationMethod m, VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { - if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { - if (f == nullptr) { - LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); - return { x, dt_try, dt_try }; - } - } - if (m != eIntegrationMethod::RK45) { - LOG_WARN("Adaptive step size integration is only implemented for RK45 method. Defaulting to RK45 Method", toString(m)); - } - - // Start with the last successful step size or the initial guess - double h_init = (_dt_last > 0.0) ? _dt_last : dt_try; - - // Enforce maximum step size if set - if (_dt_max > 0.0) { - h_init = std::min(h_init, _dt_max); - } - double h = std::min(h_init, dt_try); - - // Target end time for this adaptive step - const double t_end = t + dt_try; // target end time for this step - const double eps = 1e-12 * dt_try; // small epsilon to prevent division by zero - - // Initialise current state and time for the adaptive stepping loop - VecX x_curr = x; // current state during the adaptive step - double t_curr = t; // current time during the adaptive step - double t_total = 0.0; // total time taken for the step - - // Limit the number of substeps to prevent infinite loop - int substeps = 0; - const int max_substeps = 500; // safety limit - - // Loop until we reach the target end time or exceed the maximum number of substeps - while (t_curr < t_end && substeps < max_substeps) { - double h_try = std::min(h, t_end - t_curr); - double dt_used = 0.0; - - VecX x_next = _ODE->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); - - // Update rk45step - t_curr += dt_used; - t_total += dt_used; - x_curr = x_next; - h = h_try; - - ++substeps; - } - - // If substep limit was reached, a warning is logged - if (substeps >= max_substeps) { - LOG_WARN("Adaptive integration exceeded maximum substeps (%d) at time %f. Returning last computed state.", max_substeps, t_curr); - } - - // Persist the last good step size for next frame - _dt_last = h; - return { x_curr, t_total, h }; - } + StepOut step_adaptive(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol); void setIntegrationMethod(eIntegrationMethod m) { method = m; } eIntegrationMethod getIntegrationMethod() const { return method; } - const std::string IntegratorName(eIntegrationMethod m); - void setAdaptiveTolerances(double rtol, double atol) { _rtol = rtol; _atol = atol; } void setMaxStep(double max_dt) { _dt_max = max_dt; } - - // Reset the cached adaptive step size (call on robot load/reset) void resetAdaptiveState() { _dt_last = 0.0; } + std::shared_ptr runtimeState() { return _state; } + std::shared_ptr runtimeState() const { return _state; } private: const char* toString(eIntegrationMethod m); integration::eIntegrationMethod method; - std::unique_ptr _ODE; - + std::unique_ptr _integrator; std::string _methodStr = "RK4"; + std::shared_ptr _state; + double _rtol; double _atol; - double _dt_last = 0.0; // last successful step double _dt_max = 0.0; // maximum allowed step size }; + + class DSFE_API DifferentiableIntegrator { + public: + DifferentiableIntegrator(); + + template + StepOut_T step(eAutoDiffIntegrationMethod m, const mathlib::VecX_T& x, Scalar t, Scalar dt, Func&& f); + const std::string IntegratorName(eAutoDiffIntegrationMethod m); + void setIntegrationMethod(eAutoDiffIntegrationMethod m) { _m = m; LOG_INFO("AD integrator changed to enum %d", (int)_m); } + eAutoDiffIntegrationMethod integrationMethod() const { return _m; } + std::shared_ptr runtimeState() { return _state; } + std::shared_ptr runtimeState() const { return _state; } + + private: + const char* toString(eAutoDiffIntegrationMethod m); + + integration::eAutoDiffIntegrationMethod _m; + std::unique_ptr _integrator; + std::string _mStr = "AD_ImplicitEuler"; + + std::shared_ptr _state; + }; + } // namespace integration + +#include "IntegrationStep.inl" +#include "AutoDiffStep.inl" diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl new file mode 100644 index 00000000..0090c76e --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegrationStep.inl @@ -0,0 +1,141 @@ +// DSFE_Core IntegrationStep.inl +#pragma once + +namespace integration { + template + StepOut_T IntegrationService::step(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt, Func&& f, JacFunc&& jac) { + if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { + if (f == nullptr) { + D_WARN_ONCE("No derivative function provided for integration - Assuming constant derivative (Euler step)"); + return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; + } + } + + LOG_INFO_ONCE("Using integration method: %s", IntegratorName(m).c_str()); + + _state->backend = eIntegrationBackend::Standard; + _state->last_dt_taken = (_state->last_dt_taken != dt && !_state->adaptive) ? dt : _state->last_dt_taken; + _state->last_dt_sug = (_state->last_dt_sug != dt && !_state->adaptive) ? dt : _state->last_dt_sug; + _state->autoDiff = false; + + switch (m) { + // Explicit methods + // * currently all explicit methods use fixed step size, apart from RK45 as it is an adaptive method + case eIntegrationMethod::Euler: + _state->name = IntegratorName(m); // all of these might add more time complexity than what is needed. + _state->adaptive = false; _state->implicit = false; + return { _integrator->eulerStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Midpoint: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->midpointStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Heun: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->heunStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::Ralston: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->ralstonStep(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK4: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = false; + return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + case eIntegrationMethod::RK45: + _state->name = IntegratorName(m); + _state->adaptive = true; _state->implicit = false; + return step_adaptive(eIntegrationMethod::RK45, x, t, dt, std::forward(f), _rtol, _atol); + // Implicit methods + // * currently use fixed step size (no error estimation), but are likely to support adaptive stepping in the future + case eIntegrationMethod::ImplicitEuler: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitEuler(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::ImplicitMidpoint: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->implicitMidpoint(x, t, dt, std::forward(f), std::forward(jac)), dt, dt }; + case eIntegrationMethod::GLRK2: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK2(x, t, dt, std::forward(f), std::forward(jac), 80, 1e-10), dt, dt }; + case eIntegrationMethod::GLRK3: + _state->name = IntegratorName(m); + _state->adaptive = false; _state->implicit = true; + return { _integrator->GLRK3(x, t, dt, std::forward(f), std::forward(jac), 150, 1e-14), dt, dt }; + default: + LOG_WARN("Unknown integration method: %s. Defaulting to RK4.", toString(m)); + _state->name = IntegratorName(eIntegrationMethod::RK4); + _state->adaptive = false; _state->implicit = false; + return { _integrator->rk4Step(x, t, dt, std::forward(f)), dt, dt }; + } + } + + template + StepOut_T IntegrationService::step_adaptive(eIntegrationMethod m, const mathlib::VecX& x, double t, double dt_try, Func&& f, double rtol, double atol) { + if constexpr (std::is_pointer_v> || requires { f == nullptr; }) { + if (f == nullptr) { + LOG_WARN("No derivative function provided for adaptive integration - returning state unchanged"); + return { x, dt_try, dt_try }; + } + } + if (m != eIntegrationMethod::RK45) { + LOG_WARN("Adaptive step size integration is only implemented for RK45 method. Defaulting to RK45 Method", toString(m)); + } + + // Start with the last successful step size or the initial guess + double h_init = (_dt_last > 0.0) ? _dt_last : dt_try; + + // Enforce maximum step size if set + if (_dt_max > 0.0) { + h_init = std::min(h_init, _dt_max); + } + double h = std::min(h_init, dt_try); + + // Target end time for this adaptive step + const double t_end = t + dt_try; // target end time for this step + const double eps = 1e-12 * dt_try; // small epsilon to prevent division by zero + + // Initialise current state and time for the adaptive stepping loop + mathlib::VecX x_curr = x; // current state during the adaptive step + double t_curr = t; // current time during the adaptive step + double t_total = 0.0; // total time taken for the step + + // Limit the number of substeps to prevent infinite loop + int substeps = 0; + const int max_substeps = 500; // safety limit + + if (h <= eps) { + LOG_WARN("Adaptive integration step size is too small (%e). Returning last computed state.", h); + return { x_curr, t_total, h }; + } + + // Loop until we reach the target end time or exceed the maximum number of substeps + while (t_curr < t_end && substeps < max_substeps) { + double h_try = std::min(h, t_end - t_curr); + double dt_used = 0.0; + + mathlib::VecX x_next = _integrator->rk45Step(x_curr, t_curr, h_try, dt_used, std::forward(f), rtol, atol); + + // Update rk45step + t_curr += dt_used; + t_total += dt_used; + x_curr = x_next; + h = h_try; + + ++substeps; + } + + // If substep limit was reached, a warning is logged + if (substeps >= max_substeps) { + LOG_WARN("Adaptive integration exceeded maximum substeps (%d) at time %f. Returning last computed state.", max_substeps, t_curr); + } + + _state->last_dt_taken = t_total; + _state->last_dt_sug = h; + + // Persist the last good step size for next frame + _dt_last = h; + return { x_curr, t_total, h }; + } +} // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Numerics/IntegratorState.h b/DSFE_App/DSFE_Core/include/Numerics/IntegratorState.h new file mode 100644 index 00000000..f314a738 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Numerics/IntegratorState.h @@ -0,0 +1,23 @@ +// DSFE_Core IntegratorState.h +#pragma once + +#include + +namespace integration { + // Current simulation backend integration method (e.g., standard numerical integration[explicit, implicit] vs. auto-differentiation for gradients) + enum class eIntegrationBackend { + Standard, + AutoDiff + }; + + struct IntegratorState { + std::string name; + eIntegrationBackend backend = eIntegrationBackend::Standard; + bool adaptive = false; + bool implicit = false; // false means explicit, TODO adapt this correctly for semi- once integrated + bool autoDiff = false; + + double last_dt_taken = 0.0; + double last_dt_sug = 0.0; + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Platform/DataManager.h b/DSFE_App/DSFE_Core/include/Platform/DataManager.h index d15a04f0..b3b6f569 100644 --- a/DSFE_App/DSFE_Core/include/Platform/DataManager.h +++ b/DSFE_App/DSFE_Core/include/Platform/DataManager.h @@ -1,7 +1,5 @@ // DSFE_Core DataManager.h #pragma once - -#pragma warning(disable : 4251) #include "EngineCore.h" #include #include @@ -24,12 +22,7 @@ namespace robots { struct JointLogBuffer; } namespace data { // Variant type to hold different data types - using Value = std::variant< - std::nullptr_t, bool, int64_t, uint64_t, double, long, - std::vector, - std::vector, std::string - >; - + using Value = std::variant, std::vector, std::string>; // Field type representing a key-value pair using Field = std::pair; // List of fields diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index d4e5c4d3..dec47dd0 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -7,12 +7,15 @@ #include // Forward Declarations -namespace integration { enum class eIntegrationMethod; } +namespace integration { enum class eIntegrationMethod; enum class eAutoDiffIntegrationMethod; } namespace robots { class RobotSystem; } +namespace single_body_system { class SingleBodySystem; } namespace control { class TrajectoryManager; } namespace diagnostics { class TelemetryRecorder; } namespace interpreter { class IStoredProgram; } +enum class eSimulationBackend; + namespace core { struct DSFE_API SimulationSnapshot { double simTime; @@ -36,13 +39,20 @@ namespace core { virtual SimulationSnapshot snapshot() const = 0; // Integrator virtual void setIntegrationMethod(integration::eIntegrationMethod method) = 0; + virtual void setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) = 0; virtual std::string integrationMethodName() const = 0; virtual integration::eIntegrationMethod integrationMethod() const = 0; + virtual integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const = 0; + virtual void enableAutoDiff(bool enable) = 0; // Setter for run tag name of current script virtual void setRunTag(const std::string& tag) = 0; // Subsystems virtual robots::RobotSystem* robotSystem() = 0; + virtual single_body_system::SingleBodySystem* singleBodySystem() = 0; virtual control::TrajectoryManager* trajectoryManager() = 0; + // Body management + virtual bool hasSingleBody() const = 0; + virtual void loadSingleBody(const std::string& name) = 0; // Robot management virtual bool hasRobot() const = 0; virtual void loadRobot(const std::string& name) = 0; diff --git a/DSFE_App/DSFE_Core/include/Platform/Logger.h b/DSFE_App/DSFE_Core/include/Platform/Logger.h index 6de335ec..8dc918cd 100644 --- a/DSFE_App/DSFE_Core/include/Platform/Logger.h +++ b/DSFE_App/DSFE_Core/include/Platform/Logger.h @@ -1,7 +1,5 @@ +// DSFE_CORE Logger.h #pragma once -// File: Logger.h -// GitHub: SaltyJoss -#pragma warning(disable : 4251) #include "EngineCore.h" #include @@ -14,6 +12,7 @@ #include #include #include +#include // Log levels for debug panel and sim log enum class LogLevel { Trace, Debug, Info, Export, Warning, Error, Success, Fail, Runtime, Output }; @@ -47,7 +46,11 @@ class DSFE_API Debug { auto now = std::chrono::system_clock::now(); std::time_t now_time = std::chrono::system_clock::to_time_t(now); std::tm tm_data; + #ifdef _WIN32 localtime_s(&tm_data, &now_time); + #else + localtime_r(&now_time, &tm_data); + #endif std::ostringstream oss; oss << "Log/session_" << std::put_time(&tm_data, "%Y%m%d_%H%M%S") << ".txt"; @@ -157,7 +160,11 @@ class DSFE_API Debug { auto now = std::chrono::system_clock::now(); std::time_t now_time = std::chrono::system_clock::to_time_t(now); std::tm tm_data; + #ifdef _WIN32 localtime_s(&tm_data, &now_time); + #else + localtime_r(&now_time, &tm_data); + #endif std::ostringstream oss; oss << "[" << std::put_time(&tm_data, "%Y-%m-%d %H:%M:%S") << "] " diff --git a/DSFE_App/DSFE_Core/include/Platform/Paths.h b/DSFE_App/DSFE_Core/include/Platform/Paths.h index 15969be5..c4c1fbf6 100644 --- a/DSFE_App/DSFE_Core/include/Platform/Paths.h +++ b/DSFE_App/DSFE_Core/include/Platform/Paths.h @@ -1,6 +1,5 @@ +// DSFE_Core Paths.h #pragma once -// File: Paths.h -// GitHub: SaltyJoss #include "EngineCore.h" #include #include diff --git a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h index 50173444..446bbee0 100644 --- a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h +++ b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h @@ -38,4 +38,12 @@ struct DSFE_API modes { eRunMode _runMode = eRunMode::Interactive; }; - +// Struct to hold comparison results for integrator analysis +struct ComparisonSnapshot { + std::string integratorName; + std::vector time; // time samples + std::vector errRms; // RMS error time series + std::vector errMax; // Max error time series + std::vector> jointErr; // [joint][sample] + int jointCount = 0; +}; \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index df225154..d3a485a0 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -2,7 +2,6 @@ #pragma once #include "EngineCore.h" -#include "MathLibAPI.h" #include #include @@ -10,16 +9,16 @@ namespace robots { // Scratch buffers for dense dynamics + template struct DenseDynamicsScratch { - mathlib::MatX M; // mass matrix - mathlib::VecX rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) - mathlib::VecX h; // Coriolis and centrifugal bias vector - mathlib::VecX tau; // control torque vector + mathlib::MatX_T M; // mass matrix + mathlib::VecX_T rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) + mathlib::VecX_T h; // Coriolis and centrifugal bias vector + mathlib::VecX_T tau; // control torque vector + mathlib::VecX_T I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) - mathlib::VecX I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) - - std::vector T_world; - std::vector jointWorldPoses; + std::vector> T_world; + std::vector> jointWorldPoses; size_t jointCap = 0; size_t linkCap = 0; @@ -73,26 +72,27 @@ namespace robots { }; // Scratch buffers for spatial dynamics computations + template struct SpatialDynamicsScratch { - std::vector Xup; // spatial transformation from parent to current link - std::vector IA; // articulated body inertia - std::vector Ia; // articulated body inertia in the link frame + std::vector> Xup; // spatial transformation from parent to current link + std::vector> IA; // articulated body inertia + std::vector> Ia; // articulated body inertia in the link frame - std::vector v; // spatial velocity - std::vector c; // spatial bias acceleration - std::vector a; // spatial acceleration - std::vector pA; // articulated bias force - std::vector U; // articulated body force + std::vector> v; // spatial velocity + std::vector> c; // spatial bias acceleration + std::vector> a; // spatial acceleration + std::vector> pA; // articulated bias force + std::vector> U; // articulated body force - mathlib::VecX u; // joint force contribution - mathlib::VecX d; // joint inertia contribution + mathlib::VecX_T u; // joint force contribution + mathlib::VecX_T d; // joint inertia contribution - std::vector> dXup_dq; // derivative of spatial transformation w.r.t. joint angles + std::vector>> dXup_dq; // derivative of spatial transformation w.r.t. joint angles - std::vector> dv_dq; // derivative of spatial velocity w.r.t. joint angles - std::vector> dv_dqd; // derivative of spatial velocity w.r.t. joint velocities - std::vector> dc_dq; // derivative of spatial bias acceleration w.r.t. joint angles - std::vector> dc_dqd; // derivative of spatial bias acceleration w.r.t. joint velocities + std::vector>> dv_dq; // derivative of spatial velocity w.r.t. joint angles + std::vector>> dv_dqd; // derivative of spatial velocity w.r.t. joint velocities + std::vector>> dc_dq; // derivative of spatial bias acceleration w.r.t. joint angles + std::vector>> dc_dqd; // derivative of spatial bias acceleration w.r.t. joint velocities size_t jointCap = 0; @@ -113,6 +113,12 @@ namespace robots { u.resize(nJoints); d.resize(nJoints); + dXup_dq.resize(nJoints); + dv_dq.resize(nJoints); + dv_dqd.resize(nJoints); + dc_dq.resize(nJoints); + dc_dqd.resize(nJoints); + jointCap = nJoints; } @@ -127,16 +133,24 @@ namespace robots { U.clear(); u.resize(0); d.resize(0); + + dXup_dq.clear(); + dv_dq.clear(); + dv_dqd.clear(); + dc_dq.clear(); + dc_dqd.clear(); + jointCap = 0; } }; // Output structure for dynamics computations + template struct DynamicsResult { - mathlib::VecX dxdt; - mathlib::VecX qdd; + mathlib::VecX_T dxdt; + mathlib::VecX_T qdd; - RobotMetrics metrics; + RobotMetrics metrics; void resize(size_t n) { dxdt.resize(2 * n); @@ -146,13 +160,14 @@ namespace robots { }; // Central scratch structure that contains all buffers needed for dynamics computations, both dense and spatial + template struct DynamicsScratch { - DenseDynamicsScratch dense; - SpatialDynamicsScratch spatial; + DenseDynamicsScratch dense; + SpatialDynamicsScratch spatial; // TODO add kinematics scratch // Gravity scratch buffer - mathlib::VecX g; + mathlib::VecX_T g; // Resizes all scratch buffers using the given number of joints and links void resize(size_t nJoints, size_t nLinks) { @@ -161,8 +176,6 @@ namespace robots { g.resize(nJoints); } - - // Clears all scratch buffers void clear() { dense.clear(); diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h index bfda0f5e..c9a850b3 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h @@ -2,21 +2,29 @@ #pragma once #include "EngineCore.h" -#include "MathLibAPI.h" -#include "core/Types.h" +#include + #include "Robots/DynamicsTypes.h" #include "Robots/RobotMetrics.h" +#include "Robots/SpatialDynamics.h" +#include "Robots/SpatialModel.h" +#include "Robots/RobotKinematics.h" +#include "Robots/RobotSimSnapshot.h" + +#include "Robots/TrajectoryManager.h" + +#include +#include + +#include "EngineLib/LogMacros.h" + // Forward declarations namespace control { class TrajectoryManager; } namespace integration { class IntegrationService; enum class eIntegrationMethod; } namespace robots { // Forward declarations - class RobotKinematics; - struct RobotConstModel; - struct SpatialModel; - struct RobotSimSnapshot; struct RobotLink; struct RobotJoint; enum class eTorqueMode; @@ -28,89 +36,106 @@ namespace robots { RobotDynamics(); // Computes the inertia tensor of a robot link - mathlib::Mat3 computeLinkInertiaTensor(const RobotLink& link) const; + template + mathlib::Mat3_T computeLinkInertiaTensor(const RobotLink& link) const; // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint - double computeJointInertiaContribution( + template + Scalar computeJointInertiaContribution( const RobotJoint& joint, const RobotLink& link, - const mathlib::Pose& jointWorldPose, - const mathlib::Pose& linkWorldPose + const mathlib::Pose_T& jointWorldPose, + const mathlib::Pose_T& linkWorldPose ) const; // Computes the full mass matrix M(q) based on the current state and robot configuration + template void computeMassMatrix( const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses, - mathlib::MatX& M_out + const std::vector>& T_world, + const std::vector>& jointWorldPoses, + mathlib::MatX_T& M_out ) const; // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration - mathlib::VecX computeCoriolisVector( + template + mathlib::VecX_T computeCoriolisVector( const RobotConstModel& robot, - const mathlib::VecX& q, const mathlib::VecX& qd, - const std::vector& T_world, - const mathlib::MatX& M + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const std::vector>& T_world, + const mathlib::MatX_T& M ) const; // Computes the gravity torque for a joint based on the current state and robot configuration - mathlib::VecX computeGravityTorque( + template + mathlib::VecX_T computeGravityTorque( const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses + const std::vector>& T_world, + const std::vector>& jointWorldPoses ) const; // Computes the analytical Jacobian matrix J(q) for the robot based on the current state and robot configuration + template void analyticalJacobian( const RobotConstModel& robot, - const mathlib::VecX& x, - mathlib::MatX& J_out, - DenseDynamicsScratch& scratch + const mathlib::VecX_T& x, + mathlib::MatX_T& J_out, + DenseDynamicsScratch& scratch ); // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration - mathlib::VecX derivative_dense( - double t, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out + template + mathlib::VecX_T derivative_dense( + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out ); - mathlib::VecX derivative_spatial( - const robots::SpatialModel& model, - double t, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out + template + mathlib::VecX_T derivative_spatial( + const robots::SpatialModel& model, + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out ); + template void jacobian_spatial( - const robots::SpatialModel& model, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - const mathlib::VecX& kp, - const mathlib::VecX& kd, - mathlib::MatX& F_out, - DynamicsScratch& scratch + const robots::SpatialModel& model, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DynamicsScratch& scratch ); // Computes the derivative of the state vector with control gains based on the current state and robot configurations - mathlib::VecX derivative_with_gains( - double t, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - const mathlib::VecX& kp,const mathlib::VecX& kd, - DynamicsScratch& scratch, DynamicsResult& out + template + mathlib::VecX_T derivative_with_gains( + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + DynamicsScratch& scratch, + DynamicsResult& out ); // Computes the Jacobian matrix with control gains based on the current state and robot configuration + template void jacobian_with_gains( - const mathlib::VecX& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp, const mathlib::VecX& kd, mathlib::MatX& F_out, - DenseDynamicsScratch& scratch + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DenseDynamicsScratch& scratch ); // Set the gravity strength for the robot system @@ -125,10 +150,19 @@ namespace robots { // References and pointers std::unique_ptr _kinematics = nullptr; + static bool isControlledJoint(eJointType t) { + return + t == eJointType::REVOLUTE || + t == eJointType::PRISMATIC; + } + double _dt = 1.0 / 180.0; // default timestep for dynamics updates double _gravity{ 0.0 }; bool _baseIsFree = false; double _lastBaseForwardForce{ 0.0 }; }; -} // namespace robots \ No newline at end of file +} // namespace robots + + +#include "Robots/RobotDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl new file mode 100644 index 00000000..3469ab99 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl @@ -0,0 +1,644 @@ +// DSFE_Core RobotDynamics.inl +#pragma once + +namespace robots { + // Computes the inertia tensor of a robot link + template + mathlib::Mat3_T RobotDynamics::computeLinkInertiaTensor(const RobotLink& link) const { + const robots::Inertia& I = link.inertial.inertia; + + // Construct the inertia tensor matrix + mathlib::Mat3_T M = mathlib::Mat3_T::Zero(); + M << + I.ixx, I.ixy, I.ixz, + I.ixy, I.iyy, I.iyz, + I.ixz, I.iyz, I.izz; + + return M; // [kg*m^2], (3x3) inertia tensor in link frame + } + + // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint + template + Scalar RobotDynamics::computeJointInertiaContribution( + const RobotJoint& joint, + const RobotLink& link, + const mathlib::Pose_T& jointWorldPose, + const mathlib::Pose_T& linkWorldPose + ) const { + const Scalar m = (Scalar)link.inertial.mass; + + // Rotation from link frame to world frame + mathlib::Mat3_T R_joint = jointWorldPose.template block<3, 3>(0, 0); + // Joint axis in world frame + mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_joint * joint.axis); + + // Position of joint in world frame + mathlib::Vec3_T joint_pos_world = jointWorldPose.template block<3, 1>(0, 3); + + // Rotation from link frame to world frame + mathlib::Mat3_T R_link = linkWorldPose.template block<3, 3>(0, 0); + mathlib::Vec3_T link_pos_world = linkWorldPose.template block<3, 1>(0, 3); + mathlib::Vec3_T com_world = R_link * Vec3_T(link.inertial.com_xyz) + link_pos_world; + + // Translational contribution (parallel axis theorem) + mathlib::Vec3_T r = com_world - joint_pos_world; + + // Translational contribution to inertia about the joint axis + Scalar I_trans = m * (axis_world.cross(r)).squaredNorm(); + + // Rotational contribution + mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); + mathlib::Mat3_T I_world = R_link * I_local * R_link.transpose(); + + // Rotational contribution to inertia about the joint axis + Scalar I_rot = axis_world.transpose() * I_world * axis_world; + Scalar I_eff_i = I_trans + I_rot; + + return Eigen::numext::maxi(I_eff_i, Scalar(1e-6)); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities + } + + // Computes the full mass matrix M(q) based on the current state and robot configuration + template + void RobotDynamics::computeMassMatrix( + const RobotConstModel& robot, + const std::vector>& T_world, + const std::vector>& jointWorldPoses, + mathlib::MatX_T& M_out + ) const { + const size_t n = robot.joints.size(); + M_out.resize(n, n); + M_out.setZero(); + + // Compute its contribution to the mass matrix for each link + for (size_t k = 0; k < robot.links.size(); ++k) { + const RobotLink& link = robot.links[k]; + const Scalar m = link.inertial.mass; + + if (m <= Scalar(0)) { continue; } + + const mathlib::Mat3_T R = T_world[k].template block<3, 3>(0, 0); // Rotation from link frame to world frame + const mathlib::Vec3_T p = T_world[k].template block<3, 1>(0, 3); // Center of mass of the link in world frame + const mathlib::Vec3_T com = R * link.inertial.com_xyz + p; // Center of mass in world frame + + mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); // inertia tensor in link frame + mathlib::Mat3_T I_world = R * I_local * R.transpose(); // inertia tensor in world frame + + // Compute Jacobian columns for each joint and accumulate mass matrix contributions + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j_i = robot.joints[i]; + if (j_i.type == eJointType::FIXED) { continue; } + if (!robot.jointAffectsLink(i, k)) { continue; } // skip if joint i does not affect link k + + const mathlib::Pose_T& T_joint_i = jointWorldPoses[i]; // pose of joint i in world frame + + // Rotation from joint i frame to world frame + const mathlib::Mat3_T R_i = T_joint_i.template block<3, 3>(0, 0); // rotation from joint i frame to world frame + const mathlib::Vec3_T p_i = T_joint_i.template block<3, 1>(0, 3); // joint position in world frame + const mathlib::Vec3_T z_i = mathlib::safeNormalised(R_i * j_i.axis); // joint axis in world frame + + mathlib::Vec3_T J_vi = z_i.cross(com - p_i); // linear velocity Jacobian column for joint i + mathlib::Vec3_T J_wi = z_i; // angular velocity Jacobian column for joint i + + // Computes the contribution to the mass matrix from this link for joints i and j + for (size_t j = 0; j < n; ++j) { + const RobotJoint& j_j = robot.joints[j]; + if (j_j.type == eJointType::FIXED) { continue; } + + if (!robot.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k + + const mathlib::Pose_T& T_joint_j = jointWorldPoses[j]; // pose of joint i in world frame + + const mathlib::Mat3_T R_j = T_joint_j.template block<3, 3>(0, 0); + const mathlib::Vec3_T p_j = T_joint_j.template block<3, 1>(0, 3); + const mathlib::Vec3_T z_j = mathlib::safeNormalised(R_j * j_j.axis); + + mathlib::Vec3_T J_vj = z_j.cross(com - p_j); // linear velocity Jacobian column for joint j + mathlib::Vec3_T J_wj = z_j; // angular velocity Jacobian column for joint j + + M_out(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; // [kg*m^2] + } + } + } + } + + // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration + template + mathlib::VecX_T RobotDynamics::computeCoriolisVector( + const RobotConstModel& robot, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const std::vector>& T_world, + const mathlib::MatX_T& M + ) const { + const size_t n = robot.joints.size(); + const Scalar eps = Scalar(1e-6); // small value to prevent division by zero + mathlib::VecX_T q_eps = q; + + std::vector> dM_dq(n, mathlib::MatX_T::Zero(n, n)); // partial derivatives of M with respect to each joint angle + mathlib::VecX_T x_eps(2 * n); // state vector for kinematics + + std::vector> T_world_eps; // forward kinematics for perturbed configurations + T_world_eps.resize(robot.links.size()); + + std::vector> jointWorldPoses_eps; + jointWorldPoses_eps.resize(n); + + mathlib::MatX_T M_plus(n, n); + + // Finite difference approximation of dM/dq for each joint + for (size_t k = 0; k < n; ++k) { + q_eps = q; // reset to original configuration for each joint perturbation + q_eps[k] += eps; // perturb joint k by a small amount + + // Construct the state vector for the perturbed configuration + for (size_t i = 0; i < n; ++i) { + x_eps[i] = q_eps[i]; + x_eps[n + i] = qd[i]; + } + + // Compute forward kinematics for the perturbed state + _kinematics->computeForwardKinematics_fromState(robot, x_eps, T_world_eps); + jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); + computeMassMatrix(robot, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration + + if (!M_plus.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } + if (!M_plus.isApprox(M_plus, Scalar(1e-8))) { throw std::runtime_error("Mass matrix lost symmetry"); } + + dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with + } + + // Compute Coriolis and centrifugal bias vector h using Christoffel symbols of the first kind + VecX_T h = VecX_T::Zero(n); + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n; ++j) { + for (size_t k = 0; k < n; ++k) { + Scalar C_ijk = 0.5 * (dM_dq[k](i, j) + dM_dq[j](i, k) - dM_dq[i](j, k)) * qd[k]; // Christoffel symbol of the first kind for indices (i, j, k) + h(i) += C_ijk * qd[j] * qd[k]; // contribution to Coriolis and centrifugal bias for joint i from joints j and k + } + } + } + return h; // [Nm], Coriolis and centrifugal bias vector for the robot at configuration q and velocity qd + } + + // Computes the gravity torque for a joint based on the current state and robot configuration + template + mathlib::VecX_T RobotDynamics::computeGravityTorque( + const RobotConstModel& robot, + const std::vector>& T_world, + const std::vector>& jointWorldPoses + ) const { + const size_t n = robot.joints.size(); + mathlib::VecX_T tau_G = mathlib::VecX_T::Zero(n); + Scalar g{ _gravity }; // [m/s^2], gravity acceleration magnitude + + // For each joint, sum the gravity contributions from all links + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j = robot.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + + Scalar tau_g_i = Scalar(0); // [Nm], gravity torque contribution for joint i + + const mathlib::Pose_T& T_joint = jointWorldPoses[i]; // pose of joint i in world frame + + const mathlib::Mat3_T R_i = T_joint.template block<3, 3>(0, 0); + const mathlib::Vec3_T p_i = T_joint.template block<3, 1>(0, 3); + const mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_i * robot.joints[i].axis); + + // For each link, compute the gravitational force and its torque contribution about joint i + for (size_t k = 0; k < robot.links.size(); ++k) { + const RobotLink& link = robot.links[k]; + const Scalar m = (Scalar)link.inertial.mass; + if (m <= Scalar(0)) { continue; } + + if (!robot.jointAffectsLink(i, k)) { continue; } + + // Link's center of mass in world frame + const mathlib::Mat3_T R_k = T_world[k].template block<3, 3>(0, 0); + const mathlib::Vec3_T com_world = R_k * link.inertial.com_xyz + T_world[k].template block<3, 1>(0, 3); + + // Gravitational force on the link + mathlib::Vec3_T g_world; + g_world = mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame + const mathlib::Vec3_T F_g = m * g_world; // [N], gravitational force on the link in world frame + const mathlib::Vec3_T r = com_world - p_i; // [m] + + // Torque = r × F_g projected onto joint axis + tau_g_i += axis_world.dot(r.cross(F_g)); + } + tau_G[i] = tau_g_i; + } + return tau_G; // [Nm], gravity torques for each joint + } + + // Computes the analytical Jacobian matrix J(q) for the robot based on the current state and robot configuration + template + void RobotDynamics::analyticalJacobian( + const RobotConstModel& robot, + const mathlib::VecX_T& x, + mathlib::MatX_T& J_out, + DenseDynamicsScratch& scratch + ) { + const size_t n = robot.joints.size(); + + Eigen::Map> q_local(x.data(), n); + Eigen::Map> qd_local(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(robot, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); + computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); + + J_out.setZero(2 * n, 2 * n); // [rad/rad] position part, [rad/s / rad/s] velocity part + J_out.block(0, n, n, n).setIdentity(); + + mathlib::MatX_T dTau_dq = mathlib::MatX::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const RobotJoint& joint = robot.joints[i]; + if (joint.type == eJointType::FIXED) { continue; } + + const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency + const Scalar z = static_cast(joint.zeta_target); // damping ratio + const Scalar eps = static_cast(1e-6); + + const Scalar I_eff = mathlib::LSE_smoothMax(scratch.M(i, i), eps); + + const Scalar k_p = I_eff * wn * wn; + const Scalar k_d = Scalar(2) * z * I_eff * wn; + + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + dTau_dq(i, i) = -k_p; + + Scalar qd_i = qd_local[i]; + Scalar tanh_term = mathlib::tanh(qd_i / eps_f); + Scalar stiff_friction_slope = c * (Scalar(1) - tanh_term * tanh_term) / eps_f; + dTau_dv(i, i) = -k_d - b + stiff_friction_slope; + } + + auto solver = scratch.M.ldlt(); + mathlib::MatX_T da_dq = solver.solve(dTau_dq); + mathlib::MatX_T da_dv = solver.solve(dTau_dv); + + J_out.block(n, 0, n, n) = da_dq; + J_out.block(n, n, n, n) = da_dv; + } + + // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration + template + mathlib::VecX_T RobotDynamics::derivative_dense( + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = snap.model->joints.size(); + mathlib::VecX_T dx(2 * n); + + // Map the input state vector to joint angles and velocities + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + + computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q + scratch.dense.h = mathlib::VecX_T::Zero(n); // Temp test to isolate potential issues + //scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector + + if (!scratch.dense.M.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } + + scratch.g.setZero(); + if (snap.torqueMode != eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const RobotJoint& joint = snap.model->joints[i]; + + // Fixed joints + if (joint.type == eJointType::FIXED) { + out.metrics.q[i] = q[i]; + out.metrics.qd[i] = qd[i]; + out.metrics.qdd[i] = 0.0; + continue; + } + + const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency + const Scalar z = static_cast(joint.zeta_target); // damping ratio + + const Scalar err = snap.q_ref[i] - q[i]; // [rad], position error + const Scalar err_d = snap.qd_ref[i] - qd[i]; // [rad/s], velocity error + + const Scalar eps = static_cast(1e-6); + + const Scalar I_eff = mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps); // [kg*m^2], effective inertia for joint i with floor to prevent singularities + const Scalar k_p = I_eff * wn * wn; // [Nm/rad], proportional gain + const Scalar k_d = Scalar(2.0) * z * I_eff * wn; // [Nm/(rad/s)], derivative gain + + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i + tau_i += scratch.g[i]; // Gravity compensation + tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias + // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation + // tau_i += tau_f; + + scratch.dense.tau[i] = tau_i; + + out.metrics.q[i] = mathlib::real(q[i]); + out.metrics.qd[i] = mathlib::real(qd[i]); + + out.metrics.err[i] = mathlib::real(err); + out.metrics.errd[i] = mathlib::real(err_d); + + out.metrics.I_eff[i] = mathlib::real(I_eff); + out.metrics.tau[i] = mathlib::real(tau_i); + } + + // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g + + // Solve for Accelerations + Eigen::LDLT> solver(scratch.dense.M); + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M"); + } + + out.qdd = solver.solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics + + if (!out.qdd.allFinite()) { + LOG_ERROR("Non-finite joint accelerations computed. Check for singularities or numerical issues in the mass matrix."); + throw std::runtime_error("Non-finite joint accelerations computed from dynamics"); + } + + out.metrics.qdd = out.qdd; + + // Fill derivatives + dx.head(n) = qd; + dx.tail(n) = out.qdd; + + return dx; + } + + template + mathlib::VecX_T RobotDynamics::derivative_spatial( + const robots::SpatialModel& model, + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = model.joints.size(); + mathlib::VecX_T dx(2 * n); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + // Quick guard: check for non-finite states and bail with zero derivative + for (size_t i = 0; i < n; ++i) { + double q_r = mathlib::real(q[i]); + double qd_r = mathlib::real(qd[i]); + if (!std::isfinite(q_r) || !std::isfinite(qd_r)) { + LOG_ERROR("Non-finite state detected in derivative_spatial: q[%zu]=%g qd[%zu]=%g", i, q_r, i, qd_r); + // Return zero derivative to avoid propagating NaNs + dx.setZero(); + out.qdd.setZero(); + return dx; + } + } + + SpatialDynamics::computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(n); + mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(n); + mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, qd_zero, qdd_zero, scratch); + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& joint = model.joints[i]; + if (!isControlledJoint(joint.type)) { + scratch.dense.tau[i] = Scalar(0); + continue; + } + + const Scalar wn = static_cast(snap.model->joints[i].wn_target); + const Scalar z = static_cast(snap.model->joints[i].zeta_target); + + const Scalar err = snap.q_ref[i] - q[i]; + const Scalar err_d = snap.qd_ref[i] - qd[i]; + + const Scalar eps = static_cast(1e-6); + + const Scalar I_eff = mathlib::LSE_smoothMax(M(i, i), eps); + const Scalar k_p = I_eff * wn * wn; + const Scalar k_d = Scalar(2) * z * I_eff * wn; + + const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-3); + + Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; + Scalar tau_f = c * mathlib::tanh(qd[i] / Scalar(0.1)) + b * qd[i]; // simple friction model with viscous and Coulomb friction + tau_i += tau_f; + + const Scalar Q_max = static_cast(snap.model->joints[i].limits.maxEffort); + LOG_INFO_ONCE("Max effort for joint %zu: %g Nm", i, mathlib::real(Q_max)); + + tau_i = Q_max * mathlib::tanh(tau_i / Q_max); // saturate control torque to max effort using smooth tanh saturation + + scratch.dense.tau[i] = tau_i; + + out.metrics.q[i] = mathlib::real(q[i]); + out.metrics.qd[i] = mathlib::real(qd[i]); + + out.metrics.err[i] = mathlib::real(err); + out.metrics.errd[i] = mathlib::real(err_d); + + out.metrics.I_eff[i] = mathlib::real(I_eff); + out.metrics.tau[i] = mathlib::real(tau_i); + } + + out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); + out.metrics.qdd = out.qdd; + dx.head(n) = qd; + dx.tail(n) = out.qdd; + return dx; + } + + template + void RobotDynamics::jacobian_spatial( + const robots::SpatialModel& model, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + + F_out.setZero(2 * n, 2 * n); + F_out.block(0, n, n, n).setIdentity(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + SpatialDynamics::computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + scratch.dense.M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + + mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& joint = model.joints[i]; + if (!isControlledJoint(joint.type)) { continue; } + dTau_dq(i, i) = -kp[i]; + const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + dTau_dv(i, i) = -kd[i] - b; + } + + Eigen::LDLT> solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving + + mathlib::MatX_T dqdd_dtau_q = solver.solve(dTau_dq); // compute the partial derivative of qdd with respect to q + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); + } + + mathlib::MatX_T dqdd_dtau_v = solver.solve(dTau_dv); // compute the partial derivative of qdd with respect to qd + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); + } + + F_out.block(n, 0, n, n) = dqdd_dtau_q; // fill the Jacobian block for qdd with respect to q + F_out.block(n, n, n, n) = dqdd_dtau_v; // fill the Jacobian block for qdd with respect to qd) + } + + // Computes the derivative of the state vector with control gains based on the current state and robot configurations + template + mathlib::VecX_T RobotDynamics::derivative_with_gains( + Scalar t, + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = snap.model->joints.size(); + mathlib::VecX_T dx(2 * n); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + + computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); + scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); + + scratch.g.setZero(); + if (snap.torqueMode != eTorqueMode::NONE) { + scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); + } + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const RobotJoint& joint = snap.model->joints[i]; + if (joint.type == eJointType::FIXED) continue; + + const Scalar eps = static_cast < Scalar>(1e-6); + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; + tau_i += scratch.g[i] + scratch.dense.h[i]; + tau_i -= b * qd[i]; + tau_i -= c * mathlib::tanh(qd[i] / eps_f); + + scratch.dense.tau[i] = tau_i; + } + + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; + out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); + out.metrics.qdd = out.qdd; + + dx.head(n) = qd; + dx.tail(n) = out.qdd; + return dx; + } + + // Computes the Jacobian matrix with control gains based on the current state and robot configuration + template + void RobotDynamics::jacobian_with_gains( + const mathlib::VecX_T& x, + const RobotSimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DenseDynamicsScratch& scratch + ) { + const size_t n = snap.model->joints.size(); + + F_out.setZero(2 * n, 2 * n); + F_out.block(0, n, n, n).setIdentity(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + // Compute a local mass matrix for this exact stage evaluation frame + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); + computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); + + mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const RobotJoint& joint = snap.model->joints[i]; + if (joint.type == eJointType::FIXED) continue; + + dTau_dq(i, i) = -kp[i]; + + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + Scalar tanh_term = mathlib::tanh(qd[i] / eps_f); + Scalar stiff_friction = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; + dTau_dv(i, i) = -kd[i] - b + stiff_friction; + } + + auto solver = scratch.M.ldlt(); + F_out.block(n, 0, n, n) = solver.solve(dTau_dq); + F_out.block(n, n, n, n) = solver.solve(dTau_dv); + } +} // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h index 515aae6f..581fe529 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h @@ -2,16 +2,16 @@ #pragma once #include "EngineCore.h" -#include "MathLibAPI.h" -#include "core/Types.h" +#include +#include +#include "Robots/RobotSimSnapshot.h" + +#include "EngineLib/LogMacros.h" namespace robots { // Forward declarations - struct RobotConstModel; - struct RobotSimSnapshot; struct RobotLink; struct RobotJoint; - struct RobotMetrics; // Kinematics class responsible for computing forward kinematics and related transformations class DSFE_API RobotKinematics { @@ -20,25 +20,31 @@ namespace robots { RobotKinematics(); // Computes the forward kinematics for the robot based on the current state and robot configuration + template void computeForwardKinematics_fromState( const RobotConstModel& robot, - const mathlib::VecX& x, - std::vector& T_world_out + const mathlib::VecX_T& x, + std::vector>& T_world_out ) const; // Computes the joint world poses for all joints based on the current state and robot configuration - std::vector calcJointWorldPoses( - const std::vector& T_world, + template + std::vector> calcJointWorldPoses( + const std::vector>& T_world, const RobotConstModel& robot ); // Computes the forward kinematics for a single joint motion based on the joint axis and angle - mathlib::Pose jointMotionTransform( - const mathlib::Vec3& axis_joint, - double q + template + mathlib::Pose_T jointMotionTransform( + const mathlib::Vec3_T& axis_joint, + Scalar q ) const; // Converts roll-pitch-yaw angles (in radians) to a quaternion representation - mathlib::Quat rpyRadToQuat(const mathlib::Vec3& rpyRad); + template + mathlib::Quat_T rpyRadToQuat(const mathlib::Vec3_T& rpyRad); }; -} \ No newline at end of file +} + +#include "Robots/RobotKinematics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl new file mode 100644 index 00000000..84ed0d79 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl @@ -0,0 +1,105 @@ +// DSFE_Core RobotKinematics.inl +#pragma once + +namespace robots { + // Computes the forward kinematics for the robot based on the current state and robot configuration + template + void RobotKinematics::computeForwardKinematics_fromState( + const RobotConstModel& robot, + const mathlib::VecX_T& x, + std::vector>& T_world_out + ) const { + const auto& joints = robot.joints; + const auto& links = robot.links; + const size_t n = joints.size(); + + T_world_out.resize(robot.links.size()); + + if (T_world_out.empty()) { + LOG_ERROR("T_world_out is empty"); + return; + } + + mathlib::Pose_T T = mathlib::Pose_T::Identity(); // world -> base + T_world_out[0] = T; // base link + + // Compute the transform to the next link using each joint + for (size_t i = 0; i < n; ++i) { + const auto& joint = joints[i]; + const Scalar q = x[i]; // joint angle from state vector + + mathlib::Pose_T T_origin = mathlib::Pose_T::Identity(); // transform from parent link to joint frame (fixed) + mathlib::Quat_T q_origin = joint.origin_q.template cast(); // convert quaternion to correct scalar type + + T_origin.template block<3, 3>(0, 0) = q_origin.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON + T_origin.template block<3, 1>(0, 3) = joint.origin_xyz.template cast(); // translation from parent link to joint frame + + // Compute joint motion transform based on joint axis and angle + Pose_T T_motion = mathlib::Pose_T::Identity(); + if (joint.type == eJointType::REVOLUTE) { + T_motion = jointMotionTransform(joint.axis.template cast(), q); // rotation about joint axis + } + else if (joint.type == eJointType::PRISMATIC) { + T_motion.template block<3, 1>(0, 3) = mathlib::safeNormalised(joint.axis) * q; // translation along joint axis + } + + // compose transforms + T = T * T_origin * T_motion; // parent -> joint -> motion -> child + + int childIdx = robot.linkIndex(joint.child); + if (childIdx < 0 || childIdx >= T_world_out.size()) { + LOG_ERROR("Invalid child link index for joint {}: {}", joint.name.c_str(), childIdx); + continue; + } + + T_world_out[childIdx] = T; // world -> child link + } + } + + // Computes the joint world poses for all joints based on the current state and robot configuration + template + std::vector> RobotKinematics::calcJointWorldPoses( + const std::vector>& T_world, + const RobotConstModel& robot + ) { + std::vector> jointWorldPoses(robot.joints.size()); + + for (size_t i = 0; i < robot.joints.size(); ++i) { + const RobotJoint& joints = robot.joints[i]; + int childIdx = robot.linkIndex(joints.child); + + if (childIdx < 0 || childIdx >= T_world.size()) { + LOG_ERROR("Invalid child link index for joint {}: {}", joints.name.c_str(), childIdx); + continue; + } + + jointWorldPoses[i] = T_world[childIdx]; + } + return jointWorldPoses; + } + + // Computes the forward kinematics for a single joint motion based on the joint axis and angle + template + mathlib::Pose_T RobotKinematics::jointMotionTransform( + const mathlib::Vec3_T& axis_joint, + Scalar q + ) const { + mathlib::Pose_T T = mathlib::Pose_T::Identity(); // homogeneous transformation matrix (4x4) + T.template block<3, 3>(0, 0) = mathlib::AngleAxis(q, mathlib::safeNormalised(axis_joint)); // set upper-left 3x3 block to rotation matrix + return T; // (4x4) homogeneous transformation + } + + // Converts roll-pitch-yaw angles (in radians) to a quaternion representation + template + mathlib::Quat_T RobotKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { + const Scalar roll = rpyRad.x(); + const Scalar pitch = rpyRad.y(); + const Scalar yaw = rpyRad.z(); + + const Quat_T qx(Eigen::AngleAxis(roll, mathlib::Vec3_T(Scalar(1), Scalar(0), Scalar(0)))); + const Quat_T qy(Eigen::AngleAxis(pitch, mathlib::Vec3_T(Scalar(0), Scalar(1), Scalar(0)))); + const Quat_T qz(Eigen::AngleAxis(yaw, mathlib::Vec3_T(Scalar(0), Scalar(0), Scalar(1)))); + + return (qz * qy * qx).normalized(); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h b/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h index d412d704..0e1d1608 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h @@ -2,33 +2,33 @@ #pragma once #include "EngineCore.h" -#include -#include + namespace robots { // Per-joint metrics + template struct RobotMetrics { // State - mathlib::VecX q; - mathlib::VecX qd; - mathlib::VecX qdd; + mathlib::VecX_T q; + mathlib::VecX_T qd; + mathlib::VecX_T qdd; - mathlib::VecX err; - mathlib::VecX errd; + mathlib::VecX_T err; + mathlib::VecX_T errd; // Dynamics - mathlib::VecX I_eff; - mathlib::VecX tau; + mathlib::VecX_T I_eff; + mathlib::VecX_T tau; // Constraints / realism - mathlib::VecX tau_barrier; - mathlib::VecX tau_sat; + mathlib::VecX_T tau_barrier; + mathlib::VecX_T tau_sat; // Energy, Work, & Power - mathlib::VecX KE; - mathlib::VecX PE; - mathlib::VecX E_total; - mathlib::VecX W_actuator; + mathlib::VecX_T KE; + mathlib::VecX_T PE; + mathlib::VecX_T E_total; + mathlib::VecX_T W_actuator; // Stability flags std::vector sat_flag; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h index ce600fcd..78b54b3d 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h @@ -1,17 +1,14 @@ // DSFE_Core RobotModel.h #pragma once - #include "EngineCore.h" -#include -#include -#include -#include +#include +#include +#include +#include #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" -constexpr double DEG2RAD = std::numbers::pi / 180.0; - namespace robots { // --- Robot Model Kinematic Models --- enum class eKinematicsModel { @@ -46,7 +43,7 @@ namespace robots { // Inertial properties of a link struct Inertial { - double mass = 0.0f; + double mass = 0.0; mathlib::Vec3 com_xyz{ 0.0,0.0,0.0 }; Inertia inertia{}; }; @@ -104,7 +101,7 @@ namespace robots { bool continuous = false; double minAngle = 0.0; double maxAngle = 0.0; - double maxqd = 180.0 * DEG2RAD; + double maxqd = PI_d; double maxEffort = 0.0; // max torque/force // Soft limits double omegaRefMaxRad_s = 0.0; @@ -154,8 +151,8 @@ namespace robots { double qdd_ref = 0.0; // rad/s^2 // --- Control Parameters --- - double wn_target = 5.0; // rad/s - double zeta_target = 0.7; // damping ratio + double wn_target = 0.0; // rad/s + double zeta_target = 0.0; // damping ratio // --- Precomputed transforms --- mathlib::Mat4 jointToChildRest = mathlib::Mat4::Identity(); @@ -178,7 +175,7 @@ namespace robots { // Kinematics model (URDF or DH) eKinematicsModel kinematicsModel = eKinematicsModel::URDF; - std::vector dhParams; + std::vector> dhParams; // Visualization options eVisualFrame visualFrame = eVisualFrame::JOINT; diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h b/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h index 75417d43..bb176b7f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h @@ -3,6 +3,7 @@ #include "EngineCore.h" #include "Robots/RobotModel.h" +#include namespace robots { // Immutable robot data needed by solver threads @@ -22,24 +23,59 @@ namespace robots { }; // Runtime snapshot for one integration/derivative step - struct DSFE_API RobotSimSnapshot { + template + struct RobotSimSnapshot_T { + const RobotConstModel* model = nullptr; - mathlib::VecX q; // joint angles - mathlib::VecX qd; // joint velocities + mathlib::VecX_T q; // joint angles + mathlib::VecX_T qd; // joint velocities - mathlib::VecX q_ref; // reference joint angles - mathlib::VecX qd_ref; // reference joint velocities - mathlib::VecX qdd_ref; // reference joint accelerations + mathlib::VecX_T q_ref; // reference joint angles + mathlib::VecX_T qd_ref; // reference joint velocities + mathlib::VecX_T qdd_ref; // reference joint accelerations - mathlib::Mat4 robotRootPose = mathlib::Mat4::Identity(); + mathlib::Mat4_T robotRootPose = mathlib::Mat4_T::Identity(); bool baseIsFree = false; - double lastBaseForwardForce = 0.0; - double gravity = 0.0; + + Scalar lastBaseForwardForce = Scalar(0); + Scalar gravity = Scalar(0); eTorqueMode torqueMode = eTorqueMode::CONTROLLED; - double dt = 0.0; - double simTime = 0.0; // simulation time in seconds + + Scalar dt = Scalar(0); + Scalar simTime = Scalar(0); }; + using RobotSimSnapshot = RobotSimSnapshot_T; + + template + inline RobotSimSnapshot_T castSnapshot( + const RobotSimSnapshot_T& src + ) { + RobotSimSnapshot_T dst; + + dst.model = src.model; + + dst.q = src.q.template cast(); + dst.qd = src.qd.template cast(); + + dst.q_ref = src.q_ref.template cast(); + dst.qd_ref = src.qd_ref.template cast(); + dst.qdd_ref = src.qdd_ref.template cast(); + + dst.robotRootPose = src.robotRootPose.template cast(); + + dst.baseIsFree = src.baseIsFree; + + dst.lastBaseForwardForce = ToScalar(src.lastBaseForwardForce); + dst.gravity = ToScalar(src.gravity); + + dst.torqueMode = src.torqueMode; + + dst.dt = ToScalar(src.dt); + dst.simTime = ToScalar(src.simTime); + + return dst; + } } // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h index a1364835..53eee126 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h @@ -8,6 +8,11 @@ #include "Robots/RobotSimSnapshot.h" #include "Robots/DynamicsTypes.h" +#include +#include "Robots/RobotKinematics.h" +#include "Robots/RobotDynamics.h" +#include "Robots/SpatialDynamics.h" + #include "Analysis/MetricLogger.h" #include "Numerics/IntegrationService.h" @@ -16,8 +21,6 @@ namespace control { class TrajectoryManager; } namespace robots { // Forward declarations - class RobotKinematics; - class RobotDynamics; enum class eTorqueMode; // Joint state structure @@ -32,6 +35,17 @@ namespace robots { Baseline }; + // Step Result struct + template + struct RobotStepResult_T { + integration::StepOut_T stepOut; + RobotSimSnapshot_T snap; + DynamicsResult dynamics; + mathlib::VecX_T tau_rnea; + }; + + inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) + class DSFE_API RobotSystem { public: RobotSystem(); @@ -40,6 +54,8 @@ namespace robots { // --- Utility Methods --- static double clampJointAngle(const RobotJoint& joint, double angleRad); + template + static T clampJointAngle_T(const RobotJoint& joint, T angleRad); // ---- Accessors --- @@ -61,13 +77,11 @@ namespace robots { bool hasRobot() const { return _hasRobot; } void setGravity(double g); - double getGravity() const { return _gravity; } + const double getGravity() const { return _gravity; } void setNaturalFrequency(double wn) { _wn = wn; } double getNaturalFrequency() const { return _wn; } - void resetNaturalFrequencyToTarget() { - for (auto& joint : _robot.joints) { joint.wn_target = _wn; } - } + void resetNaturalFrequencyToTarget() { for (auto& joint : _robot.joints) { joint.wn_target = _wn; } } void setDampingRatio(double zeta) { _zeta = zeta; } double getDampingRatio() const { return _zeta; } @@ -112,7 +126,10 @@ namespace robots { // --- SIMULATION STEP METHOD --- - RobotSimSnapshot takeSnapshot(double simTime) const; + template + RobotSimSnapshot_T takeSnapshot(T simTime) const; + template + void step_AD(double dt, double simTime); void step(double dt, double simTime); void updateTrajectoryInputs(control::TrajectoryManager& traj, double t); @@ -131,18 +148,30 @@ namespace robots { void setRobotRootHome(const mathlib::Vec3& pos, const mathlib::Quat& rot); bool setDefaultPoseDeg(); - void setCurrentJointIndex(int index) { _currentJointIndex = index; } // --- GET AND SET INTEGRATION METHOD --- integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } - void setIntegrationMethod(integration::eIntegrationMethod method) { _curIntMethod = method; } std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } + void setStandardIntegrator(integration::eIntegrationMethod m) { _curIntMethod = m; } + + integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } + std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } + void setADIntegrator(integration::eAutoDiffIntegrationMethod m) { _curIntMethod_AD = m; } integration::IntegrationService* getIntegrator(); const integration::IntegrationService* getIntegrator() const; + integration::DifferentiableIntegrator* getADIntegrator(); + const integration::DifferentiableIntegrator* getADIntegrator() const; + + bool autoDiffEnabled() const { return _useAutoDiff; } + void enableAutoDiff(bool enable) { _useAutoDiff = enable; } + + std::shared_ptr runtimeIntegratorState(); + std::shared_ptr runtimeIntegratorState() const; + void setRefBuffer(robots::TrajRefBuffer* buf) { _refBuffer = buf; } void setLogBuffer(robots::JointLogBuffer* buf) { _logBuffer = buf; } void setRole(eRole role) { _role = role; } @@ -164,17 +193,32 @@ namespace robots { void buildLinkIndex(); void buildSpatialModel(); + template + RobotStepResult_T step_impl( + const mathlib::VecX_T& x, + Scalar dt, Scalar t, IntegratorT& integrator, + DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + ); + + template + void postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& scratch, const RobotStepResult_T& result); + std::unique_ptr _kinematics; std::unique_ptr _dynamics; std::unique_ptr _integrator; integration::eIntegrationMethod _curIntMethod{}; + std::unique_ptr _AD_integrator; + integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + eRole _role = eRole::Simulation; double _wn = 0.0; // configurable natural frequency for PD control (rad/s) double _zeta = 0.0; // configurable damping ratio for PD control (unitless) + bool _useAutoDiff = false; + // Compute the forward drive (velocity) of the robot's root link based on the current state and robot configuration double computeForwardDrive() const; // Integrate the floating base translation based on the current state and robot configuration @@ -186,6 +230,13 @@ namespace robots { mathlib::VecX packState() const; void unpackState(const mathlib::VecX& x); + template + void unpackState(const mathlib::VecX_T& x); + + // State packing and unpacking using a DualNumber vector. + mathlib::VecX_T> packState_AD() const; + void unpackState_AD(const mathlib::VecX_T>& x); + // Reference state packing and unpacking mathlib::VecX packRefState() const; void unpackRefState(const mathlib::VecX& xr); @@ -200,11 +251,14 @@ namespace robots { RobotModel _robot; eTorqueMode _torqueMode = _robot.torqueMode; - SpatialModel _spatialModel; + SpatialModel _spatialModel; RobotConstModel _constModel; - DynamicsScratch _dynScratch; - DynamicsResult _dynResult; + DynamicsScratch _dynScratch; + DynamicsResult _dynResult; + + DynamicsScratch> _dynScratch_AD; + DynamicsResult> _dynResult_AD; // World to robot base transform (meters) std::vector _worldTransforms; @@ -251,9 +305,9 @@ namespace robots { double _baseYawAcc = 0.0; // Tunables - double _baseMass = 62.0; // kg (H1 ~60–65) - double _baseLinearDamping = 6.0; // Ns/m - double _baseYawDamping = 2.0; // Nms/rad + double _baseMass = 62.0; // kg (H1 ~60–65) + double _baseLinearDamping = 6.0; // Ns/m + double _baseYawDamping = 2.0; // Nms/rad double _lastBaseForwardForce = 0.0; // Double-buffer design @@ -268,29 +322,4 @@ namespace robots { }; } // namespace robot - -// --- Logging macros for robot syste debugging --- - -// LOG_ROT -#ifdef LOG_ROT -#error LOG_ROT macro already defined. Please undefine it before including RobotSystem.h to avoid conflicts. -#endif -// Logs the rotation part of a 4x4 matrix with a custom tag -#define LOG_ROT(tag, M) \ - LOG_INFO("[ROT] %s | X=(%.2f %.2f %.2f) Y=(%.2f %.2f %.2f) Z=(%.2f %.2f %.2f)", \ - tag, \ - M[0][0], M[0][1], M[0][2], \ - M[1][0], M[1][1], M[1][2], \ - M[2][0], M[2][1], M[2][2]) - -#define LOG_MAT4(tag, M) \ - LOG_INFO("[MAT4] %s:\n" \ - "[ % .3f % .3f % .3f % .3f ]\n" \ - "[ % .3f % .3f % .3f % .3f ]\n" \ - "[ % .3f % .3f % .3f % .3f ]\n" \ - "[ % .3f % .3f % .3f % .3f ]", \ - tag, \ - M[0][0], M[1][0], M[2][0], M[3][0], \ - M[0][1], M[1][1], M[2][1], M[3][1], \ - M[0][2], M[1][2], M[2][2], M[3][2], \ - M[0][3], M[1][3], M[2][3], M[3][3]) \ No newline at end of file +#include "RobotSystemStep.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl new file mode 100644 index 00000000..55f85b73 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl @@ -0,0 +1,252 @@ +// DSFE_Core RobotSystemStep.inl +#pragma once + +namespace robots { + template + T RobotSystem::clampJointAngle_T(const RobotJoint& joint, T angleRad) { + if (joint.limits.continuous) { return mathlib::wrapRad(angleRad); } + else { return std::clamp(angleRad, T(joint.limits.minAngle), T(joint.limits.maxAngle)); } + } + + // Method to take a snapshot of the current robot state + template + RobotSimSnapshot_T RobotSystem::takeSnapshot(T simTime) const { + RobotSimSnapshot_T snap; + snap.model = &_constModel; + const size_t n = (size_t)_robot.joints.size(); + + snap.q.resize(n); + snap.qd.resize(n); + + snap.q_ref.resize(n); + snap.qd_ref.resize(n); + snap.qdd_ref.resize(n); + + for (size_t i = 0; i < n; ++i) { + const auto& j = _robot.joints[i]; + + snap.q[i] = j.q; + snap.qd[i] = j.qd; + + snap.q_ref[i] = j.q_ref; + snap.qd_ref[i] = j.qd_ref; + snap.qdd_ref[i] = j.qdd_ref; + } + + snap.robotRootPose = _robotRootPose.template cast(); + snap.baseIsFree = _baseIsFree; + snap.lastBaseForwardForce = T(_lastBaseForwardForce); + snap.gravity = T(_gravity); + + snap.torqueMode = _robot.torqueMode; + + snap.dt = T(_dynamics->dt()); + snap.simTime = simTime; + + return snap; + } + + template + RobotStepResult_T RobotSystem::step_impl( + const mathlib::VecX_T& x, + Scalar dt, Scalar t, IntegratorT& integrator, + DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + ) { + RobotStepResult_T result; + result.snap = takeSnapshot(t); + auto& snap = result.snap; + const size_t n = snap.model->joints.size(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + mathlib::VecX_T qdd(n); + for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } + + std::vector> T_start(snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); + std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); + + SpatialModel spatialModel = _spatialModel.template cast(); + auto& dynScratch = dynamicScratch; + auto& dynResult = dynamicResult; + + SpatialDynamics::computeSpatialKinematicsAndBias( + spatialModel, + q, qd, + dynScratch.spatial.Xup, + dynScratch.spatial.v, dynScratch.spatial.c + ); + + // CRBA only for controller inertia scaling + mathlib::MatX_T M_start = SpatialDynamics::CRBA( + spatialModel, + dynScratch.spatial.Xup, + dynScratch + ); + + // Cache frozen joint gains for this step + mathlib::VecX_T kp_frozen(n), kd_frozen(n); + for (size_t i = 0; i < n; ++i) { + const auto& joint = snap.model->joints[i]; + //LOG_INFO("Snap model joint name = %s", joint.name.c_str()); + if (joint.type == eJointType::FIXED) { continue; } + + dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); + const Scalar I_eff = dynScratch.dense.I_eff_controller[i]; + + kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; + kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; + } + + // Compute RNEA torques for feedforward control + mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( + spatialModel, + q, qd, qdd, + dynScratch + ); // [Nm] + result.tau_rnea = tau_rnea; + //LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); + + // Define the derivative function for integration, capturing necessary variables by reference + auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { + return _dynamics->derivative_spatial( + spatialModel, + t, xIn, + snap, + dynScratch, dynResult + ); + }; + // Define the Jacobian function for integration, capturing necessary variables by reference + auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { + _dynamics->jacobian_spatial( + spatialModel, + xIn, snap, + kp_frozen, kd_frozen, + J_out, dynScratch + ); + }; + + if (!x.allFinite()) { LOG_ERROR("[step_impl] input state already non-finite"); } + + if constexpr (std::is_same_v, integration::IntegrationService>) { + mathlib::VecX x_real = x.template cast(); + auto step = integrator.step(_curIntMethod, x_real, static_cast(t), static_cast(dt), f_deriv, f_J); + result.stepOut.x_next = step.x_next.template cast(); + result.stepOut.dt_taken = step.dt_taken; + result.stepOut.dt_sug = step.dt_sug; + } + else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { + result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); + } + + for (int i = 0; i < result.stepOut.x_next.size(); ++i) { + const auto v = mathlib::real(result.stepOut.x_next[i]); + if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } // TODO add Scalar isnan and isinf checks to mathlib and use those instead (need to handle both float and double cases) + } + + result.dynamics = dynResult; + return result; + } + + template + void RobotSystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const RobotStepResult_T& result) { + const size_t n = result.snap.model->joints.size(); + + Eigen::Map q_next(x.data(), n); + Eigen::Map qd_next(x.data() + n, n); + + // Enforce joint limits + /*for (auto& j : _robot.joints) { enforceJointLimits(j); }*/ + + // Recompute kinematics and dynamics at the new state for logging and control purposes + std::vector T_world(result.snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*result.snap.model, x, T_world); + + // Alternative would be just + + // Compute mass matrix at the new state + mathlib::MatX M = dynScratch.dense.M.unaryExpr([](const auto& v) { return mathlib::real(v); }); + + // Extract real parts of relevant variables for logging and control + mathlib::VecX q_real = result.snap.q.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX qd_real = result.snap.qd.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX q_ref_real = result.snap.q_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX qd_ref_real = result.snap.qd_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX tau_rnea_real = result.tau_rnea.unaryExpr([](const auto& v) { return mathlib::real(v); }); + + // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd + double sys_KE = 0.5 * qd_real.transpose() * M * qd_real; // [J], kinetic energy of the robot at configuration q and velocity qd + + // Compute system potential energy at configuration q (relative to gravity) + double sys_PE = 0.0; + double g = _dynamics->getGravity(); + + for (size_t k = 0; k < _robot.links.size(); ++k) { + const RobotLink& link = _robot.links[k]; + const double m = link.inertial.mass; + if (m <= 0.0) { continue; } + Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); + sys_PE += m * g * com_world.z(); + } + + const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system + + // Log metrics to buffer if logging is enabled + robots::JointLogBuffer* buf = nullptr; + if (_useInternalLogging) { int idx = _activeLogBufIdx.load(std::memory_order_acquire); buf = &_logBuffers[idx]; } + else { buf = _logBuffer; } + + if (buf) { + auto dynResult = result.dynamics; + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j = _robot.joints[i]; + + const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); + const double err = q_ref_real[i] - q_real[i]; + const double err_d = qd_ref_real[i] - qd_real[i]; + JointLogBuffer::JointLogEntry e{}; + + e.sim_time = _simTime; + e.dt_taken = mathlib::real(result.stepOut.dt_taken); + e.dt_sug = mathlib::real(result.stepOut.dt_sug); + e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = mathlib::real(dynResult.metrics.qdd[i]); + e.err = err; e.err_d = err_d; + e.I_eff = I_eff; + e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = 0.0; + e.tau_sat = mathlib::real(dynResult.metrics.tau_sat[i]); + e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; + e.clamp_theta = mathlib::real(_clampTheta[i]); e.clamp_omega = mathlib::real(_clampOmega[i]); + e.sat_flag = mathlib::real(dynResult.metrics.sat_flag[i]); e.joint_index = (int)i; + buf->push_entry(e); + } + } + } + + template + void RobotSystem::step_AD(double dt, double simTime) { + if (!hasRobot()) { return; } + using Dual = mathlib::DualNumber_T; + _simTime = simTime; + const size_t n = _robot.joints.size(); + mathlib::VecX_T x = packState_AD(); + + assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit + for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } + + auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator, _dynScratch_AD, _dynResult_AD); + unpackState_AD(result.stepOut.x_next); + _dynamics->setDt(result.stepOut.dt_taken); + + auto x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); + postStepUpdate(x_real, _dynScratch_AD, result); + + // Update base pose if free-floating + if (_baseIsFree) { + integrateBaseTranslation(dt); + updateBaseRootPose(); + } + // Update kinematics + computeRobotKinematics(_worldTransforms); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h index f1ee66af..9963d617 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h @@ -4,83 +4,92 @@ #include "EngineCore.h" #include "Robots/SpatialModel.h" #include "Robots/DynamicsTypes.h" -#include namespace robots { - class DSFE_API SpatialDynamics { public: + template static void computeSpatialKinematicsAndBias( - const SpatialModel& model, - const mathlib::VecX& q, const mathlib::VecX& qd, - std::vector& Xup_out, - std::vector& v_out, - std::vector& c_out + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + std::vector>& Xup_out, + std::vector>& v_out, + std::vector>& c_out ); - + + template static void computeAccelerations_RNEA( - const SpatialModel& model, - const mathlib::VecX& qdd, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& g, - std::vector& a_out + const SpatialModel& model, + const mathlib::VecX_T& qdd, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& g, + std::vector>& a_out ); + template static void computeBackwardForces_RNEA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& a, - mathlib::VecX& tau_out + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out ); - static mathlib::VecX RNEA( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& qdd, - DynamicsScratch& scratch + template + static mathlib::VecX_T RNEA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& qdd, + DynamicsScratch& scratch ); - static mathlib::MatX CRBA( - const SpatialModel& model, - const std::vector& Xup, - DynamicsScratch& scratch + template + static mathlib::MatX_T CRBA( + const SpatialModel& model, + const std::vector>& Xup, + DynamicsScratch& scratch ); + template static void computeArticulatedBodies_ABA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& c, - const mathlib::VecX& tau, - std::vector& IA_out, - std::vector& pA_out, - std::vector& Ia_out, - mathlib::VecX& u_out, - mathlib::VecX& d_out, - std::vector& U_out + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& c, + const mathlib::VecX_T& tau, + std::vector>& IA_out, + std::vector>& pA_out, + std::vector>& Ia_out, + mathlib::VecX_T& u_out, + mathlib::VecX_T& d_out, + std::vector>& U_out ); + template static void computeAccelerations_ABA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& u_out, - const mathlib::VecX& d_out, - const std::vector& U, - const SpatialVec& a0, - std::vector& a_out, - mathlib::VecX& qdd_out + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& u_out, + const mathlib::VecX_T& d_out, + const std::vector>& U, + const SpatialVec_T& a0, + std::vector>& a_out, + mathlib::VecX_T& qdd_out ); - static mathlib::VecX ABA( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& tau, - DynamicsScratch& scratch + template + static mathlib::VecX_T ABA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& tau, + DynamicsScratch& scratch ); }; -} \ No newline at end of file +} + +#include "Robots/SpatialDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl new file mode 100644 index 00000000..5382736c --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl @@ -0,0 +1,319 @@ +// DSFE_Core SpatialDynamics.inl +#pragma once + +namespace robots { + template + void SpatialDynamics::computeSpatialKinematicsAndBias( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + std::vector>& Xup_out, + std::vector>& v_out, + std::vector>& c_out + ) { + const size_t n = model.joints.size(); + v_out.resize(n); + Xup_out.resize(n); + c_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + // Joint Transform XJ + mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); + + if (j.type == eJointType::REVOLUTE) { + mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.angular()); + mathlib::Mat3_T R = mathlib::AngleAxis(q[i], axis); + mathlib::Vec3_T r = mathlib::Vec3_T::Zero(); + XJ = mathlib::spatialTransform(R, r); + } + else if (j.type == eJointType::PRISMATIC) { + mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.linear()); + mathlib::Vec3_T r = q[i] * axis; + mathlib::Mat3_T R = mathlib::Mat3_T::Identity(); + XJ = mathlib::spatialTransform(R, r); + } + + Xup_out[i] = XJ * j.Xtree; // Combined Transform + mathlib::SpatialVec_T vJ = j.S * qd[i]; // Joint Velocity + + // Root Link + if (j.parent < 0) { v_out[i] = vJ; } + else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } + + c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term + + using corScalar = typename std::decay_t; + static_assert(std::is_same_v, "c_out scalar type does not match model scalar type"); + } + } + + template + void SpatialDynamics::computeAccelerations_RNEA( + const SpatialModel& model, + const mathlib::VecX_T& qdd, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& g, + std::vector>& a_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + + mathlib::SpatialVec_T a0; // base acceleration (gravity) + a0.v << + g.template segment<3>(0), + g.template segment<3>(3); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T aJ = j.S * qdd[i]; // Joint Acceleration + + // Root Link + if (j.parent < 0) { + a_out[i] = Xup[i] * a0 + aJ + c[i]; + continue; + } + + a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; + } + } + + template + void SpatialDynamics::computeBackwardForces_RNEA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out + ) { + const size_t n = model.joints.size(); + tau_out.resize(n); + + std::vector> f(n); + + // Forward Force Computation + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T I_v = j.inertia * v[i]; + mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); + f[i].v = j.inertia * a[i].v + coriolis.v; + } + + // Backward Recursion Computation + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + tau_out[i] = j.S.dot(f[i]); + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + if (j.parent >= 0) { f[j.parent] += XupT * f[i]; } + } + } + + template + mathlib::VecX_T SpatialDynamics::RNEA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& qdd, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + + // TODO Remove these temp scratches AFTER debugging + std::vector> v(n); + std::vector> Xup(n); + std::vector> c(n); + std::vector> a(n); + mathlib::VecX_T tau; + + // Compute spatial velocities and transforms + computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); + // Compute spatial accelerations + computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); + // Compute inverse dynamics (joint torques) + computeBackwardForces_RNEA(model, Xup, v, a, tau); + + return tau; + } + + template + mathlib::MatX_T SpatialDynamics::CRBA( + const SpatialModel& model, + const std::vector>& Xup, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + scratch.dense.M.setZero(n, n); + std::vector> Ic(n); // spatial inertia for each link + + // Initialise spatial inertia for each link based on the robot model + for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } + + // Upward pass: propagate spatial inertia from child links to parent joints + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + int p = j.parent; + if (p >= 0) { + mathlib::MatX_T XupT = Xup[i].transpose(); + Ic[p] += XupT * Ic[i] * Xup[i]; + } + } + + // Downward pass: compute mass matrix contributions for each joint + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + mathlib::SpatialVec_T F = Ic[i] * j.S; + scratch.dense.M(i, i) = j.S.dot(F); + + int jIdx = (int)i; + while (model.joints[jIdx].parent >= 0) { + int p = model.joints[jIdx].parent; + mathlib::SpatialMat_T XupT = Xup[jIdx].transpose(); // TODO Make Eigen-copatible operator overloads for spatial transforms to avoid the errors from this transpose operation in a matrix multiplication context + F = XupT * F; + scratch.dense.M(i, p) = model.joints[p].S.dot(F); + scratch.dense.M(p, i) = scratch.dense.M(i, p); + jIdx = p; + } + } + return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) + } + + template + void SpatialDynamics::computeArticulatedBodies_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& c, + const mathlib::VecX_T& tau, + std::vector>& IA_out, + std::vector>& pA_out, + std::vector>& Ia_out, + mathlib::VecX_T& u_out, + mathlib::VecX_T& d_out, + std::vector>& U_out + ) { + const size_t n = model.joints.size(); + + // Resize scratch buffers + IA_out.resize(n); + pA_out.resize(n); + Ia_out.resize(n); + U_out.resize(n); + u_out.resize(n); + d_out.resize(n); + + // Upward pass: compute articulated body inertias and bias forces + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + + if (j.type == eJointType::FIXED) { + Ia_out[i] = IA_out[i]; + if (j.parent >= 0) { + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; + pA_out[j.parent] += XupT * pA_out[i]; + } + continue; + } + + U_out[i] = IA_out[i] * j.S; + d_out[i] = dot(j.S, U_out[i]); + if (d_out[i] < Scalar(1e-12)) { + d_out[i] = Scalar(1e-12); + } + + u_out[i] = tau[i] - dot(j.S, pA_out[i]); + Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; + + // pA = pA + Ia * c + U * (u/d) + pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); + + if (j.parent >= 0) { + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; + pA_out[j.parent] += XupT * pA_out[i]; + } + } + } + + template + void SpatialDynamics::computeAccelerations_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& u_out, + const mathlib::VecX_T& d_out, + const std::vector>& U, + const SpatialVec_T& a0, + std::vector>& a_out, + mathlib::VecX_T& qdd_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + qdd_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } + else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } + + if (j.type == eJointType::FIXED) { + qdd_out[i] = Scalar(0); + continue; + } + + qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; + a_out[i] += j.S * qdd_out[i]; + } + } + + template + mathlib::VecX_T SpatialDynamics::ABA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& tau, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + mathlib::VecX_T qdd = mathlib::VecX_T::Zero(n); + + mathlib::SpatialVec_T a0; // base acceleration (gravity) + a0.v << + scratch.g.template segment<3>(0), + scratch.g.template segment<3>(3); + + computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + for (size_t i = 0; i < n; ++i) { + scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia + scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); + } + + // Compute articulated body inertias and bias forces + computeArticulatedBodies_ABA( + model, scratch.spatial.Xup, + scratch.spatial.v, scratch.spatial.c, tau, + scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U + ); + + // Compute joint accelerations using the articulated body algorithm + computeAccelerations_ABA( + model, scratch.spatial.Xup, scratch.spatial.c, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, + a0, scratch.spatial.a, qdd + ); + + return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h index 05af1253..90c1a31c 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h @@ -1,29 +1,33 @@ // DSFE_Core SpatialModel.h #pragma once -#include "EngineCore.h" - #include #include #include "Robots/RobotModel.h" namespace robots { // Spatial joint struct - struct DSFE_API SpatialJoint { + template + struct SpatialJoint { int parent = -1; eJointType type = eJointType::FIXED; - mathlib::SpatialMat Xtree; - mathlib::SpatialMat inertia; - mathlib::SpatialVec S; + mathlib::SpatialMat_T Xtree; + mathlib::SpatialMat_T inertia; + mathlib::SpatialVec_T S; std::string name; }; // Spatial model struct - struct DSFE_API SpatialModel { - std::vector joints; + template + struct SpatialModel { + std::vector> joints; std::unordered_map linkNameToIndex; + + template + SpatialModel cast() const; }; -} \ No newline at end of file +} // namespace robots +#include "SpatialModelCast.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl new file mode 100644 index 00000000..8ceb6c06 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl @@ -0,0 +1,22 @@ +// DSFE_Core SpatialModelCast.inl +#pragma once + +namespace robots { + template + template + SpatialModel SpatialModel::cast() const { + SpatialModel out; + out.joints.resize(joints.size()); + for (size_t i = 0; i < joints.size(); ++i) { + const auto& j = joints[i]; + auto& out_j = out.joints[i]; + out_j.parent = j.parent; + out_j.type = j.type; + out_j.Xtree = j.Xtree.template cast(); + out_j.inertia = j.inertia.template cast(); + out_j.S = j.S.template cast(); + out_j.name = j.name; + } + return out; + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h b/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h index ac3e64b5..03c440fd 100644 --- a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h +++ b/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h @@ -1,54 +1,36 @@ +// DSFE_Core TrajectoryManager.h #pragma once -// File: TrajectoryManager.h -// GitHub: SaltyJoss -// ----- -// Initially templated off a tutorial: -// GitHub: jayanam/jgl_demos/JGL_MeshLoader #include "EngineCore.h" -#include -#include -#include +#include +#include namespace robots { class DSFE_API RobotSystem; } namespace control { - // Trajectory Manager Class class DSFE_API TrajectoryManager { public: TrajectoryManager() = default; ~TrajectoryManager() = default; - - // non-copyable (because unique_ptr) + // non-copyable TrajectoryManager(const TrajectoryManager&) = delete; TrajectoryManager& operator=(const TrajectoryManager&) = delete; - // movable is fine TrajectoryManager(TrajectoryManager&&) noexcept = default; TrajectoryManager& operator=(TrajectoryManager&&) noexcept = default; - // Clear trajectory for a specific robot link void clear(const std::string& link); - // Clears all active trajectories void clearAll(); bool empty() const { return _active.empty(); } - - // - bool tryEval(const std::string& link, double t, control::TrajState& out) const; - // Check if a trajectory is active for a specific robot link + bool tryEval(const std::string& link, double t, control::TrajState& out) const; bool hasActive(const std::string& link) const; - // Set a trajectory for a specific robot link void set(const std::string& link, std::unique_ptr traj); - // Apply active trajectories to the robot at time t void apply(robots::RobotSystem& robot, double t); std::size_t activeCount() const { return _active.size(); } private: -#pragma warning(push) -#pragma warning(disable: 4251) std::unordered_map> _active; -#pragma warning(pop) }; } // namespace control \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index dce60c5e..2da971e7 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -11,9 +11,7 @@ #include "Platform/ISimulationCore.h" #include "Platform/SimulationState.h" - -#include -#include +#include "Numerics/IntegratorState.h" #include "Analysis/Telemetry.h" #include "Platform/DataManager.h" @@ -22,16 +20,16 @@ #include "Platform/Logger.h" // Forward Declarations -namespace integration { enum class eIntegrationMethod; } namespace control { class TrajectoryManager; } namespace robots { class RobotSystem; } +namespace single_body_system { class SingleBodySystem; } namespace interpreter { class IStoredProgram; } namespace core { // configurable defaults (not part of class to allow tuning without recompilation) - constexpr double DEFAULT_INTERACTIVE_MINUTES = 60.0; // long runs for interactive mode - constexpr double DEFAULT_SYNC_MINUTES = 10.0; // short runs for synchronous mode - constexpr size_t MAX_LOG_ENTRIES = 50'000'000; // hard cap to avoid OutOfMemory crashes + inline constexpr double DEFAULT_INTERACTIVE_MINUTES = 60.0; // long runs for interactive mode + inline constexpr double DEFAULT_SYNC_MINUTES = 10.0; // short runs for synchronous mode + inline constexpr size_t MAX_LOG_ENTRIES = 50'000'000; // hard cap to avoid OutOfMemory crashes class DSFE_API SimulationCore : public ISimulationCore { public: @@ -69,16 +67,27 @@ namespace core { // Integrator void setupSimulationIntegrator(); void setIntegrationMethod(integration::eIntegrationMethod method) override; + void setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) override; std::string integrationMethodName() const override; integration::eIntegrationMethod integrationMethod() const override; + integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const override; + void enableAutoDiff(bool enable) override; + void setRunTag(const std::string& tag) override { _runTag = tag; } // Subsystems access robots::RobotSystem* robotSystem() override; const robots::RobotSystem* robotSystem() const; + single_body_system::SingleBodySystem* singleBodySystem() override; + const single_body_system::SingleBodySystem* singleBodySystem() const; control::TrajectoryManager* trajectoryManager() override; const control::TrajectoryManager* trajectoryManager() const; + // Body management + bool hasSingleBody() const override; + void loadSingleBody(const std::string& name) override; + void loadSingleBodyInternal(const std::string& name); // Internal method that assumes ownership + // Robot management bool hasRobot() const override; void loadRobot(const std::string& name) override; @@ -94,6 +103,7 @@ namespace core { // Setters for subsystems and scene objects void setRobotSystem(robots::RobotSystem* robot); + void setSingleBodySystem(single_body_system::SingleBodySystem* singleBody); void setTrajectoryManager(control::TrajectoryManager* traj); void setJointLogBuffer(robots::JointLogBuffer* buffer); void setTrajRefBuffer(robots::TrajRefBuffer* buffer); @@ -134,6 +144,7 @@ namespace core { private: // Export thread management void exportThreadMain(); + void scriptParallelisation(interpreter::IStoredProgram* program); std::thread _expThread; std::mutex _expMutex; @@ -144,11 +155,13 @@ namespace core { // Owning storage (used only in owning mode) // std::unique_ptr>> _objectsOwned; std::unique_ptr _robotOwned; + std::unique_ptr _singleBodyOwned; std::unique_ptr _trajOwned; // Non-owning access (always used by logic) // std::vector>* _objects = nullptr; robots::RobotSystem* _robot = nullptr; + single_body_system::SingleBodySystem* _singleBody = nullptr; control::TrajectoryManager* _traj = nullptr; mutable std::mutex _stateMutex; @@ -173,6 +186,7 @@ namespace core { // Active Script Program interpreter::IStoredProgram* _activeProgram = nullptr; bool _robotPresentationDirty = false; + bool _singleBodyPresentationDirty = false; // Telemetry diagnostics::TelemetryRecorder _telemetry; // Dynamic telemetry recorder diff --git a/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h new file mode 100644 index 00000000..3b4224e0 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/SingleBodySystem/Body.h @@ -0,0 +1,72 @@ +// DSFE_CORE Body.h +#pragma once + +#include "EngineCore.h" +#include +#include "SingleBodySystems/Dynamics.h" +#include "Numerics/IntegrationService.h" + +#include "Analysis/MetricLogger.h" + +namespace single_body_system { + class DSFE_API SingleBodySystem { // FOR NOW, this represents a single RIGID BODY, but in the near-near-near-future I will extend this for particles (or make a separate ParticleSystem class, changing this to RigidBodySystem) + public: + SingleBodySystem(); + ~SingleBodySystem() = default; + + void step(double dt, double t); + + Body* body() { return _body; } // return pointer to the body + const Body* body() const { return _body; } // return const pointer to the body + + void loadBody(const std::string& name); // this does not load the actual graphical model, just the physical properties of the body (mass, inertia, etc.) from the stl file or other source + void resetBody(); // reset the body to its initial state (position, orientation, velocity, etc.) + + bool hasBody() const { return _body != nullptr; } + + integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } + std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } + void setStandardIntegrator(integration::eIntegrationMethod m) { _curIntMethod = m; } + + integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } + std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } + void setADIntegrator(integration::eAutoDiffIntegrationMethod m) { _curIntMethod_AD = m; } + + integration::IntegrationService* getIntegrator(); + const integration::IntegrationService* getIntegrator() const; + + integration::DifferentiableIntegrator* getADIntegrator(); + const integration::DifferentiableIntegrator* getADIntegrator() const; + + bool autoDiffEnabled() const { return _useAutoDiff; } + void enableAutoDiff(bool enable) { _useAutoDiff = enable; } + + std::shared_ptr runtimeIntegratorState(); + std::shared_ptr runtimeIntegratorState() const; + + private: + mathlib::VecX packState() const; + void unpackState(const mathlib::VecX& s); + + Body* _body; + std::unique_ptr _dynamics; + + std::unique_ptr _integrator; + integration::eIntegrationMethod _curIntMethod{}; + std::unique_ptr _AD_integrator; + integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + + mathlib::Vec3 _F_ext{ 0.0, 0.0, 0.0 }; + mathlib::Vec3 _tau_ext{ 0.0, 0.0, 0.0 }; + + bool _useAutoDiff = false; + + // precomputed clamp lookup tables + mutable std::vector _clampVel; + mutable std::vector _clampAngVel; + + double _simTime = 0.0; + double _mass = 1.0; + mathlib::Vec3 _g{ 0.0, 0.0, 0.0 }; + }; +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp b/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp index 0deb9bd8..7824a98f 100644 --- a/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp +++ b/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp @@ -49,7 +49,7 @@ namespace diagnostics { // Clamping flags jt.clampTheta = (j.q <= j.limits.minAngle) || (j.q >= j.limits.maxAngle); - jt.clampOmega = (std::abs(j.qd) >= j.limits.maxqd); + jt.clampOmega = (mathlib::abs(j.qd) >= j.limits.maxqd); // Accumulate clamping events clampThetaSum += (int)jt.clampTheta; @@ -58,7 +58,7 @@ namespace diagnostics { // Trajectory data if (trajOpt) { - control::TrajState ts{}; + control::TrajState ts{}; if (trajOpt->tryEval(std::string(j.child), t, ts)) { jt.traj_q = ts.q; jt.traj_qd = ts.qd; @@ -79,7 +79,7 @@ namespace diagnostics { } // Error statistics - s.err_rms = (n > 0) ? std::sqrt(sum_e2 / (double)n) : 0.0f; // RMS error + s.err_rms = (n > 0) ? mathlib::sqrt(sum_e2 / (double)n) : 0.0f; // RMS error s.err_max = max_abs_e; // max error s.clamp_theta = clampThetaSum; // total angle clamping events s.clamp_omega = clampOmegaSum; // total velocity clamping events diff --git a/DSFE_App/DSFE_Core/src/EngineCore.cpp b/DSFE_App/DSFE_Core/src/EngineCore.cpp index bf752c23..489a2b36 100644 --- a/DSFE_App/DSFE_Core/src/EngineCore.cpp +++ b/DSFE_App/DSFE_Core/src/EngineCore.cpp @@ -1,6 +1,5 @@ +// DSFE_CORE EngineCore.cpp #include "pch.h" -// File: EngineCore.cpp -// GitHub: SaltyJoss #include "EngineCore.h" #include "Scene/SimulationCore.h" diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp index 4380f64c..ea4325d9 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp @@ -6,9 +6,9 @@ #include "Robots/RobotSystem.h" #include "Robots/TrajectoryManager.h" -#include "Control/TrapezoidTrajectory.h" -#include "Control/SinusoidalTrajectory.h" -#include "Control/MultisineTrajectory.h" +#include "control/TrapezoidTrajectory.h" +#include "control/SinusoidalTrajectory.h" +#include "control/MultisineTrajectory.h" #include #include @@ -98,7 +98,7 @@ namespace commands { const double vmax = degToRad(_params[1]); const double amax = degToRad(_params[2]); - auto traj = std::make_unique(t0, q0, q1, vmax, amax); + auto traj = std::make_unique>(t0, q0, q1, vmax, amax); auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); @@ -148,7 +148,7 @@ namespace commands { // Phase double phi = (_params.size() == 5) ? degToRad(_params[4]) : 0.0; // radians - auto traj = std::make_unique(t0, t0 + dur, centre, amp, fHz, phi); + auto traj = std::make_unique>(t0, t0 + dur, centre, amp, fHz, phi); auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); @@ -186,11 +186,11 @@ namespace commands { return { CmdState::Failed, {}, "trajSet(MSINE): params after duration must be triples (amp,f,phase)." }; } - std::vector comps; + std::vector> comps; comps.reserve(rest / 3); for (size_t k = 2; k + 2 < _params.size(); k += 3) { - control::SineComponent comp; + control::SineComponent comp; comp.amp = degToRad(_params[k + 0]); // radians comp.freqHz = _params[k + 1]; // Hz comp.phaseRad = degToRad(_params[k + 2]); // radians @@ -218,7 +218,7 @@ namespace commands { } - auto traj = std::make_unique(t0, t0 + dur, centre, std::move(comps)); + auto traj = std::make_unique>(t0, t0 + dur, centre, std::move(comps)); auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); diff --git a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp index 6e7c9ef8..682e6942 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp @@ -151,7 +151,12 @@ namespace interpreter { if (_core->hasRobot()) { robots::RobotSystem* robot = _core->robotSystem(); if (!robot) { LOG_ERROR("No robot system found in simulation manager."); return; } - robot->setIntegrationMethod(static_cast(method)); + if (method == IntegratorMethod::AD_ImplicitEuler || method == IntegratorMethod::AD_ImplicitMidpoint || method == IntegratorMethod::AD_GLRK2 || method == IntegratorMethod::AD_GLRK3) { + _core->setADIntegrationMethod(static_cast(method)); + } + else { + _core->setIntegrationMethod(static_cast(method)); + } } } } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp index fb6029f3..773cf221 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp @@ -135,7 +135,7 @@ namespace utils { } // Helper function to parse double from string_view - double utils::parseDouble(const std::string_view s) { + double parseDouble(const std::string_view s) { double out = 0.0; auto first = s.data(); auto last = s.data() + s.size(); @@ -145,7 +145,7 @@ namespace utils { return out; } - float utils::parseFloat(const std::string s) { + float parseFloat(const std::string s) { float out = 0.0f; auto first = s.data(); auto last = s.data() + s.size(); @@ -156,7 +156,7 @@ namespace utils { } // Helper function to parse Vec3 from string (format: "{x, y, z}") - mathlib::Vec3 utils::parseVec3(const std::string& str) { + mathlib::Vec3 parseVec3(const std::string& str) { std::string s = stripBraces(str); std::vector vStr; std::string cur; @@ -179,7 +179,7 @@ namespace utils { } // Helper function to parse an axis mask from a string (e.g. "X", "YZ", "{X Y Z}") - AxisMask utils::parseAxisMask(const std::string& args) { + AxisMask parseAxisMask(const std::string& args) { std::string s = stripBraces(args); AxisMask mask{}; // <-- start empty (no recursion - trust me this was a pain) diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index 22ec082c..b03433a8 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -31,35 +31,44 @@ namespace integration { // Helper to convert integration method enum to string const char* IntegrationService::toString(eIntegrationMethod m) { switch (m) { - case eIntegrationMethod::Euler: - return "euler"; - case eIntegrationMethod::Midpoint: - return "midpoint"; - case eIntegrationMethod::Heun: - return "heun"; - case eIntegrationMethod::Ralston: - return "ralston"; - case eIntegrationMethod::RK4: - return "rk4"; - case eIntegrationMethod::RK45: - return "rk45"; - case eIntegrationMethod::ImplicitEuler: - return "implicit_euler"; - case eIntegrationMethod::ImplicitMidpoint: - return "implicit_midpoint"; - case eIntegrationMethod::GLRK2: - return "glrk2"; - case eIntegrationMethod::GLRK3: - return "glrk3"; - default: - return "Unknown"; + case eIntegrationMethod::Euler: return "euler"; + case eIntegrationMethod::Midpoint: return "midpoint"; + case eIntegrationMethod::Heun: return "heun"; + case eIntegrationMethod::Ralston: return "ralston"; + case eIntegrationMethod::RK4: return "rk4"; + case eIntegrationMethod::RK45: return "rk45"; + case eIntegrationMethod::ImplicitEuler: return "implicit_euler"; + case eIntegrationMethod::ImplicitMidpoint: return "implicit_midpoint"; + case eIntegrationMethod::GLRK2: return "glrk2"; + case eIntegrationMethod::GLRK3: return "glrk3"; + default: return "Unknown method"; + } + } + + const char* DifferentiableIntegrator::toString(eAutoDiffIntegrationMethod m) { + switch (m) { + case eAutoDiffIntegrationMethod::AD_ImplicitEuler: return "ad_implicit_euler"; + case eAutoDiffIntegrationMethod::AD_ImplicitMidpoint: return "ad_implicit_midpoint"; + case eAutoDiffIntegrationMethod::AD_GLRK2: return "ad_glrk2"; + case eAutoDiffIntegrationMethod::AD_GLRK3: return "ad_glrk3"; + default: return "Unknown method"; } } // Get integrator name const std::string IntegrationService::IntegratorName(eIntegrationMethod m) { return std::string(toString(m)); } - + const std::string DifferentiableIntegrator::IntegratorName(eAutoDiffIntegrationMethod m) { return std::string(toString(m)); } + // Constructor - IntegrationService::IntegrationService() - : _ODE(std::make_unique()), method(eIntegrationMethod::RK4), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() {} + IntegrationService::IntegrationService() + : _integrator(std::make_unique()), method(eIntegrationMethod::RK4), + _state(std::make_shared()), _rtol(1e-3), _atol(1e-6), _dt_last(), _dt_max() { + } + + // Constructor for autodiff + DifferentiableIntegrator::DifferentiableIntegrator() + : _integrator(std::make_unique()), _m(eAutoDiffIntegrationMethod::AD_ImplicitEuler), + _state(std::make_shared()) { + LOG_INFO("DifferentiableIntegrator constructed. Default=%d", (int)_m); + } } // namespace integration \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp b/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp index 232f4c18..31f815f5 100644 --- a/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp +++ b/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp @@ -42,12 +42,6 @@ namespace data { oss << std::setprecision(10) << d; return oss.str(); } - std::string operator()(long l) const { - std::ostringstream oss; - oss.setf(std::ios::fixed); - oss << std::setprecision(10) << l; - return oss.str(); - } std::string operator()(const std::vector& vec) const { std::ostringstream oss; oss << "["; @@ -497,8 +491,7 @@ namespace data { // Scalar numeric types -> 1D double dataset if (std::holds_alternative(val) || std::holds_alternative(val) || - std::holds_alternative(val) || - std::holds_alternative(val)) { + std::holds_alternative(val)) { if (auto it = _ds1D_D.find(dPath); it != _ds1D_D.end()) { return it->second; } @@ -559,9 +552,6 @@ namespace data { else if (std::holds_alternative(value)) { appendDouble1D(ds, static_cast(std::get(value))); } - else if (std::holds_alternative(value)) { - appendDouble1D(ds, static_cast(std::get(value))); - } else if (std::holds_alternative>(value)) { appendDouble2D(ds, std::get>(value)); } diff --git a/DSFE_App/DSFE_Core/src/Platform/Paths.cpp b/DSFE_App/DSFE_Core/src/Platform/Paths.cpp index 995321ab..0d546c44 100644 --- a/DSFE_App/DSFE_Core/src/Platform/Paths.cpp +++ b/DSFE_App/DSFE_Core/src/Platform/Paths.cpp @@ -1,29 +1,48 @@ +// DSFE_Core Paths.cpp #include "pch.h" -// File: Paths.cpp -// GitHub: SaltyJoss #include "Platform/Paths.h" +// Windows-specific includes for known folder paths #ifdef _WIN32 #define NOMINMAX #include - #ifdef _MSC_VER + #ifdef _MSC_VER #pragma comment(lib, "Shell32.lib") #endif #include // SHGetKnownFolderPath #include // CoTaskMemFree #endif +// Linux-specific includes for known folder paths +#ifdef __linux__ + #include + #include +#endif namespace { std::filesystem::path g_root, g_assets, g_configs, g_logs, g_runs; // Get the directory of the currently executing module (executable) inline std::filesystem::path getExecDir() { + #ifdef _WIN32 wchar_t buf[MAX_PATH]{}; GetModuleFileNameW(NULL, buf, MAX_PATH); return std::filesystem::path(buf).parent_path(); + #elif defined(__linux__) + char buf[PATH_MAX]{}; + ssize_t len = readlink("/proc/self/exe", buf, sizeof(buf) - 1); + if (len != -1) { + buf[len] = '\0'; + return std::filesystem::path(buf).parent_path(); + } + return std::filesystem::current_path(); + #else + return std::filesystem::current_path(); // Fallback for other platforms + #endif } + // Get the LocalAppData directory for the current user inline std::filesystem::path localAppDataDir() { + #ifdef _WIN32 PWSTR raw = nullptr; if (SUCCEEDED(SHGetKnownFolderPath(FOLDERID_LocalAppData, 0, nullptr, &raw))) { std::filesystem::path p(raw); @@ -31,6 +50,16 @@ namespace { return p; } return {}; + #else + // Linux equivalent: use $XDG_DATA_HOME or fallback to ~/.local/share + if (const char* xdg = std::getenv("XDG_DATA_HOME")) { + return std::filesystem::path(xdg); + } + if (const char* home = std::getenv("HOME")) { + return std::filesystem::path(home) / ".local" / "share"; + } + return std::filesystem::current_path(); // Fallback to current path if all else fails + #endif } } diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp index 059a0897..34204c43 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp @@ -2,581 +2,10 @@ // File: RobotDynamics.cpp // GitHub: SaltyJoss #include "Robots/RobotDynamics.h" -#include "Robots/RobotKinematics.h" -#include "Robots/RobotSimSnapshot.h" - -#include "Robots/SpatialModel.h" -#include "Robots/SpatialDynamics.h" - -#include "Robots/TrajectoryManager.h" - -#include - -#include -#include - -#include "EngineLib/LogMacros.h" namespace robots { // Constructor RobotDynamics::RobotDynamics() : _kinematics(std::make_unique()) { } - - // Computes the inertia tensor of a robot link - mathlib::Mat3 RobotDynamics::computeLinkInertiaTensor(const RobotLink& link) const { - const robots::Inertia& I = link.inertial.inertia; - - // Construct the inertia tensor matrix - Mat3 M = Mat3::Zero(); - M << I.ixx, I.ixy, I.ixz, - I.ixy, I.iyy, I.iyz, - I.ixz, I.iyz, I.izz; - - return M; // [kg*m^2], (3x3) inertia tensor in link frame - } - - static bool isControlledJoint(eJointType t) { - return - t == eJointType::REVOLUTE || - t == eJointType::PRISMATIC; - } - - // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint - double RobotDynamics::computeJointInertiaContribution( - const RobotJoint& joint, - const RobotLink& link, - const mathlib::Pose& jointWorldPose, // pose of joint frame in world - const mathlib::Pose& linkWorldPose // pose of the link in world - ) const { - const double mass = link.inertial.mass; - - // Rotation from link frame to world frame - Mat3 R_joint = jointWorldPose.block<3, 3>(0, 0); - // Joint axis in world frame - Vec3 axis_world = (R_joint * joint.axis).normalized(); - - // Position of joint in world frame - Vec3 joint_pos_world = jointWorldPose.block<3, 1>(0, 3); - - // Rotation from link frame to world frame - Mat3 R_link = linkWorldPose.block<3, 3>(0, 0); - Vec3 link_pos_world = linkWorldPose.block<3, 1>(0, 3); - Vec3 com_world = R_link * link.inertial.com_xyz + link_pos_world; - - // Translational contribution (parallel axis theorem) - Vec3 r = com_world - joint_pos_world; - - // Translational contribution to inertia about the joint axis - double I_trans = mass * (axis_world.cross(r)).squaredNorm(); - - // Rotational contribution - Mat3 I_local = computeLinkInertiaTensor(link); - Mat3 I_world = R_link * I_local * R_link.transpose(); - - // Rotational contribution to inertia about the joint axis - double I_rot = axis_world.transpose() * I_world * axis_world; - double I_eff_i = I_trans + I_rot; - - return std::max(I_eff_i, 1e-6); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities - } - - // Computes the full mass matrix M(q) based on the current state and robot configuration - void RobotDynamics::computeMassMatrix( - const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses, - mathlib::MatX& M_out - ) const { - const size_t n = robot.joints.size(); - M_out.resize(n, n); - M_out.setZero(); - - // Compute its contribution to the mass matrix for each link - for (size_t k = 0; k < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; - const double m = link.inertial.mass; - - if (m <= 0.0) { continue; } - - const Mat3 R = T_world[k].block<3, 3>(0, 0); // Rotation from link frame to world frame - const Vec3 p = T_world[k].block<3, 1>(0, 3); // Center of mass of the link in world frame - const Vec3 com = R * link.inertial.com_xyz + p; // Center of mass in world frame - - Mat3 I_local = computeLinkInertiaTensor(link); // inertia tensor in link frame - Mat3 I_world = R * I_local * R.transpose(); // inertia tensor in world frame - - // Compute Jacobian columns for each joint and accumulate mass matrix contributions - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j_i = robot.joints[i]; - if (j_i.type == eJointType::FIXED) { continue; } - - if (!robot.jointAffectsLink(i, k)) { continue; } // skip if joint i does not affect link k - - const Pose& T_joint_i = jointWorldPoses[i]; // pose of joint i in world frame - - // Rotation from joint i frame to world frame - const Mat3 R_i = T_joint_i.block<3, 3>(0, 0); // rotation from joint i frame to world frame - const Vec3 p_i = T_joint_i.block<3, 1>(0, 3); // joint position in world frame - const Vec3 z_i = (R_i * j_i.axis).normalized(); // joint axis in world frame - - Vec3 J_vi = z_i.cross(com - p_i); // linear velocity Jacobian column for joint i - Vec3 J_wi = z_i; // angular velocity Jacobian column for joint i - - // Computes the contribution to the mass matrix from this link for joints i and j - for (size_t j = 0; j < n; ++j) { - const RobotJoint& j_j = robot.joints[j]; - if (j_j.type == eJointType::FIXED) { continue; } - - if (!robot.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k - - const Pose& T_joint_j = jointWorldPoses[j]; // pose of joint i in world frame - - const Mat3 R_j = T_joint_j.block<3, 3>(0, 0); - const Vec3 p_j = T_joint_j.block<3, 1>(0, 3); - const Vec3 z_j = (R_j * j_j.axis).normalized(); - - Vec3 J_vj = z_j.cross(com - p_j); // linear velocity Jacobian column for joint j - Vec3 J_wj = z_j; // angular velocity Jacobian column for joint j - - M_out(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; // [kg*m^2] - } - } - } - } - - // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration - mathlib::VecX RobotDynamics::computeCoriolisVector( - const RobotConstModel& robot, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const std::vector& T_world, - const mathlib::MatX& M - ) const { - const size_t n = robot.joints.size(); - const double eps = 1e-6; // small value to prevent division by zero - mathlib::VecX q_eps = q; - - std::vector dM_dq(n, MatX::Zero(n, n)); // partial derivatives of M with respect to each joint angle - VecX x_eps(2 * n); // state vector for kinematics - - std::vector T_world_eps; // forward kinematics for perturbed configurations - T_world_eps.resize(robot.links.size()); - - std::vector jointWorldPoses_eps; - jointWorldPoses_eps.resize(n); - - MatX M_plus(n, n); - - // Finite difference approximation of dM/dq for each joint - for (size_t k = 0; k < n; ++k) { - q_eps = q; // reset to original configuration for each joint perturbation - q_eps[k] += eps; // perturb joint k by a small amount - - // Construct the state vector for the perturbed configuration - for (size_t i = 0; i < n; ++i) { - x_eps[i] = q_eps[i]; - x_eps[n + i] = qd[i]; - } - - // Compute forward kinematics for the perturbed state - _kinematics->computeForwardKinematics_fromState(robot, x_eps, T_world_eps); - - jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); - - computeMassMatrix(robot, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration - - dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with - } - - // Compute Coriolis and centrifugal bias vector h using Christoffel symbols of the first kind - VecX h = VecX::Zero(n); - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j) { - for (size_t k = 0; k < n; ++k) { - double C_ijk = 0.5 * (dM_dq[k](i, j) + dM_dq[j](i, k) - dM_dq[i](j, k)) * qd[k]; // Christoffel symbol of the first kind for indices (i, j, k) - h(i) += C_ijk * qd[j] * qd[k]; // contribution to Coriolis and centrifugal bias for joint i from joints j and k - } - } - } - return h; // [Nm], Coriolis and centrifugal bias vector for the robot at configuration q and velocity qd - } - - // Computes the gravity torque for a joint based on the current state and robot configuration - mathlib::VecX RobotDynamics::computeGravityTorque( - const RobotConstModel& robot, - const std::vector& T_world, - const std::vector& jointWorldPoses - ) const { - const size_t n = robot.joints.size(); - mathlib::VecX tau_G = VecX::Zero(n); - double g{ _gravity }; // [m/s^2], gravity acceleration magnitude - - // For each joint, sum the gravity contributions from all links - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = robot.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - - double tau_g_i = 0.0; // [Nm], gravity torque contribution for joint i - - const Pose& T_joint = jointWorldPoses[i]; // pose of joint i in world frame - - const Mat3 R_i = T_joint.block<3, 3>(0, 0); - const Vec3 p_i = T_joint.block<3, 1>(0, 3); - const Vec3 axis_world = (R_i * robot.joints[i].axis).normalized(); - - // For each link, compute the gravitational force and its torque contribution about joint i - for (size_t k = 0; k < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; - const double m = link.inertial.mass; - if (m <= 0.0) { continue; } - - if (!robot.jointAffectsLink(i, k)) { continue; } - - // Link's center of mass in world frame - const Mat3 R_k = T_world[k].block<3, 3>(0, 0); - const Vec3 com_world = R_k * link.inertial.com_xyz + T_world[k].block<3, 1>(0, 3); - - // Gravitational force on the link - Vec3 g_world = Vec3(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame - const Vec3 F_g = m * g_world; // [N], gravitational force on the link in world frame - - // Torque contribution from this link's weight about joint i - const Vec3 r = com_world - p_i; // [m] - - // Torque = r × F_g projected onto joint axis - tau_g_i += axis_world.dot(r.cross(F_g)); - } - tau_G[i] = tau_g_i; - } - return tau_G; // [Nm], gravity torques for each joint - } - - void RobotDynamics::analyticalJacobian( - const RobotConstModel& robot, - const mathlib::VecX& x, - mathlib::MatX& J_out, - DenseDynamicsScratch& scratch - ) { - const size_t n = robot.joints.size(); - - Eigen::Map q_local(x.data(), n); - Eigen::Map qd_local(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(robot, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); - computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); - - J_out.setZero(2 * n, 2 * n); // [rad/rad] position part, [rad/s / rad/s] velocity part - J_out.block(0, n, n, n).setIdentity(); - - MatX dTau_dq = MatX::Zero(n, n); - MatX dTau_dv = MatX::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = robot.joints[i]; - if (joint.type == eJointType::FIXED) { continue; } - - const double wn = joint.wn_target; - const double z = joint.zeta_target; - const double I_eff = std::max(scratch.M(i, i), 1e-6); - - const double k_p = I_eff * wn * wn; - const double k_d = 2.0 * z * I_eff * wn; - - dTau_dq(i, i) = -k_p; - - double qd_i = qd_local[i]; - double tanh_term = std::tanh(qd_i / 1e-2); - double stiff_friction_slope = -0.05 * (1.0 - tanh_term * tanh_term) / 1e-2; - dTau_dv(i, i) = -k_d - 0.2 + stiff_friction_slope; - } - - auto solver = scratch.M.ldlt(); - MatX da_dq = solver.solve(dTau_dq); - MatX da_dv = solver.solve(dTau_dv); - - J_out.block(n, 0, n, n) = da_dq; - J_out.block(n, n, n, n) = da_dv; - } - - // Computes the derivative of the state vector (q, qd) based on the current state and robot configurations - mathlib::VecX RobotDynamics::derivative_dense( - double /*t*/, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - mathlib::VecX dx(2 * n); - - // Map the input state vector to joint angles and velocities - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); - - computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector - - scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); - } - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = snap.model->joints[i]; - - // Fixed joints - if (joint.type == eJointType::FIXED) { - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; - out.metrics.qdd[i] = 0.0; - continue; - } - - const double wn = joint.wn_target; // [rad/s], natural frequency - const double z = joint.zeta_target; // damping ratio - - const double err = snap.q_ref[i] - q[i]; // [rad], position error - const double err_d = snap.qd_ref[i] - qd[i]; // [rad/s], velocity error - - const double I_eff = std::max(scratch.dense.M(i, i), 1e-6); // [kg*m^2], effective inertia for joint i with floor to prevent singularities - const double k_p = I_eff * wn * wn; // [Nm/rad], proportional gain - const double k_d = 2.0 * z * I_eff * wn; // [Nm/(rad/s)], derivative gain - - double tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i - tau_i += scratch.g[i]; // Gravity compensation - tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias - tau_i -= /*joint.dynamics.damping*/ 0.2 * qd[i]; // subtract viscous damping - tau_i -= /*joint.dynamics.friction*/ 0.05 * std::tanh(qd[i] / 1e-2); // subtract Coulomb friction - - scratch.dense.tau[i] = tau_i; - - // metrics - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; - - out.metrics.err[i] = err; - out.metrics.errd[i] = err_d; - - out.metrics.I_eff[i] = I_eff; - out.metrics.tau[i] = tau_i; - - //_metrics.tau_sat[i] = tau_sat; - //_metrics.sat_flag[i] = saturated; - } - - // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g - - // Solve for Accelerations - out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics - out.metrics.qdd = out.qdd; - - // Fill derivatives - dx.head(n) = qd; - dx.tail(n) = out.qdd; - - return dx; - } - - mathlib::VecX RobotDynamics::derivative_spatial( - const robots::SpatialModel& model, - double /*t*/, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - VecX dx(2 * n); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - SpatialDynamics::computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - MatX M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - // RNEA to compute gravity compensation (q, 0, 0) for gravity, (q, qd, 0) for Coriolis - VecX tau_g = SpatialDynamics::RNEA(model, q, VecX::Zero(n), VecX::Zero(n), scratch); - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { - scratch.dense.tau[i] = 0.0; - continue; - } - - const double wn = 5.0; - const double z = 0.7; - - const double err = snap.q_ref[i] - q[i]; - const double err_d = snap.qd_ref[i] - qd[i]; - - const double I_eff = std::max(M(i, i), 1e-6); - const double k_p = I_eff * wn * wn; - const double k_d = 2.0 * z * I_eff * wn; - - double tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - tau_i -= 0.2 * qd[i]; - //tau_i -= 0.05 * std::tanh(qd[i] / 1e-2); - - scratch.dense.tau[i] = tau_i; - - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; - - out.metrics.err[i] = err; - out.metrics.errd[i] = err_d; - - out.metrics.I_eff[i] = I_eff; - out.metrics.tau[i] = tau_i; - } - - out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); - out.metrics.qdd = out.qdd; - dx.head(n) = qd; - dx.tail(n) = out.qdd; - - return dx; - } - - void RobotDynamics::jacobian_spatial( - const robots::SpatialModel& model, - const mathlib::VecX& x, - const RobotSimSnapshot& snap, - const mathlib::VecX& kp, - const mathlib::VecX& kd, - mathlib::MatX& F_out, - DynamicsScratch& scratch - ) { - const size_t n = snap.model->joints.size(); - - F_out.setZero(2 * n, 2 * n); - F_out.block(0, n, n, n).setIdentity(); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - SpatialDynamics::computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - scratch.dense.M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - - MatX dTau_dq = MatX::Zero(n, n); - MatX dTau_dv = MatX::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { continue; } - dTau_dq(i, i) = -kp[i]; - const double eps_fric = 1e-2; - double tanh_term = std::tanh(qd[i] / eps_fric); - double stiff_friction_slope = -0.05 * (1.0 - tanh_term * tanh_term) / eps_fric; - dTau_dv(i, i) = -kd[i] - 0.2; - } - - Eigen::LDLT solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving - MatX dqdd_dtau_q = solver.solve(dTau_dq); // compute the partial derivative of qdd with respect to q - MatX dqdd_dtau_v = solver.solve(dTau_dv); // compute the partial derivative of qdd with respect to qd - - F_out.block(n, 0, n, n) = dqdd_dtau_q; // fill the Jacobian block for qdd with respect to q - F_out.block(n, n, n, n) = dqdd_dtau_v; // fill the Jacobian block for qdd with respect to qd) - } - - mathlib::VecX RobotDynamics::derivative_with_gains( - double t, const mathlib::VecX& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp, const mathlib::VecX& kd, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - mathlib::VecX dx(2 * n); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); - - computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); - - scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); - } - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - if (snap.model->joints[i].type == eJointType::FIXED) continue; - - // FIXED: Use the constant, frozen step-start values passed down - double tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + std::max(scratch.dense.M(i, i), 1e-6) * snap.qdd_ref[i]; - tau_i += scratch.g[i] + scratch.dense.h[i]; - tau_i -= 0.2 * qd[i]; - tau_i -= 0.05 * std::tanh(qd[i] / 1e-2); - - scratch.dense.tau[i] = tau_i; - } - - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; - out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); - out.metrics.qdd = out.qdd; - - dx.head(n) = qd; - dx.tail(n) = out.qdd; - return dx; - } - - void RobotDynamics::jacobian_with_gains( - const mathlib::VecX& x, const RobotSimSnapshot& snap, - const mathlib::VecX& kp, const mathlib::VecX& kd, mathlib::MatX& F_out, - DenseDynamicsScratch& scratch - ) { - const size_t n = snap.model->joints.size(); - - F_out.setZero(2 * n, 2 * n); - F_out.block(0, n, n, n).setIdentity(); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - // Compute a local mass matrix for this exact stage evaluation frame - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); - computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); - - mathlib::MatX dTau_dq = mathlib::MatX::Zero(n, n); - mathlib::MatX dTau_dv = mathlib::MatX::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - if (snap.model->joints[i].type == eJointType::FIXED) continue; - - dTau_dq(i, i) = -kp[i]; - - double tanh_term = std::tanh(qd[i] / 1e-2); - double stiff_friction = -0.05 * (1.0 - tanh_term * tanh_term) / 1e-2; - dTau_dv(i, i) = -kd[i] - 0.2 + stiff_friction; - } - - auto solver = scratch.M.ldlt(); - F_out.block(n, 0, n, n) = solver.solve(dTau_dq); - F_out.block(n, n, n, n) = solver.solve(dTau_dv); - } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp index b4e64f0e..c7b181ac 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp @@ -2,11 +2,6 @@ // File: RobotKinematics.cpp // GitHub: SaltyJoss #include "Robots/RobotKinematics.h" -#include "Robots/RobotSimSnapshot.h" -#include -#include - -#include "EngineLib/LogMacros.h" using namespace mathlib; using namespace constants; @@ -15,102 +10,4 @@ namespace robots { // Constructor RobotKinematics::RobotKinematics() { } - - // Computes the forward kinematics for the robot based on the current state and robot configuration - void RobotKinematics::computeForwardKinematics_fromState( - const RobotConstModel& robot, - const mathlib::VecX& x, - std::vector& T_world_out - ) const { - const auto& joints = robot.joints; - const auto& links = robot.links; - const size_t n = (size_t)joints.size(); - - T_world_out.resize(robot.links.size()); - - Pose T = Pose::Identity(); // world -> base - T_world_out[0] = T; // base link - - if (T_world_out.empty()) { - LOG_ERROR("T_world_out is empty"); - return; - } - - // Compute the transform to the next link using each joint - for (size_t i = 0; i < n; ++i) { - const auto& joint = joints[i]; - const double q = x[i]; // joint angle from state vector - - Pose T_origin = Pose::Identity(); // transform from parent link to joint frame (fixed) - T_origin.block<3, 3>(0, 0) = joint.origin_q.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON - T_origin.block<3, 1>(0, 3) = joint.origin_xyz; // translation from parent link to joint frame - - // Compute joint motion transform based on joint axis and angle - Pose T_motion = Pose::Identity(); - if (joint.type == eJointType::REVOLUTE) { - T_motion = jointMotionTransform(joint.axis, q); // rotation about joint axis - } - else if (joint.type == eJointType::PRISMATIC) { - T_motion.block<3, 1>(0, 3) = joint.axis.normalized() * q; // translation along joint axis - } - - // compose transforms - T = T * T_origin * T_motion; // parent -> joint -> motion -> child - - int childIdx = robot.linkIndex(joint.child); - if (childIdx < 0 || childIdx >= T_world_out.size()) { - LOG_ERROR("Invalid child link index for joint {}: {}", joint.name.c_str(), childIdx); - continue; - } - - T_world_out[childIdx] = T; // world -> child link - } - } - - // Computes the joint world poses for all joints based on the current state and robot configuration - std::vector RobotKinematics::calcJointWorldPoses( - const std::vector& T_world, - const RobotConstModel& robot - ) { - std::vector jointWorldPoses(robot.joints.size()); - - for (size_t i = 0; i < robot.joints.size(); ++i) { - const RobotJoint& joints = robot.joints[i]; - int childIdx = robot.linkIndex(joints.child); - - if (childIdx < 0 || childIdx >= T_world.size()) { - LOG_ERROR("Invalid child link index for joint {}: {}", joints.name.c_str(), childIdx); - continue; - } - - jointWorldPoses[i] = T_world[childIdx]; - } - return jointWorldPoses; - } - - // Computes the forward kinematics for a single joint motion based on the joint axis and angle - mathlib::Pose RobotKinematics::jointMotionTransform( - const mathlib::Vec3& axis_joint, - double q - ) const { - Pose T = Pose::Identity(); // homogeneous transformation matrix (4x4) - - Eigen::AngleAxisd aa(q, axis_joint.normalized()); // create angle-axis rotation from joint angle and axis - T.block<3, 3>(0, 0) = aa.toRotationMatrix(); // set upper-left 3x3 block to rotation matrix - - return T; // (4x4) homogeneous transformation - } - - // Converts roll-pitch-yaw angles (in radians) to a quaternion representation - mathlib::Quat RobotKinematics::rpyRadToQuat(const mathlib::Vec3& rpyRad) { - const double roll = rpyRad.x(); - const double pitch = rpyRad.y(); - const double yaw = rpyRad.z(); - - const Quat qx(Eigen::AngleAxisd(roll, Vec3(1.0, 0.0, 0.0))); - const Quat qy(Eigen::AngleAxisd(pitch, Vec3(0.0, 1.0, 0.0))); - const Quat qz(Eigen::AngleAxisd(yaw, Vec3(0.0, 0.0, 1.0))); - - return (qz * qy * qx).normalized(); - } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp index 7426b063..b0b0b171 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp @@ -2,9 +2,7 @@ #include "pch.h" #include "Robots/RobotLoader.h" - -#include -#include +#include #include "EngineLib/LogMacros.h" #include @@ -19,8 +17,7 @@ namespace robots { // --- Static Helper Functions --- // tf2::Quaternion::setRPY(roll,pitch,yaw) corresponds to q = qz * qy * qx. - static Quat rpyRadToQuat(const Vec3& rpyRad) - { + static Quat rpyRadToQuat(const Vec3& rpyRad) { const double roll = rpyRad.x(); const double pitch = rpyRad.y(); const double yaw = rpyRad.z(); @@ -318,11 +315,13 @@ namespace robots { joint.dynamics.damping = 0.0; joint.dynamics.friction = 0.0; - if (!jointData.contains("dynamics") || !jointData["dynamics"].is_object()) { return; } + if (!jointData.contains("dynamics") || !jointData["dynamics"].is_object()) { LOG_ERROR("Could not find joint dynamic data"); return; } const auto& D = jointData["dynamics"]; joint.dynamics.damping = D.value("damping", joint.dynamics.damping); joint.dynamics.friction = D.value("friction", joint.dynamics.friction); + joint.wn_target = D.value("wn_target", joint.wn_target); + joint.zeta_target = D.value("zeta_target", joint.zeta_target); if (joint.dynamics.damping < 0.0) { joint.dynamics.damping = 0.0; } if (joint.dynamics.friction < 0.0) { joint.dynamics.friction = 0.0; } @@ -336,7 +335,7 @@ namespace robots { } // Parse DH parameters if present - static bool parseDHParameters(const json& jointData, DH_Params& out) { + static bool parseDHParameters(const json& jointData, DH_Params& out) { // Accept "dh" ONLY (your JSON uses "dh") if (!jointData.contains("dh") || !jointData["dh"].is_object()) return false; @@ -344,7 +343,7 @@ namespace robots { out.a = dh.value("a", 0.0); out.alpha = dh.value("alpha", 0.0); out.d = dh.value("d", 0.0); - out.theta = dh.value("theta0", 0.0); // your key is theta0 + out.theta = dh.value("theta0", 0.0); out.type = parseDHType(dh.value("type", "revolute")); return true; } @@ -453,8 +452,8 @@ namespace robots { joint.type = eJointType::FIXED; joint.axis = Vec3::Zero(); joint.limits.continuous = false; - joint.limits.minAngle = 0.0f; - joint.limits.maxAngle = 0.0f; + joint.limits.minAngle = 0.0; + joint.limits.maxAngle = 0.0; } else { parseJointAxis(jointData, joint); @@ -466,7 +465,7 @@ namespace robots { // If robot is DH-mode, also parse DH table if (robot.kinematicsModel == eKinematicsModel::DH) { - DH_Params dh{}; + DH_Params dh{}; if (!parseDHParameters(jointData, dh)) { LOG_WARN("Joint %s missing 'dh' unexpectedly; forcing URDF mode.", joint.name.c_str()); robot.kinematicsModel = eKinematicsModel::URDF; @@ -478,16 +477,16 @@ namespace robots { } if (abs(joint.limits.minAngle) == abs(joint.limits.maxAngle) && !joint.limits.continuous) { - LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle); - D_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle); + LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); + D_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); } else { - LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle); - D_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle); + LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); + D_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: % .2f, | Friction : % .2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction);; } } diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp index 42e43597..888f166e 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp @@ -2,19 +2,13 @@ #include "pch.h" #include "Robots/RobotSystem.h" - -#include -#include "Robots/RobotKinematics.h" -#include "Robots/RobotDynamics.h" #include "Robots/RobotLoader.h" -#include - #include #include #include -#include +#include #include "Robots/TrajectoryManager.h" #include "Platform/Paths.h" @@ -28,16 +22,15 @@ namespace robots { // Constructor RobotSystem::RobotSystem() : _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), + _AD_integrator(std::make_unique()), _curIntMethod_AD(integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler), _kinematics(std::make_unique()), _dynamics(std::make_unique()), _torqueMode(eTorqueMode::CONTROLLED) { - if (!_integrator) { LOG_WARN("RobotSystem got null IntegrationService*"); } + if (!_integrator ) { LOG_WARN("RobotSystem got null IntegrationService*"); } } // Destructor RobotSystem::~RobotSystem() = default; - const robots::RobotModel& RobotSystem::model() const { - return _robot; - } + const robots::RobotModel& RobotSystem::model() const { return _robot; } // Helper function to convert std::vector to Eigen::VectorXd static VecX toVecX(const std::vector& a) { @@ -92,7 +85,7 @@ namespace robots { for (size_t i = 0; i < n; ++i) { const RobotJoint& j = _robot.joints[i]; - SpatialJoint& sj = _spatialModel.joints[i]; + auto& sj = _spatialModel.joints[i]; sj.name = j.name; sj.type = j.type; @@ -204,7 +197,6 @@ namespace robots { if (theta_out != theta_in) { const double upperLimit = j.limits.maxAngle; const double lowerLimit = j.limits.minAngle; - if (theta_out >= upperLimit && omega_in > 0.0f) { omega_out = 0.0f; } if (theta_out <= lowerLimit && omega_in < 0.0f) { omega_out = 0.0f; } } @@ -212,13 +204,75 @@ namespace robots { // Record clamping _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; - // Update joint states j.q = theta_out; j.qd = omega_out; } } + // Method to pack robot joint states into a state vector + mathlib::VecX_T> RobotSystem::packState_AD() const { + using Dual = DualNumber_T; // only hardcoded since I am testing the same arm, TODO provide a better final way to derive the NVar val. + const size_t n = static_cast(_robot.joints.size()); + mathlib::VecX_T x(2 * n); + + // Pack angles and velocities + for (size_t i = 0; i < n; ++i) { + auto& j = _robot.joints[i]; + + // Current states + x[i] = Dual(j.q, { 0.0 }); + x[i + n] = Dual(j.qd, { 0.0 }); + } + return x; // state vector + } + + // Method to unpack state vector into robot joints + void RobotSystem::unpackState_AD(const mathlib::VecX_T>& x) { + using Dual = DualNumber_T; + const size_t n = static_cast(_robot.joints.size()); + + // Resize clamping vectors if necessary + if (_clampTheta.size() != n) { _clampTheta.assign(n, 0); } + if (_clampOmega.size() != n) { _clampOmega.assign(n, 0); } + + // For each joint + for (size_t i = 0; i < n; ++i) { + auto& j = _robot.joints[i]; + + // Current states + Dual theta_in = x[i]; // [rad] + Dual omega_in = x[i + n]; // [rad/s] + + // Clamp joint angle + Dual theta_out = clampJointAngle_T(j, theta_in); + + // max |omega| + const double wMax_hw = mathlib::abs(j.limits.maxqd); + Dual omega_out = omega_in; + + // Velocity limit clamping + if (wMax_hw > 0.0) { + omega_out = wMax_hw * mathlib::tanh(omega_in / wMax_hw); // smoothly clamp omega to wMax_hw using a tanh function + } + + // Velocity limit enforcement + if (theta_out != theta_in) { + const double upperLimit = j.limits.maxAngle; + const double lowerLimit = j.limits.minAngle; + if (theta_out >= upperLimit && omega_in > Dual(0)) { omega_out = Dual(0); } + if (theta_out <= lowerLimit && omega_in < Dual(0)) { omega_out = Dual(0); } + } + + // Record clamping + _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; + _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; + // Update joint states + j.q = mathlib::real(theta_out); + j.qd = mathlib::real(omega_out); + } + } + // Method to pack reference state vector (target angles and velocities) for control mathlib::VecX RobotSystem::packRefState() const { const size_t n = (int)_robot.joints.size(); @@ -255,199 +309,28 @@ namespace robots { if (j.q > hi) { j.q = hi; if (j.qd > 0.0f) { j.qd = 0.0f; }} } - // Method to take a snapshot of the current robot state - RobotSimSnapshot RobotSystem::takeSnapshot(double simTime) const { - RobotSimSnapshot snap; - snap.model = &_constModel; - - const size_t n = (size_t)_robot.joints.size(); - - snap.q.resize(n); - snap.qd.resize(n); - snap.q_ref.resize(n); - snap.qd_ref.resize(n); - snap.qdd_ref.resize(n); - - for (size_t i = 0; i < n; ++i) { - const auto& j = _robot.joints[i]; - snap.q[i] = j.q; - snap.qd[i] = j.qd; - snap.q_ref[i] = j.q_ref; - snap.qd_ref[i] = j.qd_ref; - snap.qdd_ref[i] = j.qdd_ref; - } - - snap.robotRootPose = _robotRootPose; - snap.baseIsFree = _baseIsFree; - snap.lastBaseForwardForce = _lastBaseForwardForce; - snap.gravity = _gravity; - - snap.torqueMode = _robot.torqueMode; - snap.dt = _dynamics->dt(); - snap.simTime = simTime; - - return snap; - } - // Method to advance the robot state by dt using the selected integrator void RobotSystem::step(double dt, double simTime) { if (!_hasRobot) { return; } - _simTime = simTime; - mathlib::VecX x = packState(); - - RobotSimSnapshot snap = takeSnapshot(simTime); - const size_t n = snap.model->joints.size(); - - Eigen::Map q(x.data(), n); - Eigen::Map qd(x.data() + n, n); - - mathlib::VecX qdd(n); - for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } - - std::vector T_start(snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); - std::vector jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); - - SpatialDynamics::computeSpatialKinematicsAndBias( - _spatialModel, - q, qd, - _dynScratch.spatial.Xup, - _dynScratch.spatial.v, _dynScratch.spatial.c - ); - - // CRBA only for controller inertia scaling - mathlib::MatX M_start = SpatialDynamics::CRBA( - _spatialModel, - _dynScratch.spatial.Xup, - _dynScratch - ); - - // Cache frozen joint gains for this step - mathlib::VecX kp_frozen(n), kd_frozen(n); - for (size_t i = 0; i < n; ++i) { - const auto& joint = snap.model->joints[i]; - if (joint.type == eJointType::FIXED) { continue; } - - _dynScratch.dense.I_eff_controller[i] = std::max(M_start(i, i), 1e-6); - const double I_eff = _dynScratch.dense.I_eff_controller[i]; - kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; - kd_frozen[i] = 2.0 * joint.zeta_target * I_eff * joint.wn_target; + if (_useAutoDiff) { + step_AD(dt, simTime); + return; } - // Compute RNEA torques for feedforward control - mathlib::VecX tau_rnea = SpatialDynamics::RNEA( - _spatialModel, - q, qd, qdd, - _dynScratch - ); // [Nm] - LOG_INFO_ONCE("tau_rnea size = %lld", (long long)tau_rnea.size()); - - // Define the derivative function for integration, capturing necessary variables by reference - auto f_deriv = [&, kp_frozen, kd_frozen](double t, const mathlib::VecX& xIn) { - return _dynamics->derivative_spatial(_spatialModel, - t, xIn, snap, - _dynScratch, _dynResult - ); - }; - // Define the Jacobian function for integration, capturing necessary variables by reference - auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX& xIn, mathlib::MatX& J_out) { - _dynamics->jacobian_spatial(_spatialModel, - xIn, snap, - kp_frozen, kd_frozen, - J_out, _dynScratch - ); - }; - - auto step = _integrator->stepODE(_curIntMethod, x, simTime, dt, f_deriv, f_J); - - unpackState(step.x_next); - _dynamics->setDt(step.dt_taken); - - Eigen::Map q_next(step.x_next.data(), n); - Eigen::Map qd_next(step.x_next.data() + n, n); - - // Enforce joint limits - for (auto& j : _robot.joints) { enforceJointLimits(j); } - - // Recompute kinematics and dynamics at the new state for logging and control purposes - std::vector T_world(snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*snap.model, step.x_next, T_world); - std::vector jointWorldPoses = _kinematics->calcJointWorldPoses(T_world, *snap.model); - - // Compute spatial kinematics and bias terms for the new state - SpatialDynamics::computeSpatialKinematicsAndBias( - _spatialModel, - q_next, qd_next, - _dynScratch.spatial.Xup, - _dynScratch.spatial.v, _dynScratch.spatial.c - ); - // Compute mass matrix at the new state - MatX M_full = SpatialDynamics::CRBA( - _spatialModel, - _dynScratch.spatial.Xup, - _dynScratch - ); - - VecX tau_g = _dynamics->computeGravityTorque(*snap.model, T_world, jointWorldPoses); - - // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd - double sys_KE = 0.5 * qd_next.transpose() * M_full * qd_next; // [J], kinetic energy of the robot at configuration q and velocity qd - - // Compute system potential energy at configuration q (relative to gravity) - double sys_PE = 0.0; - double g = _dynamics->getGravity(); - - for (size_t k = 0; k < _robot.links.size(); ++k) { - const RobotLink& link = _robot.links[k]; - const double m = link.inertial.mass; - - if (m <= 0.0) { continue; } - - Vec3 com_world = - (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + - T_world[k].block<3, 1>(0, 3); + _simTime = simTime; + const size_t n = _robot.joints.size(); + mathlib::VecX x = packState(); - sys_PE += m * g * com_world.z(); - } + auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); - const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system + unpackState(result.stepOut.x_next); + _dynamics->setDt(result.stepOut.dt_taken); - // Log metrics to buffer if logging is enabled - robots::JointLogBuffer* buf = nullptr; + const auto scratchCopy = _dynScratch; + const auto resultCopy = result; - // If using internal logging, get the active buffer - if (_useInternalLogging) { - int idx = _activeLogBufIdx.load(std::memory_order_acquire); - buf = &_logBuffers[idx]; - } else { - buf = _logBuffer; - } - - if (buf) { - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = snap.model->joints[i]; - - const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : _dynResult.metrics.I_eff[i]; - const double err = snap.q_ref[i] - q_next[i]; - const double err_d = snap.qd_ref[i] - qd_next[i]; - - JointLogBuffer::JointLogEntry e{}; - - e.sim_time = simTime; - e.dt_taken = step.dt_taken; - e.dt_sug = step.dt_sug; - e.theta = q_next[i]; e.omega = qd_next[i]; e.alpha = _dynResult.metrics.qdd[i]; - e.err = err; e.err_d = err_d; - e.I_eff = I_eff; - e.tau = _dynResult.metrics.tau[i]; e.tau_ff = tau_rnea[i]; e.tau_gravity = tau_g[i]; - e.tau_sat = _dynResult.metrics.tau_sat[i]; - e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; - e.clamp_theta = (double)_clampTheta[i]; e.clamp_omega = (double)_clampOmega[i]; - e.sat_flag = _dynResult.metrics.sat_flag[i]; e.joint_index = (int)i; - buf->push_entry(e); - } - } + postStepUpdate(resultCopy.stepOut.x_next, scratchCopy, resultCopy); // Update base pose if free-floating if (_baseIsFree) { @@ -469,7 +352,7 @@ namespace robots { // Sample trajectories ("ground truth" inputs) for (size_t i = 0; i < n; ++i) { RobotJoint& j = _robot.joints[i]; - control::TrajState s{}; + control::TrajState s{}; // Try to evaluate trajectory if (traj.tryEval(std::string(j.child), t, s)) { j.q_ref = clampJointAngle(j, s.q); // set ref angle @@ -564,9 +447,6 @@ namespace robots { buildSpatialModel(); _hasRobot = true; - _dynScratch.resize(n, m); - _dynResult.resize(n); - resetRobot(); LOG_INFO("Loaded robot model -> %s", name.c_str()); @@ -575,7 +455,7 @@ namespace robots { // Method to reset the robot to its home position void RobotSystem::resetRobot() { - if (!_hasRobot || !_robotHomeValid) { return; } + if (!_hasRobot || !_robotHomeValid) { LOG_ERROR("Reset aborterd."); return; } _robotRootPose = _robotRootHome; _robot.setJointVector(_robotQHome); @@ -596,8 +476,18 @@ namespace robots { _baseYawRate = 0.0; _baseYawAcc = 0.0; + _dynScratch.clear(); + _dynScratch_AD.clear(); + + _dynResult.resize(0); + _dynResult_AD.resize(0); + _dynScratch.resize(_robot.joints.size(), _robot.links.size()); + _dynScratch_AD.resize(_robot.joints.size(), _robot.links.size()); _dynResult.resize(_robot.joints.size()); + _dynResult_AD.resize(_robot.joints.size()); + + _dynScratch.g.setConstant(_gravity); // Reset adaptive integrator so it doesn't carry a stale step size _integrator->resetAdaptiveState(); @@ -618,6 +508,17 @@ namespace robots { integration::IntegrationService* RobotSystem::getIntegrator() { return _integrator.get(); } const integration::IntegrationService* RobotSystem::getIntegrator() const { return _integrator.get(); } + integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() { return _AD_integrator.get(); } + const integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() const { return _AD_integrator.get(); } + + std::shared_ptr RobotSystem::runtimeIntegratorState() { + return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); + } + + std::shared_ptr RobotSystem::runtimeIntegratorState() const { + return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); + } + // --- ROBOT KINEMATICS AND JOINT STATE METHODS --- std::string RobotSystem::findRootLink() const { @@ -988,9 +889,7 @@ namespace robots { double RobotSystem::computeForwardDrive() const { double drive = 0.0; for (const auto& j : _robot.joints) { - if (j.name.find("hip_pitch") != std::string::npos) { - drive += -j.qd; - } + if (j.name.find("hip_pitch") != std::string::npos) { drive += -j.qd; } } return drive; } @@ -1019,12 +918,8 @@ namespace robots { _baseVel += _baseAcc * dt; _basePos += _baseVel * dt; - LOG_INFO_ONCE("hipL=%.3f hipR=%.3f gaitPhase=%.3f", - hipL, hipR, hipR - hipL - ); - LOG_INFO_ONCE("baseVel = (%.3f, %.3f, %.3f)", - _baseVel.x(), _baseVel.y(), _baseVel.z() - ); + LOG_INFO_ONCE("hipL=%.3f hipR=%.3f gaitPhase=%.3f", hipL, hipR, hipR - hipL); + LOG_INFO_ONCE("baseVel = (%.3f, %.3f, %.3f)", _baseVel.x(), _baseVel.y(), _baseVel.z()); } // Method to update the robot root pose based on the integrated base translation (for legged robots) diff --git a/DSFE_App/DSFE_Core/src/Robots/SpatialDynamics.cpp b/DSFE_App/DSFE_Core/src/Robots/SpatialDynamics.cpp deleted file mode 100644 index 116a51ee..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/SpatialDynamics.cpp +++ /dev/null @@ -1,304 +0,0 @@ -// DSFE_Core SpatialDynamics.cpp -#include "pch.h" - -#include "Robots/SpatialDynamics.h" - -namespace robots { - // Recursive function to compute spatial velocities using the articulated body algorithm - void SpatialDynamics::computeSpatialKinematicsAndBias( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - std::vector& Xup_out, - std::vector& v_out, - std::vector& c_out - ) { - const size_t n = model.joints.size(); - v_out.resize(n); - Xup_out.resize(n); - c_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - // Joint Transform XJ - mathlib::SpatialMat XJ = mathlib::SpatialMat::Identity(); - - if (j.type == eJointType::REVOLUTE) { - Eigen::AngleAxisd aa(q[i], j.S.angular().normalized()); - mathlib::Mat3 R = aa.toRotationMatrix(); - XJ = mathlib::spatialTransform(R, mathlib::Vec3::Zero()); - } - else if (j.type == eJointType::PRISMATIC) { - mathlib::Vec3 r = q[i] * j.S.linear().normalized(); - XJ = mathlib::spatialTransform(mathlib::Mat3::Identity(), r); - } - - Xup_out[i] = XJ * j.Xtree; // Combined Transform - mathlib::SpatialVec vJ = j.S * qd[i]; // Joint Velocity - - // Root Link - if (j.parent < 0) { v_out[i] = vJ; } - else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } - - c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term - } - } - - // Recursive function to compute spatial accelerations using the articulated body algorithm - void SpatialDynamics::computeAccelerations_RNEA( - const SpatialModel& model, - const mathlib::VecX& qdd, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& g, - std::vector& a_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - - SpatialVec a0; - a0.v << g.segment<3>(0), g.segment<3>(3); // Base acceleration (gravity) - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec aJ = j.S * qdd[i]; // Joint Acceleration - - // Root Link - if (j.parent < 0) { - a_out[i] = Xup[i] * a0 + aJ + c[i]; - continue; - } - - a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; - } - } - - // Recursive function to compute inverse dynamics (joint torques) using the articulated body algorithm - void SpatialDynamics::computeBackwardForces_RNEA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& a, - mathlib::VecX& tau_out - ) { - const size_t n = model.joints.size(); - tau_out.resize(n); - - std::vector f(n); - - // Forward Force Computation - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec I_v = j.inertia * v[i]; - mathlib::SpatialVec coriolis = crossForce(v[i], I_v); - f[i].v = j.inertia * a[i].v + coriolis.v; - } - - // Backward Recursion Computation - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - tau_out[i] = j.S.dot(f[i]); - if (j.parent >= 0) { f[j.parent] += Xup[i].transpose() * f[i]; } - } - } - - // Main function to compute inverse dynamics (joint torques) given joint states and accelerations using the Recursive Newton-Euler Algorithm (RNEA) - mathlib::VecX SpatialDynamics::RNEA( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& qdd, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - - // TODO Remove these temp scratches AFTER debugging - std::vector v(n); - std::vector Xup(n); - std::vector c(n); - std::vector a(n); - mathlib::VecX tau; - - // Compute spatial velocities and transforms - computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); - // Compute spatial accelerations - computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); - // Compute inverse dynamics (joint torques) - computeBackwardForces_RNEA(model, Xup, v, a, tau); - - return tau; - } - - // Main function to compute the mass matrix of the robot at a given configuration using the Composite Rigid Body Algorithm (CRBA) - mathlib::MatX SpatialDynamics::CRBA( - const SpatialModel& model, - const std::vector& Xup, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - scratch.dense.M.setZero(n, n); - std::vector Ic(n); // spatial inertia for each link - - // Initialise spatial inertia for each link based on the robot model - for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } - - // Upward pass: propagate spatial inertia from child links to parent joints - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - int p = j.parent; - if (p >= 0) { - Ic[p] += Xup[i].transpose() * Ic[i] * Xup[i]; - } - } - - // Downward pass: compute mass matrix contributions for each joint - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - SpatialVec F = Ic[i] * j.S; - scratch.dense.M(i, i) = j.S.dot(F); - - int jIdx = (int)i; - while (model.joints[jIdx].parent >= 0) { - int p = model.joints[jIdx].parent; - F = Xup[jIdx].transpose() * F; - scratch.dense.M(i, p) = model.joints[p].S.dot(F); - scratch.dense.M(p, i) = scratch.dense.M(i, p); - jIdx = p; - } - } - return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) - } - - // Recursive function to compute articulated body inertias and bias forces using the Articulated Body Algorithm (ABA) - void SpatialDynamics::computeArticulatedBodies_ABA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& v, - const std::vector& c, - const mathlib::VecX& tau, - std::vector& IA_out, - std::vector& pA_out, - std::vector& Ia_out, - mathlib::VecX& u_out, - mathlib::VecX& d_out, - std::vector& U_out - ) { - const size_t n = model.joints.size(); - - // Resize scratch buffers - IA_out.resize(n); - pA_out.resize(n); - Ia_out.resize(n); - U_out.resize(n); - u_out.resize(n); - d_out.resize(n); - - // Upward pass: compute articulated body inertias and bias forces - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - - if (j.type == eJointType::FIXED) { - Ia_out[i] = IA_out[i]; - if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; - } - continue; - } - - U_out[i] = IA_out[i] * j.S; - d_out[i] = dot(j.S, U_out[i]); - if (std::abs(d_out[i]) < 1e-12) { d_out[i] = 1e-12; } // Regularisation to avoid singularities - - u_out[i] = tau[i] - dot(j.S, pA_out[i]); - Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; - - // pA = pA + Ia * c + U * (u/d) - pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); - - if (j.parent >= 0) { - IA_out[j.parent] += Xup[i].transpose() * Ia_out[i] * Xup[i]; - pA_out[j.parent] += Xup[i].transpose() * pA_out[i]; - } - } - } - - // Recursive function to compute joint accelerations using the Articulated Body Algorithm (ABA) - void SpatialDynamics::computeAccelerations_ABA( - const SpatialModel& model, - const std::vector& Xup, - const std::vector& c, - const mathlib::VecX& u_out, - const mathlib::VecX& d_out, - const std::vector& U, - const SpatialVec& a0, - std::vector& a_out, - mathlib::VecX& qdd_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - qdd_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } - else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } - - if (j.type == eJointType::FIXED) { - qdd_out[i] = 0.0; - continue; - } - - qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; - a_out[i] += j.S * qdd_out[i]; - } - } - - // Main function to compute joint accelerations given joint states and torques using the Articulated Body Algorithm (ABA) - mathlib::VecX SpatialDynamics::ABA( - const SpatialModel& model, - const mathlib::VecX& q, - const mathlib::VecX& qd, - const mathlib::VecX& tau, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - VecX qdd = VecX::Zero(n); - - SpatialVec a0; // base acceleration (gravity) - a0.v << scratch.g.segment<3>(0), scratch.g.segment<3>(3); - - computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - for (size_t i = 0; i < n; ++i) { - scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia - scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); - } - - // Compute articulated body inertias and bias forces - computeArticulatedBodies_ABA( - model, scratch.spatial.Xup, - scratch.spatial.v, scratch.spatial.c, tau, - scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U - ); - - // Compute joint accelerations using the articulated body algorithm - computeAccelerations_ABA( - model, scratch.spatial.Xup, scratch.spatial.c, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, - a0, scratch.spatial.a,qdd - ); - - return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp b/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp index 0a40036d..ce546e2b 100644 --- a/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp +++ b/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp @@ -1,13 +1,11 @@ +// DSFE_Core TrajectoryManager.cpp #include "pch.h" -// File: TrajectoryManager.cpp -// GitHub: SaltyJoss #include "Robots/TrajectoryManager.h" #include "Robots/RobotSystem.h" -#include "Control/IJointTrajectory.h" -#include -#include -#include +#include +#include +#include namespace control { // Clear trajectory for a specific robot link @@ -17,7 +15,7 @@ namespace control { void TrajectoryManager::clearAll() { _active.clear(); } // Evaluate the trajectory for a specific robot link at time t, returning the desired state in out - bool TrajectoryManager::tryEval(const std::string& link, double t, control::TrajState& out) const { + bool TrajectoryManager::tryEval(const std::string& link, double t, control::TrajState& out) const { auto it = _active.find(link); if (it == _active.end()) { return false; } const auto r = it->second->eval(t); diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 91c7817a..c7f9880b 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -5,6 +5,7 @@ #include "Robots/RobotSystem.h" #include "Robots/RobotModel.h" #include "Robots/TrajectoryManager.h" +#include "SingleBodySystem/Body.h" #include "Interpreter/StoredProgram.h" #include "Interpreter/Parser.h" @@ -15,10 +16,12 @@ namespace core { // Owned constructed subsystems (default) SimulationCore::SimulationCore() - : _trajOwned(std::make_unique()), _robotOwned(std::make_unique()) + : _trajOwned(std::make_unique()), _robotOwned(std::make_unique()), + _singleBodyOwned(std::make_unique()) { _traj = _trajOwned.get(); _robot = _robotOwned.get(); + _singleBody = _singleBodyOwned.get(); startExportThread(); } @@ -30,32 +33,68 @@ namespace core { // Non-owning constructor (used when subsystems are managed externally, e.g. by the SimulationManager) SimulationCore::SimulationCore(robots::RobotSystem& robot, control::TrajectoryManager& traj) - : _robot(&robot), _traj(&traj) { + : _robot(&robot), _traj(&traj), _singleBody(nullptr) { startExportThread(); } // Simulation System void SimulationCore::setupSimulationIntegrator() { - if (!_robot) return; - auto* intgr = _robot->getIntegrator(); + if (!_robot && !_singleBody) { return; } + integration::IntegrationService* intgr; + integration::DifferentiableIntegrator* adIntgr; + if (_robot) { + intgr = _robot->getIntegrator(); + adIntgr = _robot->getADIntegrator(); + } + if (_singleBody) { + intgr = _singleBody->getIntegrator(); + adIntgr = _singleBody->getADIntegrator(); + } intgr->resetAdaptiveState(); intgr->setAdaptiveTolerances(1e-3, 1e-6); intgr->setMaxStep(_dt); + adIntgr->runtimeState()->last_dt_taken = _dt; + adIntgr->runtimeState()->last_dt_sug = _dt; } // Set the integration method for the simulation (also updates the robot's integrator if it exists) void SimulationCore::setIntegrationMethod(integration::eIntegrationMethod method) { - if (!_robot) { return; } - _robot->getIntegrator()->setIntegrationMethod(method); + if (!_robot && !_singleBody) { return; } + if (_singleBody) { _singleBody->setStandardIntegrator(method); } + if (_robot) { _robot->setStandardIntegrator(method); } + } + // Set the auto-diff integration method for the simulation (also updates the robot's AD integrator if it exists) + void SimulationCore::setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { + if (!_robot && !_singleBody) { return; } + if (_singleBody) { _singleBody->setADIntegrator(method); } + if (_robot) { _robot->setADIntegrator(method); } } // Get the name of the current integration method (returns "no_robot" if no robot is loaded) std::string SimulationCore::integrationMethodName() const { - if (!_robot) { return "no_robot"; } - return _robot->getIntegratorName(); + std::string intName; + if (_robot) { + if (_robot->autoDiffEnabled()) { intName = _robot->getIntegratorName(); } + else { intName = _robot->AD_integratorName(); } + } + else if(_singleBody) { intName = _singleBody->getIntegratorName(); } + else { intName = "no_system"; } + LOG_INFO("Integration Method: %s", intName.c_str()); + return intName; } // Get the current integration method integration::eIntegrationMethod SimulationCore::integrationMethod() const { - if (!_robot) { return integration::eIntegrationMethod::RK4; } - return _robot->getIntegrator()->getIntegrationMethod(); + if (_robot) { return _robot->getIntegrationMethod(); } + if (_singleBody) { return _singleBody->getIntegrationMethod(); } + return integration::eIntegrationMethod::RK4; + } + // Get the current auto-diff integration method + integration::eAutoDiffIntegrationMethod SimulationCore::autoDiffIntegrationMethod() const { + if (_robot) { return _robot->AD_IntegrationMethod(); } + if (_singleBody) { return _singleBody->AD_IntegrationMethod(); } + return integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler; + } + void SimulationCore::enableAutoDiff(bool enable) { + if (!_robot) { return; } + _robot->enableAutoDiff(enable); } // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time @@ -91,6 +130,8 @@ namespace core { _scriptRunning.store(false); } + const auto state = _robot->runtimeIntegratorState(); + // Update physics and robot system if sim is running if (_simRunning.load()) { simTime += _dt; @@ -107,6 +148,7 @@ namespace core { } _telemetry.update(simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); } + if (hasSingleBody()) { _singleBody->step(_dt, simTime); } } _accum -= _dt; // decrease accumulator by fixed timestep until we catch up to the current frame time } @@ -124,10 +166,13 @@ namespace core { if (_simRunning.load()) { return; } telemetry().clear(); D_RUNTIME("starting simulation"); + LOG_INFO("Starting Simulation -> Debug Log"); _simTime.store(0.0, std::memory_order_relaxed); _accum = 0.0; + std::string intName; + // Reset simulation system if (_robot) { _robot->resetRobot(); @@ -153,13 +198,21 @@ namespace core { _trajRefBuffer.clear(); _trajRefBuffer.reserve(std::max(1024, total / (26 / 5))); // 26 to 5 entries, so reserving 1/(26/5) of total steps as a heuristic for ref buffer size _robot->setRefBuffer(&_trajRefBuffer); - } + const auto state = _robot->runtimeIntegratorState(); + intName = (_robot->autoDiffEnabled()) ? _robot->AD_integratorName() : _robot->getIntegratorName(); + } + if (_singleBody) { + _singleBody->resetBody(); + const auto state = _singleBody->runtimeIntegratorState(); + intName = (_singleBody->autoDiffEnabled()) ? _singleBody->AD_integratorName() : _singleBody->getIntegratorName(); + } + _data.setParentFolder(paths::runs().string()); // Ensure reference sim system have their integrators configured for the new run setupSimulationIntegrator(); - _data.setIntegratorName(integrationMethodName()); + _data.setIntegratorName(intName); _data.setRunTag(_runTag); _simRunning.store(true); @@ -189,7 +242,8 @@ namespace core { void SimulationCore::exportLogsToHDF5(const robots::JointLogBuffer& exportBuf) { auto t0 = std::chrono::steady_clock::now(); - const std::string intName = _robot->getIntegratorName(); + const auto state = _robot->runtimeIntegratorState(); + const std::string intName = (state && state->autoDiff) ? _robot->AD_integratorName() : _robot->getIntegratorName(); const std::string robotName = _robot->hasRobot() ? _robot->robotName() : "no_robot"; const std::string header = robotName + "_sim_" + intName; @@ -267,38 +321,46 @@ namespace core { _trajRefBuffer.clear(); // Inject reference buffer only _robot->setRefBuffer(&_trajRefBuffer); + // Set integrator on both physics and robot systems + _robot->setStandardIntegrator(method); + + scriptParallelisation(program); + + D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", methodName.c_str(), _simTime.load(), _telemetry.ring.size()); + LOG_INFO("SimulationCore::runScriptToCompletion -> END method=%s result=%d simTime=%.6f samples=%zu", methodName.c_str(), (int)(_telemetry.ring.size() >= 2), _simTime.load(), _telemetry.ring.size()); + return (_telemetry.ring.size() >= 2); + } + void SimulationCore::scriptParallelisation(interpreter::IStoredProgram* program) { // Reset simulation state _simTime.store(0.0, std::memory_order_relaxed); _simRunning.store(false); _telemetryBegun = false; _accum = 0.0; - // Set integrator on both physics and robot systems - _robot->setIntegrationMethod(method); - _activeProgram = program; _scriptRunning.store(true); // Set run mode to synchronous for the duration of this run _runMode = eRunMode::Synchronous; + const double dt = _dt; + const int maxSteps = static_cast((24.0 * 3600.0) / dt); // safety to prevent infinite loops in faulty scripts (max 24 hours of sim time) + // enable sim stepping and telemetry for synchronous run startSimulation(); LOG_INFO("SimulationCore::runScriptToCompletion -> startSimulation called; simRunning=%d simTime=%.6f", (int)_simRunning, _simTime.load()); // Run tight simulation loop until program completes - const double dt = _dt; double simTime = _simTime.load(); - const int maxSteps = static_cast((24.0 * 3600.0) / dt); // safety to prevent infinite loops in faulty scripts (max 24 hours of sim time) // Main loop: step the program and simulation until completion for (int step = 0; step < maxSteps; ++step) { // Check program completion if (program->isCompleted() || program->isFaulted() || program->isStopped()) { - LOG_INFO("SimulationCore::runScriptToCompletion -> program end detected at step=%d completed=%d faulted=%d stopped=%d", step, (int)program->isCompleted(), (int)program->isFaulted(), (int)program->isStopped()); - break; + LOG_INFO("SimulationCore::runScriptToCompletion -> program end detected at step=%d completed=%d faulted=%d stopped=%d", step, (int)program->isCompleted(), (int)program->isFaulted(), (int)program->isStopped()); + break; } // Step the program (DSL command execution) @@ -307,21 +369,16 @@ namespace core { // Step physics and robot if sim is running if (_simRunning.load()) { simTime += dt; - if (hasRobot()) { // Update Trajectory Inputs _robot->updateTrajectoryInputs(*_traj, simTime); - - // Step robot system _robot->step(dt, simTime); - // Telemetry beginRun if (!_telemetryBegun) { _telemetry.beginRun(simTime, _telHz, 300.0); _telemetryBegun = true; D_INFO_ONCE("Telemtry Capture Started (dt=%.6f s, simTime=%.3f s)", (1 / _telHz), simTime); } - // Telemetry update _telemetry.update(simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); } @@ -334,7 +391,7 @@ namespace core { else { _simTime.store(0.0); } - + // Clean up stopSimulation(); @@ -346,10 +403,6 @@ namespace core { _scriptRunning.store(false); _simRunning.store(false); _telemetryBegun = false; - - D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", methodName.c_str(), _simTime.load(), _telemetry.ring.size()); - LOG_INFO("SimulationCore::runScriptToCompletion -> END method=%s result=%d simTime=%.6f samples=%zu", methodName.c_str(), (int)(_telemetry.ring.size() >= 2), _simTime.load(), _telemetry.ring.size()); - return (_telemetry.ring.size() >= 2); } // Setter for fixed timestep duration @@ -380,12 +433,31 @@ namespace core { loadRobotInternal(name); _robotPresentationDirty = true; } - + // Internal method to load a robot, assumes ownership of the robot system void SimulationCore::loadRobotInternal(const std::string& name) { if (!_robot) { LOG_ERROR("Cannot load robot: RobotSystem not set"); return; } _robot->loadRobot(name); } + // Accessor for the single body system (non-const and const versions) + single_body_system::SingleBodySystem* SimulationCore::singleBodySystem() { return _singleBody; } + const single_body_system::SingleBodySystem* SimulationCore::singleBodySystem() const { return _singleBody; } + + // Setter and checker for Single Body System + void SimulationCore::setSingleBodySystem(single_body_system::SingleBodySystem* singleBody) { _singleBody = singleBody; } + bool SimulationCore::hasSingleBody() const { return _singleBody && _singleBody->hasBody(); } + + // Loads a single body into the single body system by name + void SimulationCore::loadSingleBody(const std::string& name) { + loadSingleBodyInternal(name); + _singleBodyPresentationDirty = true; + } + // Internal method to load a single body, assumes ownership of the single body system + void SimulationCore::loadSingleBodyInternal(const std::string& name) { + if (!_singleBody) { LOG_ERROR("Cannot load single body: SingleBodySystem not set"); return; } + _singleBody->loadBody(name); + } + // Setter for the trajectory manager void SimulationCore::setTrajectoryManager(control::TrajectoryManager* traj) { _traj = traj; } diff --git a/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp new file mode 100644 index 00000000..04a560e1 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/SingleBodySystem/Body.cpp @@ -0,0 +1,111 @@ +// DSFE_CORE Body.cpp +#include "pch.h" +#include "SingleBodySystem/Body.h" +#include "EngineLib/LogMacros.h" + +namespace single_body_system { + SingleBodySystem::SingleBodySystem() + : _body(new Body()), _dynamics(std::make_unique()), + _integrator(std::make_unique()), _AD_integrator(std::make_unique()) + { + } + + mathlib::VecX SingleBodySystem::packState() const { + mathlib::VecX x(13); + x.segment<3>(0) = _body->state.p; // position + x.segment<3>(3) = _body->state.pd; // linear velocity + x.segment<4>(6) = _body->state.q.coeffs(); + x.segment<3>(10) = _body->state.w; // angular velocity + + return x; + } + + void SingleBodySystem::unpackState(const mathlib::VecX& x) { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + if (x.size() != 13) { LOG_ERROR("State vector size mismatch! Expected 13, got %d", (int)x.size()); return; } + + if (_clampVel.size() != 3) { _clampVel.resize(3, 0); } + if (_clampAngVel.size() != 3) { _clampAngVel.resize(3, 0); } + + auto& p_in = x.segment<3>(0); + auto& pd_in = x.segment<3>(3); + auto& w_in = x.segment<3>(10); + + for (int i = 0; i < 3; ++i) { + double pd_i = pd_in[i]; // Store the original value before clamping + double w_i = w_in[i]; // Store the original value before clamping + + double pdMax_hpd = std::abs(_body->limits.pdMax[i]); + double pd_i_out = pd_i; + double wMax_hw = std::abs(_body->limits.wMax[i]); + double w_i_out = w_i; + + const double eps = 0.05; // 5 % tolerance for clamping + // TODO: Check the correct way to clamp the velocities close the S.o.L + // * Not really sure if I have done this correctly + if (pdMax_hpd > 0.0) { + if (pdMax_hpd == constants::c_0) { pd_i_out = std::clamp(pd_i, -pdMax_hpd, pdMax_hpd); } + else if (std::abs(pd_i) > (1.0 + eps) * pdMax_hpd) { pd_i_out = std::clamp(pd_i, -pdMax_hpd, pdMax_hpd); } + } + if (wMax_hw > 0.0) { + if (wMax_hw == constants::c_0) { w_i_out = std::clamp(w_i, -wMax_hw, wMax_hw); } + else if (std::abs(w_i) > (1.0 + eps) * wMax_hw) { w_i_out = std::clamp(w_i, -wMax_hw, wMax_hw); } + } + + _clampVel[i] = (pd_i_out != pd_i) ? 1 : 0; + _clampAngVel[i] = (w_i_out != w_i) ? 1 : 0; + + _body->state.p[i] = p_in[i]; + _body->state.pd[i] = pd_i_out; + _body->state.w[i] = w_i_out; + } + _body->state.q.coeffs() = x.segment<4>(6); + + + if (_clampVel[0] || _clampVel[1] || _clampVel[2]) { LOG_WARN("Linear velocity clamping applied: pd = [%f, %f, %f]", pd_in[0], pd_in[1], pd_in[2]); } + if (_clampAngVel[0] || _clampAngVel[1] || _clampAngVel[2]) { LOG_WARN("Angular velocity clamping applied: w = [%f, %f, %f]", w_in[0], w_in[1], w_in[2]); } + } + + // Step the simulation forward by dt seconds at time t + void SingleBodySystem::step(double dt, double t) { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + if (!_dynamics) { LOG_ERROR("Dynamics system not initialised!"); return; } + + _simTime = t; + VecX x = packState(); + // Not going to use the computeDynamics methods initially, just want a straight cut test first. + auto f_deriv = [&](auto /*t*/, const auto& x) { return _dynamics->derivatives(*_body, x, _F_ext, _tau_ext); }; + auto f_jac = [&](const auto& x, auto& J_out) { _dynamics->jacobian(*_body, x, J_out); }; + auto step = _integrator->step(_curIntMethod, x, t, dt, f_deriv, f_jac); // Wont work with Implicit since no jacobian provided, but will work with RK4 and other explicit methods + VecX x_next = step.x_next; + + unpackState(x); + + for (int i = 0; i < x_next.size(); ++i) { + const auto v = mathlib::real(x_next[i]); + if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } + } + } + + void SingleBodySystem::loadBody(const std::string& name) { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + _body->name = name; + } + + void SingleBodySystem::resetBody() { + if (!_body) { LOG_ERROR("Body not initialised!"); return; } + _body->state.p = mathlib::Vec3::Zero(); + _body->state.pd = mathlib::Vec3::Zero(); + _body->state.q = mathlib::Quat::Identity(); + _body->state.w = mathlib::Vec3::Zero(); + } + + integration::IntegrationService* SingleBodySystem::getIntegrator() { return _integrator.get(); } + const integration::IntegrationService* SingleBodySystem::getIntegrator() const { return _integrator.get(); } + + integration::DifferentiableIntegrator* SingleBodySystem::getADIntegrator() { return _AD_integrator.get(); } + const integration::DifferentiableIntegrator* SingleBodySystem::getADIntegrator() const { return _AD_integrator.get(); } + + std::shared_ptr SingleBodySystem::runtimeIntegratorState() { return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); } + std::shared_ptr SingleBodySystem::runtimeIntegratorState() const { return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/CMakeLists.txt b/DSFE_App/DSFE_Engine/CMakeLists.txt index f68e79b6..aa9b4743 100644 --- a/DSFE_App/DSFE_Engine/CMakeLists.txt +++ b/DSFE_App/DSFE_Engine/CMakeLists.txt @@ -1,7 +1,4 @@ # CMakeLists.txt for the DSFE_Engine executable -cmake_minimum_required(VERSION 3.10) -project(DSFE_Engine LANGUAGES C CXX) - find_package(Threads REQUIRED) add_executable(DSFE_Engine @@ -14,17 +11,26 @@ target_include_directories(DSFE_Engine PRIVATE ) target_link_libraries(DSFE_Engine PRIVATE - DSFE_GUI DSFE_Core Threads::Threads ) +if (DSFE_ENABLE_GUI) + target_link_libraries(DSFE_Engine PRIVATE DSFE_GUI) + target_compile_definitions(DSFE_Engine PRIVATE DSFE_ENABLE_GUI) +endif() + add_custom_command(TARGET DSFE_Engine POST_BUILD - COMMAND ${CMAKE_COMMAND} -E copy_if_different - "$" + COMMAND ${CMAKE_COMMAND} -E make_directory "$" + + COMMAND $<$>:${CMAKE_COMMAND}> + $<$>:-E> + $<$>:copy_if_different> + $<$>:$> + $<$>:$> + COMMAND_EXPAND_LISTS - COMMENT "Copying runtime DLLs..." ) # Defines the source and destination for assets and configs diff --git a/DSFE_App/DSFE_Engine/assets/DSLScripts/Test_Suite/vispa_target_sat_intercept_servicing.dsl b/DSFE_App/DSFE_Engine/assets/DSLScripts/Test_Suite/vispa_target_sat_intercept_servicing.dsl new file mode 100644 index 00000000..1bf27b07 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/DSLScripts/Test_Suite/vispa_target_sat_intercept_servicing.dsl @@ -0,0 +1,112 @@ +# -------------------------------------------- +# MISSION: Airbus VISPA => Satellite Interception & Abort (Aggressive reversals) +# -------------------------------------------- +# +# MISSION PROFILE +# --------------- +# 1. System Initialization & Calibration Check (0.0s - 5.0s) +# 2. Nominal Deployment to Ready-State Observer Position (5.0s - 35.0s) +# 3. Proximity Close-Approach Synchronisation (35.0s - 95.0s) +# 4. Multi-Point Surface Inspection (95.0s - 215.0s) +# 5. Synchronised Contact & Stabilisation Hold (215.0s - 275.0s) +# 6. Secure Safe-State Stow & Mission Completion (275.0s - 335.0s) +# +# JOINT LIMITS +# ------------ +# j1-j6: +/- ~180 deg (+/-3.14149 rad) +# v_max: 5.38 deg/s (0.0940 rad/s) hardware limit +# v_operating: < 1.5 deg/s +# Q_max: 50 Nm per joint +# Damping / Friction: 0.2 / 0.05 (all joints) +# +# LINK MASSES (kg) +# ---------------- +# link00 0.627 base adapter (gold coloured) +# link01 2.328 shoulder yaw +# link02 3.995 upper arm (0.8m, heaviest link) +# link03 2.328 elbow +# link04 3.157 forearm (0.65m) +# link05 2.695 wrist roll +# link06 0.924 end-effector flange +# +# Create By: Joss Salton +# GitHub: SaltyJoss +# +# -------------------------------------------- + +load(robot, VISPA) + +wait(4.0) +trajClear() +wait(1.0) + +# Begins sim run and logging. +start() + +# Nominal Deployment +parallel(30.0) { + trajSet(link01, TRAP, -30.0, 1.5, 5.0) + trajSet(link02, TRAP, 45.0, 2.0, 5.0) + trajSet(link03, TRAP, -90.0, 2.0, 5.0) + trajSet(link04, TRAP, 0.0, 1.0, 3.0) + trajSet(link05, TRAP, 45.0, 1.5, 4.0) + trajSet(link06, TRAP, 0.0, 1.0, 3.0) +} + +wait(30.0) + +# Close-approach synchronisation +parallel(60.0) { + trajSet(link01, TRAP, -45.0, 0.5, 10.0) + trajSet(link02, TRAP, 55.0, 0.4, 10.0) + trajSet(link03, TRAP, -80.0, 0.4, 10.0) + trajSet(link04, TRAP, 10.0, 0.3, 8.0) + trajSet(link05, TRAP, 30.0, 0.5, 10.0) + trajSet(link06, TRAP, 15.0, 0.5, 8.0) +} + +wait(60.0) + +# Multipoint surface inspection +parallel(120.0) { + trajSet(link01, MSINE, 120.0, -45.0, 4.0, 0.002, 0.0, 2.0, 0.004, 90.0) + trajSet(link02, MSINE, 120.0, 55.0, 3.0, 0.001, 0.0, 1.5, 0.003, 45.0) + trajSet(link03, MSINE, 120.0, -80.0, 3.0, 0.002, 0.0, 1.0, 0.004, 120.0) + trajSet(link04, MSINE, 120.0, 10.0, 2.5, 0.003, 0.0, 1.0, 0.005, 60.0) + trajSet(link05, MSINE, 120.0, 30.0, 2.0, 0.004, 0.0, 1.0, 0.006, 90.0) + trajSet(link06, MSINE, 120.0, 15.0, 2.0, 0.005, 0.0, 1.0, 0.007, 45.0) +} + +wait(120.0) + +# Contact, docking, and servicing lock +parallel(60.0) { + trajSet(link01, TRAP, -20.0, 0.8, 8.0) + trajSet(link02, TRAP, 30.0, 0.8, 8.0) + trajSet(link03, TRAP, -100.0, 0.8, 8.0) + trajSet(link04, TRAP, 0.0, 0.5, 8.0) + trajSet(link05, TRAP, 60.0, 0.8, 8.0) + trajSet(link06, TRAP, 0.0, 0.5, 8.0) +} + +wait(60.0) + +# Safe-state return +parallel(60.0) { + trajSet(link01, TRAP, 0.0, 1.0, 10.0) + trajSet(link02, TRAP, 0.0, 1.0, 10.0) + trajSet(link03, TRAP, 0.0, 1.0, 10.0) + trajSet(link04, TRAP, 0.0, 1.0, 10.0) + trajSet(link05, TRAP, 0.0, 1.0, 10.0) + trajSet(link06, TRAP, 0.0, 1.0, 10.0) +} + +wait(60.0) + +trajClear() +wait(1.0) + +stop() +# -------------------------------------------- +# END -> ~60 seconds +# -------------------------------------------- \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json b/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json index f0d18fea..2c337eb5 100644 --- a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json +++ b/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json @@ -265,8 +265,10 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05, + "wn_target": 4.0, + "zeta_target": 0.8 } }, @@ -287,8 +289,10 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05, + "wn_target": 4.0, + "zeta_target": 0.8 } }, @@ -309,8 +313,10 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05, + "wn_target": 2.5, + "zeta_target": 1.0 } }, @@ -331,8 +337,10 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05, + "wn_target": 2.5, + "zeta_target": 1.0 } }, @@ -353,8 +361,10 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05, + "wn_target": 1.5, + "zeta_target": 1.3 } }, @@ -375,8 +385,10 @@ "effort": 50 }, "dynamics": { - "damping": 0.0, - "friction": 0.0 + "damping": 0.2, + "friction": 0.05, + "wn_target": 1.5, + "zeta_target": 1.3 } } ] diff --git a/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl b/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl new file mode 100644 index 00000000..51d0d135 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/shaders/present.frag.glsl @@ -0,0 +1,11 @@ +#version 460 core + +in vec2 uv; + +out vec4 FragColour; + +uniform sampler2D screenTexture; + +void main() { + FragColour = texture(screenTexture, uv); +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/src/Main.cpp b/DSFE_App/DSFE_Engine/src/Main.cpp index 39994ac8..c477399a 100644 --- a/DSFE_App/DSFE_Engine/src/Main.cpp +++ b/DSFE_App/DSFE_Engine/src/Main.cpp @@ -4,10 +4,13 @@ #include #include #include -#include #include "BatchEntry.h" #include "BatchArgs.h" +#ifdef DSFE_ENABLE_GUI + #include +#endif + // Helper function to split a comma-separated string into a vector of strings, trimming whitespace static std::vector splitComma(const std::string& input) { std::vector result; @@ -46,41 +49,47 @@ int main(int argc, char** argv) { batchMode = true; break; } + else if (arg == "--about") { + std::cout + << "DSFE (Dynamic Systems Framework Engine)\n" + << " > A research-focused simulation engine for numerically modelling dynamic systems with different integration methods (explicit and implicit).\n" + << " > Version 0.9.1-alpha\n" + << " > Developed by Joss Salton\n"; + return 0; + } + else if (arg == "--help" || arg == "-h") { + std::cout + << "Usage:\n" + << " --batch -t