From 0b4371d6bf04abd1ef8e7a802a3eeabb8e513542 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Wed, 18 Feb 2026 16:20:18 +0000 Subject: [PATCH 01/20] fixes --- Sim_Engine/EngineLib/include/Platform/imguiWidgets.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h b/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h index b387786..f8be6cb 100644 --- a/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h +++ b/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h @@ -194,7 +194,7 @@ namespace ImGui { // Limits for k to prevent unreasonable values const int MIN_K = 1; - const int MAX_K = isTelem ? 10 : 100; + const int MAX_K = isTelem ? 10 : 200; // Format denominator text char denomBuf[32]; From 170e33988362b027b550d8af929167f778f181c0 Mon Sep 17 00:00:00 2001 From: Joss Salton <145990319+SaltyJoss@users.noreply.github.com> Date: Fri, 20 Feb 2026 00:14:20 +0000 Subject: [PATCH 02/20] Revise Todo List format and item status Updated the Todo List with checkboxes and corrected a strikethrough item. --- README.md | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index f74d36b..06afccb 100644 --- a/README.md +++ b/README.md @@ -86,21 +86,21 @@ Download the latest release of DSFE from `Release` in the `DSFE GitHub Repositor > Therefore, I most likely will not make any updates until that period is over.
### Todo List: - * Implement `collision meshes` with existin dynamics pipeline - * Improve UI, making it `Research-Oriented` - * Add workspace layouts - * Support multiple concurrent sessions - * Move simulation data output to a `dedicated Data-specific thread` **(IMPORTANT)** - * Replace the current diagonal Coriolis approximation to the `full Coriolis matrix formulation` - * Support `multiple articulated systems` within a single simulation instance - * Extend DSFE to support `other classes of dynamical systems` outside robotic manipulators - * Further `extend physcial modelling` for different robot models (humanoid, legged) - * Seperate physics/mathematics core from the GUI and visualisation layers - * Explore migration to a `CMake-only` build system - * Explore `DX11` and `Vulkan` alternatives - * Get DSFE to work on `Linux` - * Explore `non-x64 systesm` support - * Explore the adoption of `standardised URDF XML` in place of the current DSFE json format + * [ ] Implement `collision meshes` with existin dynamics pipeline + * [ ] Improve UI, making it `Research-Oriented` + * [ ] Add workspace layouts + * [ ] Support multiple concurrent sessions + * [ ] Move simulation data output to a `dedicated Data-specific thread` **(IMPORTANT)** + * [x] Replace the current diagonal model with the standarised `full-matrix rigid-body model` + * [ ] Support `multiple articulated systems` within a single simulation instance + * [ ] Extend DSFE to support `other classes of dynamical systems` outside robotic manipulators + * [ ] Further `extend physcial modelling` for different robot models (humanoid, legged) + * [ ] Seperate physics/mathematics core from the GUI and visualisation layers + * [ ] Explore migration to a `CMake-only` build system + * [ ] Explore `DX11` and `Vulkan` alternatives + * [ ] Get DSFE to work on `Linux` + * [ ] Explore `non-x64 systesm` support + * [ ] Explore the adoption of `standardised URDF XML` in place of the current DSFE json format
From c2d9e8a24e28e6e11af57a3c61f51376ee03a3ce Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 20 Feb 2026 11:08:46 +0000 Subject: [PATCH 03/20] fixes --- Sim_Engine/EngineLib/include/Platform/imguiWidgets.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h b/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h index f8be6cb..b36bd98 100644 --- a/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h +++ b/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h @@ -194,7 +194,7 @@ namespace ImGui { // Limits for k to prevent unreasonable values const int MIN_K = 1; - const int MAX_K = isTelem ? 10 : 200; + const int MAX_K = isTelem ? 10 : 800; // Format denominator text char denomBuf[32]; From 29f18ab3862e2b12a97ca9fa15d7f6b6d5c2f620 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 20 Feb 2026 11:09:12 +0000 Subject: [PATCH 04/20] fixes --- Sim_Engine/EngineLib/include/Platform/imguiWidgets.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h b/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h index b36bd98..e09cefb 100644 --- a/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h +++ b/Sim_Engine/EngineLib/include/Platform/imguiWidgets.h @@ -194,7 +194,7 @@ namespace ImGui { // Limits for k to prevent unreasonable values const int MIN_K = 1; - const int MAX_K = isTelem ? 10 : 800; + const int MAX_K = isTelem ? 20 : 800; // Format denominator text char denomBuf[32]; From 7ff691959e8b0ff03ad6fcda3632886db79dd8a8 Mon Sep 17 00:00:00 2001 From: Joss Salton <145990319+SaltyJoss@users.noreply.github.com> Date: Sat, 21 Feb 2026 01:01:26 +0000 Subject: [PATCH 05/20] Refactor of RobotSystem.cpp (#72) * feat: move robot kinematic and dynamic logic to seperate headers and compiler files respectively * fixes: fixed previous files to correctly function with new abstraction NOTES: - This whole refactor allowed me to reduce the size of robot system and actually create a better pipeline for the physics/mathematics backend. - In the future I aim to move most methods to mathlib, with general logic applicable to various systems, but until I safely KNOW how to do that, I decided thsi was still a good way to abstract the actual robotic system physics logic from the visual/simulator * fixes: fixed previous files to correctly function with new abstraction NOTES: - This whole refactor allowed me to reduce the size of robot system and actually create a better pipeline for the physics/mathematics backend. - In the future I aim to move most methods to mathlib, with general logic applicable to various systems, but until I safely KNOW how to do that, I decided thsi was still a good way to abstract the actual robotic system physics logic from the visual/simulator --- MathLib/MathLib/include/core/Utils.h | 63 +- Sim_Engine/EngineLib/EngineLib.vcxproj | 5 + .../EngineLib/EngineLib.vcxproj.filters | 15 + .../EngineLib/include/Robots/RobotCommon.h | 15 + .../EngineLib/include/Robots/RobotDynamics.h | 96 +++ .../include/Robots/RobotKinematics.h | 45 ++ .../EngineLib/include/Robots/RobotSystem.h | 27 +- .../EngineLib/src/Robots/RobotDynamics.cpp | 510 ++++++++++++++ .../EngineLib/src/Robots/RobotKinematics.cpp | 99 +++ .../EngineLib/src/Robots/RobotSystem.cpp | 635 ++---------------- Sim_Engine/EngineLib/src/ui/ControlPanel.cpp | 1 + .../External/MathLib/Release/MathLib.dll | Bin 61952 -> 61952 bytes .../External/MathLib/include/core/Utils.h | 93 +-- 13 files changed, 930 insertions(+), 674 deletions(-) create mode 100644 Sim_Engine/EngineLib/include/Robots/RobotCommon.h create mode 100644 Sim_Engine/EngineLib/include/Robots/RobotDynamics.h create mode 100644 Sim_Engine/EngineLib/include/Robots/RobotKinematics.h create mode 100644 Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp create mode 100644 Sim_Engine/EngineLib/src/Robots/RobotKinematics.cpp diff --git a/MathLib/MathLib/include/core/Utils.h b/MathLib/MathLib/include/core/Utils.h index 16e2415..e4e4d4b 100644 --- a/MathLib/MathLib/include/core/Utils.h +++ b/MathLib/MathLib/include/core/Utils.h @@ -1,5 +1,4 @@ #pragma once -#pragma warning(disable : 4251) #include "MathLibAPI.h" #include "core/Types.h" @@ -11,21 +10,21 @@ using namespace constants; namespace mathlib { // Converts a non-eigen vector to Vec3 (if it has 3 elements) template - Vec3 toVec3(const T& vec) { + inline Vec3 toVec3(const T& vec) { static_assert(std::tuple_size::value == 3, "Input vector must have exactly 3 elements"); return Vec3(vec[0], vec[1], vec[2]); } // Converts a non-eigen vector to Vec4 (if it has 4 elements) template - Vec4 toVec4(const T& vec) { + inline Vec4 toVec4(const T& vec) { static_assert(std::tuple_size::value == 4, "Input vector must have exactly 4 elements"); return Vec4(vec[0], vec[1], vec[2], vec[3]); } // Converts a non-eigen 3x3 matrix to Mat3 (if it has 3 rows and 3 columns) template - Mat3 toMat3(const T& mat) { + inline Mat3 toMat3(const T& mat) { static_assert(std::tuple_size::value == 3 && std::tuple_size::value == 3, "Input matrix must be 3x3"); return Mat3(mat[0][0], mat[0][1], mat[0][2], mat[1][0], mat[1][1], mat[1][2], @@ -35,7 +34,7 @@ namespace mathlib { // Converts a non-eigen 4x4 matrix to Mat4 (if it has 4 rows and 4 columns) template - Mat4 toMat4(const T& mat) { + inline Mat4 toMat4(const T& mat) { static_assert(std::tuple_size::value == 4 && std::tuple_size::value == 4, "Input matrix must be 4x4"); return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0], mat[1][1], mat[1][2], mat[1][3], @@ -45,48 +44,62 @@ namespace mathlib { } // Convert degrees to radians - double deg2rad(double degrees) { return degrees * (PI_d / 180.0); } + inline double deg2rad(double degrees) { return degrees * (PI_d / 180.0); } // Convert radians to degrees - double rad2deg(double radians) { return radians * (180.0 / PI_d); } + inline double rad2deg(double radians) { return radians * (180.0 / PI_d); } // Clamp a value between min and max - double clamp(double value, double minVal, double maxVal) { + inline double clamp(double value, double minVal, double maxVal) { if (value < minVal) { return minVal; } if (value > maxVal) { return maxVal; } return value; } + // Method to wrap an angle in radians to the range [-pi, pi] + inline double wrapToPi(double angleRad) { + angleRad = std::fmod(angleRad + PI_d, TWO_PI_d); + if (angleRad < 0.0f) { angleRad += TWO_PI_d; } + return angleRad - PI_d; // [rad] + } + + // Method to wrap an angle in radians to the range [0, 2pi] + inline double wrapRad(double angleRad) { + angleRad = fmod(angleRad, TWO_PI_d); + if (angleRad < 0.0f) { angleRad += TWO_PI_d; } + return angleRad; // [rad] + } + // Linear interpolation between a and b by factor t (0 <= t <= 1) - double lerp(double a, double b, double t) { return a + t * (b - a); } + inline double lerp(double a, double b, double t) { return a + t * (b - a); } // Check if two doubles are approximately equal within a tolerance - bool approximatelyEqual(double a, double b, double tolerance = 1e-9) { return std::fabs(a - b) <= tolerance; } + inline bool approximatelyEqual(double a, double b, double tolerance = 1e-9) { return std::fabs(a - b) <= tolerance; } // Check if a value is within a specified range [minVal, maxVal] - bool isInRange(double value, double minVal, double maxVal) { return (value >= minVal) && (value <= maxVal); } + inline bool isInRange(double value, double minVal, double maxVal) { return (value >= minVal) && (value <= maxVal); } // Sign function: returns -1 for negative, 1 for positive, and 0 for zero - double sgn(double val) { return (val > 0) - (val < 0); } + inline double sgn(double val) { return (val > 0) - (val < 0); } // Returns a quadratic function f(x) = a*x^2 + b*x + c - std::function quadratic(double a, double b, double c) { return [a, b, c](double x) { return a * x * x + b * x + c; }; } + inline std::function quadratic(double a, double b, double c) { return [a, b, c](double x) { return a * x * x + b * x + c; }; } // Returns a cubic function f(x) = a*x^3 + b*x^2 + c*x + d - std::function cubic(double a, double b, double c, double d) { return [a, b, c, d](double x) { return a * x * x * x + b * x * x + c * x + d; }; } + inline std::function cubic(double a, double b, double c, double d) { return [a, b, c, d](double x) { return a * x * x * x + b * x * x + c * x + d; }; } // Dot product of two Vec3 - double dot(const Vec3& v1, const Vec3& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } + inline double dot(const Vec3& v1, const Vec3& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } // Natural frequency of a mass-spring system - double natural_freq(double k_p, double I) { + inline double natural_freq(double k_p, double I) { if (!std::isfinite(k_p) || !std::isfinite(I)) return std::numeric_limits::quiet_NaN(); if (k_p < 0.0 || I <= 0.0) return std::numeric_limits::quiet_NaN(); return std::sqrt(k_p / I); } // Damping ratio of a mass-spring-damper system - double damping_ratio(double k_d, double I, double k_p) { + inline double damping_ratio(double k_d, double I, double k_p) { if (!std::isfinite(k_p) || !std::isfinite(k_d) || !std::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } if (k_p <= 0.0 || I <= 0.0) { return std::numeric_limits::quiet_NaN(); } double w_n = natural_freq(k_p, I); @@ -95,33 +108,33 @@ namespace mathlib { // Overloads for std::array and C arrays for Vec3 conversion template - std::enable_if_t toVec3(const std::array& arr) { + inline std::enable_if_t toVec3(const std::array& arr) { return Vec3(arr[0], arr[1], arr[2]); } template - Vec3 toVec3(const T arr[3]) { + inline Vec3 toVec3(const T arr[3]) { return Vec3(arr[0], arr[1], arr[2]); } // Overloads for std::array and C arrays for Vec4 conversion template - std::enable_if_t toVec4(const std::array& arr) { + inline std::enable_if_t toVec4(const std::array& arr) { return Vec4(arr[0], arr[1], arr[2], arr[3]); } template - Vec4 toVec4(const T arr[4]) { + inline Vec4 toVec4(const T arr[4]) { return Vec4(arr[0], arr[1], arr[2], arr[3]); } // Overloads for std::array and C arrays for Mat3 conversion template - std::enable_if_t toMat3(const std::array, N>& mat) { + inline std::enable_if_t toMat3(const std::array, N>& mat) { return Mat3(mat[0][0], mat[0][1], mat[0][2], mat[1][0], mat[1][1], mat[1][2], mat[2][0], mat[2][1], mat[2][2]); } template - Mat3 toMat3(const T mat[3][3]) { + inline Mat3 toMat3(const T mat[3][3]) { return Mat3(mat[0][0], mat[0][1], mat[0][2], mat[1][0], mat[1][1], mat[1][2], mat[2][0], mat[2][1], mat[2][2]); @@ -129,14 +142,14 @@ namespace mathlib { // Overloads for std::array and C arrays for Mat4 conversion template - std::enable_if_t toMat4(const std::array, N>& mat) { + inline std::enable_if_t toMat4(const std::array, N>& mat) { return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0], mat[1][1], mat[1][2], mat[1][3], mat[2][0], mat[2][1], mat[2][2], mat[2][3], mat[3][0], mat[3][1], mat[3][2], mat[3][3]); } template - Mat4 toMat4(const T mat[4][4]) { + inline Mat4 toMat4(const T mat[4][4]) { return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], mat[1][0], mat[1][1], mat[1][2], mat[1][3], mat[2][0], mat[2][1], mat[2][2], mat[2][3], diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj b/Sim_Engine/EngineLib/EngineLib.vcxproj index 7c098fb..21f118f 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj @@ -123,6 +123,8 @@ if "$(Configuration)"=="Release" ( + + @@ -225,6 +227,9 @@ if "$(Configuration)"=="Release" ( + + + diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters index 98b240a..e5275ab 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters @@ -537,6 +537,15 @@ Header Files\include\Interpreter\Commands\PrimaryFunc + + Header Files\include\Robots + + + Header Files\include\Robots + + + Header Files\include\Robots + @@ -752,5 +761,11 @@ Source Files\src\Interpreter\Commands\PrimaryFunc + + Source Files\src\Robots + + + Source Files\src\Robots + \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Robots/RobotCommon.h b/Sim_Engine/EngineLib/include/Robots/RobotCommon.h new file mode 100644 index 0000000..83cdc39 --- /dev/null +++ b/Sim_Engine/EngineLib/include/Robots/RobotCommon.h @@ -0,0 +1,15 @@ +#pragma once +// File: RobotCommon.h +// GitHub: SaltyJoss +#include "EngineCore.h" +#include "MathLibAPI.h" +#include "core/Types.h" + +namespace robots { + // Dynamics Mode + enum class eTorqueMode { + NONE, // No physics simulation, just kinematics (e.g., for testing) + PASSIVE, // Physics simulation with passive joints (e.g., for observing natural dynamics or testing underactuated behavior) + CONTROLLED // Full physics simulation with active control (e.g., for testing control algorithms, trajectory tracking, or simulating real-world behavior) + }; +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h new file mode 100644 index 0000000..1bb2ffa --- /dev/null +++ b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h @@ -0,0 +1,96 @@ +#pragma once +// File: RobotDynamics.h +// GitHub: SaltyJoss +#include "EngineCore.h" +#include "MathLibAPI.h" +#include "core/Types.h" + +// Forward declarations +namespace control { class ENGINE_API TrajectoryManager; } +namespace integration { class ENGINE_API IntegrationService; enum class eIntegrationMethod; } + +namespace robots { + // Forward declarations + class ENGINE_API RobotKinematics; + struct ENGINE_API RobotModel; + struct ENGINE_API RobotLink; + struct ENGINE_API RobotJoint; + struct ENGINE_API RobotMetrics; + enum class eTorqueMode; + + // Dynamics class responsible for computing inertia, mass matrix, gravity torque, control torques, and state derivatives + class ENGINE_API RobotDynamics { + public: + // Constructor + RobotDynamics(RobotModel& robot, eTorqueMode mode); + + // Computes the inertia tensor of a robot link + mathlib::Mat3 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( + 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; + + // Computes the full mass matrix M(q) based on the current state and robot configuration + mathlib::MatX computeMassMatrix( + const std::vector& q, + const std::vector& T_world + ) const; + + // Computes the gravity torque for a joint based on the current state and robot configuration + std::vector computeGravityTorque( + const std::vector& q, + const std::vector& T_world, + mathlib::VecX x + ) const; + + // Computes the control torque for a joint based on the current state, reference, and robot configuration + mathlib::VecX computeAppliedTorques( + const std::vector& q, + const std::vector& qd, + const std::vector& eta, + const std::vector& T_world, + std::vector I_eff, + std::vector tau_gravity + ) const; + + // Computes control and dynamics metrics for a specific joint based on the current state and reference + RobotMetrics computeJointMetrics( + const RobotJoint& joint, const RobotLink& link, + double I_eff, + double q, double qd, double eta, + double q_ref, double qd_ref, double qdd_ref, + double tau_coriolis, double tau_g + ) const; + + // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration + mathlib::VecX derivative( + double t, + const mathlib::VecX& x + ) const; + + // Set the torque mode for the robot system + void setTorqueMode(eTorqueMode mode) { _torqueMode = mode; } + eTorqueMode getTorqueMode() const { return _torqueMode; } + + // Accessor for the robot model + void setRobot(RobotModel& robot); + + // Set the gravity strength for the robot system + void setGravity(double gravity) { _gravity = gravity; } + + private: + // References and pointers + RobotModel& _robot; + std::unique_ptr _kinematics; + + eTorqueMode _torqueMode; + double _gravity{ 0.0 }; + bool _baseIsFree = false; + double _lastBaseForwardForce{ 0.0 }; + }; +} // namespace robots diff --git a/Sim_Engine/EngineLib/include/Robots/RobotKinematics.h b/Sim_Engine/EngineLib/include/Robots/RobotKinematics.h new file mode 100644 index 0000000..0740ae0 --- /dev/null +++ b/Sim_Engine/EngineLib/include/Robots/RobotKinematics.h @@ -0,0 +1,45 @@ +#pragma once +// File: RobotDynamics.h +// GitHub: SaltyJoss +#include "EngineCore.h" +#include "MathLibAPI.h" +#include "core/Types.h" + +namespace robots { + // Forward declarations + struct ENGINE_API RobotModel; + struct ENGINE_API RobotLink; + struct ENGINE_API RobotJoint; + struct ENGINE_API RobotMetrics; + + // Kinematics class responsible for computing forward kinematics and related transformations + class ENGINE_API RobotKinematics { + public: + // Constructor + RobotKinematics(RobotModel& robot); + + // Computes the forward kinematics for the robot based on the current state and robot configuration + std::vector computeForwardKinematics_fromState(const mathlib::VecX& x) 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, + const std::vector& joints + ); + + // 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 theta + ) const; + + // Converts roll-pitch-yaw angles (in radians) to a quaternion representation + mathlib::Quat rpyRadToQuat(const mathlib::Vec3& rpyRad); + + // Accessor for the robot model + void setRobot(RobotModel& robot); + + private: + RobotModel& _robot; + }; +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h index 3fe7069..e20702f 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h @@ -8,9 +8,15 @@ #include #include +// Forward declarations namespace control { class ENGINE_API TrajectoryManager; } namespace robots { + // Forward declarations + class ENGINE_API RobotKinematics; + class ENGINE_API RobotDynamics; + enum class eTorqueMode; + // Joint state structure struct ENGINE_API JointState { double theta; @@ -23,24 +29,16 @@ namespace robots { Baseline }; - // Dynamics Mode - enum class eTorqueMode { - NONE, // No physics simulation, just kinematics (e.g., for testing, mathematical analysis, or kinematic control) - PASSIVE, // Physics simulation with passive joints (e.g., for observing natural dynamics or testing underactuated behavior) - CONTROLLED // Full physics simulation with active control (e.g., for testing control algorithms, trajectory tracking, or simulating real-world behavior) - }; - class ENGINE_API RobotSystem { public: using spawnFn = std::function(const std::string&)>; // function type for loading meshes RobotSystem(std::vector>& sceneObjects, spawnFn meshLoader); + ~RobotSystem(); // --- Utility Methods --- static double clampJointAngle(const RobotJoint& joint, double angleRad); - static double wrapToPi(double angleRad); - static double wrapRad(double angleRad); // ---- Accessors --- @@ -58,8 +56,8 @@ namespace robots { const std::string& robotName() const { return _robot.name; } bool hasRobot() const { return _hasRobot; } + void setGravity(double g); double getGravity() const { return _gravity; } - void setGravity(double g) { _gravity = g; } // Get pointer to this RobotSystem const RobotSystem& getRobot() const { return *this; } @@ -131,14 +129,17 @@ namespace robots { void setLogBuffer(robots::JointLogBuffer* buf) { _logBuffer = buf; } void setRole(eRole role) { _role = role; } - // Set the torque mode for the robot system - void setTorqueMode(eTorqueMode mode) { _torqueMode = mode; } + // Setter and getter the torque mode for the robot system + void setTorqueMode(eTorqueMode mode); eTorqueMode getTorqueMode() const { return _torqueMode; } private: void instantiateRobotLinks(); void buildLinkIndex(); + std::unique_ptr _kinematics; + std::unique_ptr _dynamics; + std::unique_ptr _integrator; integration::eIntegrationMethod _curIntMethod{}; @@ -200,7 +201,7 @@ namespace robots { // Robot model, and robot mode RobotModel _robot; - eTorqueMode _torqueMode = eTorqueMode::NONE; + eTorqueMode _torqueMode; // Buffers for logging and reference state (not owned by RobotSystem) robots::JointLogBuffer* _logBuffer = nullptr; diff --git a/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp new file mode 100644 index 0000000..ebd2c5b --- /dev/null +++ b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp @@ -0,0 +1,510 @@ +#include "pch.h" +// File: RobotDynamics.cpp +// GitHub: SaltyJoss +#include "Robots/RobotDynamics.h" +#include "Robots/RobotKinematics.h" +#include "Robots/RobotModel.h" +#include "Robots/RobotCommon.h" +#include "Robots/TrajectoryManager.h" +#include +#include + +#include "EngineLib/LogMacros.h" + +namespace robots { + // Constructor + RobotDynamics::RobotDynamics(RobotModel& robot, eTorqueMode mode) + : _robot(robot), _torqueMode(mode) { + _kinematics = std::make_unique(robot); + } + + // 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 + } + + // 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 + mathlib::MatX RobotDynamics::computeMassMatrix( + const std::vector& /*q*/, + const std::vector& T_world + ) const { + const size_t n = _robot.joints.size(); + MatX M = MatX::Zero(n, n); // mass matrix to be computed + + // Compute its contribution to the mass matrix for each link - based on its mass, inertia, and Jacobian columns for each joint + for (size_t k = 0; k < _robot.links.size(); ++k) { + const RobotLink& link = _robot.links[k]; + const double m = link.inertial.mass; + + // Skip massless links + if (m <= 0.0) { continue; } + + // Rotation and position of the link in world frame + 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 + + // Inertia tensor of the link 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]; + // Skip fixed joints since they don't contribute to the mass matrix + if (j_i.type == eJointType::FIXED) { continue; } + + // Rotation from joint i frame to world frame + const Mat3 R_i = T_world[i + 1].block<3, 3>(0, 0); // rotation from joint i frame to world frame + const Vec3 z_i = R_i * j_i.axis; // joint axis in world frame + const Vec3 p_i = T_world[i + 1].block<3, 1>(0, 3); // joint position in world frame + + // Only include contribution if joint i affects link k + if (k <= i) continue; + + // Jacobian columns for joint i + 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]; + // Skip fixed joints since they don't contribute to the mass matrix + if (j_j.type == eJointType::FIXED) { continue; } + + // Rotation from joint j frame to world frame + const Mat3 R_j = T_world[j + 1].block<3, 3>(0, 0); // rotation from joint j frame to world frame + const Vec3 z_j = R_j * j_j.axis; // joint axis in world frame + const Vec3 p_j = T_world[j + 1].block<3, 1>(0, 3); // joint position in world frame + + // Only include contribution if joint i affects link k + if (k <= j) continue; + + // Jacobian columns for joints i and j + 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 + // Mass matrix contribution from this link for joints i and j + + // Mass matrix contribution from this link for joints i and j + M(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; + } + } + } + return M; // [kg*m^2], mass matrix for the robot at configuration q + } + + // Computes the gravity torque for a joint based on the current state and robot configuration + std::vector RobotDynamics::computeGravityTorque( + const std::vector& q, + const std::vector& T_world, + mathlib::VecX x + ) const { + const size_t n = _robot.joints.size(); + std::vector tau_G(n, 0.0); // [Nm], gravity torque for each joint + double g{ _gravity }; // [m/s^2], gravity acceleration magnitude + + // Create state vector with current joint angles + for (size_t k = 0; k < q.size(); ++k) { + x[k] = q[k]; // [rad] + } + + // 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 Vec3 p_i = T_world[i + 1].block<3, 1>(0, 3); + const Mat3 R_i = T_world[i + 1].block<3, 3>(0, 0); + 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; } + + // 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 + } + + // Computes the control torque for a joint based on the current state, reference, and robot configuration + mathlib::VecX RobotDynamics::computeAppliedTorques( + const std::vector& q, + const std::vector& qd, + const std::vector& eta, + const std::vector& /*T_world*/, + std::vector I_eff, + std::vector tau_gravity + ) const { + const size_t n = _robot.joints.size(); + VecX tau = VecX::Zero(n); // [Nm], torque for each joint + + switch (_torqueMode) { + case eTorqueMode::PASSIVE: + // Compute passive damping and friction torques + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j = _robot.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + + const double c = j.dynamics.damping; + const double mu = j.dynamics.friction; + const double v_eps = 1e-2; // small velocity threshold for friction model + + tau[i] -= c * qd[i]; // viscous damping + tau[i] -= mu * std::tanh(qd[i] / v_eps); // Coulomb friction with a small velocity threshold + } + break; + case eTorqueMode::CONTROLLED: + // State-Consistent effective inertia + for (size_t i = 0; i < n; ++i) { + const RobotJoint& j = _robot.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + + // Compute control torque using the computed metrics for this joint + RobotMetrics m = computeJointMetrics( + j, _robot.links[i + 1], + I_eff[i], + q[i], qd[i], eta[i], + j.thetaRefRad, j.omegaRefRad_s, j.alphaRefRad_s2, + 0.0, tau_gravity[i] + ); + tau[i] = m.tau; + } + break; + } + return tau; // [Nm], applied torques for each joint + } + + // Computes control and dynamics metrics for a specific joint based on the current state and reference + RobotMetrics RobotDynamics::computeJointMetrics( + const RobotJoint& joint, const RobotLink& link, + double I_eff, + double q, double qd, double eta, + double q_ref, double qd_ref, double qdd_ref, + double tau_coriolis, double tau_g + ) const { + RobotMetrics m{}; + if (_torqueMode == eTorqueMode::NONE) { + // Current states + m.theta = q; // [rad] + m.omega = qd; // [rad/s] + // Effective inertia + m.I_eff = I_eff; // [kg*m^2] + // Control parameters + m.tau = 0.0; + m.tau_fb = 0.0; + m.tau_coriolis = 0.0; + m.tau_gravity = 0.0; + m.tau_damping = 0.0; + m.tau_friction = 0.0; + m.tau_sat = 0.0; + m.tau_barrier = 0.0; + + return m; + } + + // Current states + m.theta = qd; // [rad] + m.omega = qd; // [rad/s] + // Errors + m.err = q_ref - q; // [rad] + m.err_d = qd_ref - qd; // [rad/s] + + // Effective inertia + m.I_eff = I_eff; // [kg*m^2] + + // Control parameters + const double wn = joint.wn_target; // [rad/s], natural frequency + const double z = joint.zeta_target; // damping ratio + const double b = joint.beta_target; // overshoot ratio + + // Compute PID gains + double k_p = m.I_eff * wn * wn; // [Nm/rad], proportional gain + double k_i = b * k_p * wn; // [Nm/(rad*s)], integral gain + double k_d = 2.0 * z * m.I_eff * wn; // [Nm/(rad/s)], derivative gain + + // Integral term + double tau_i = k_i * eta; + + // Integral anti-windup + if (joint.limits.maxEffort > 0.0f) { + const double rho = 0.3; // fraction of max effort allocated to I-term + const double tau_i_max = rho * joint.limits.maxEffort; + tau_i = std::clamp(tau_i, -tau_i_max, tau_i_max); + } + + // Inverse dynamics control law (PD + feedforward) + double tau_fb = k_p * m.err + tau_i + k_d * m.err_d; + + // Feedforward term based on reference acceleration and passive dynamics compensation + double tau_ff = m.I_eff * qdd_ref + tau_coriolis; + + // Passive dynamics + const double c = joint.dynamics.damping; + const double mu = joint.dynamics.friction; + const double v_eps = 1e-2; // small velocity threshold + + // Friction model (viscous + Coulomb/Stribeck) + double tau_damping{ 0.0 }, tau_friction{ 0.0 }; + tau_damping = c * qd; // viscous damping + tau_friction = mu * std::tanh(qd / v_eps); // Coulomb friction + + // Net torque + m.tau = tau_fb + tau_ff - (tau_damping + tau_friction); // [Nm], net torque applied to the joint after passive dynamics + + // Cache torques in metrics + m.tau_fb = tau_fb; + m.tau_coriolis = tau_coriolis; + m.tau_gravity = tau_g; + m.tau_damping = tau_damping; // [+] viscous damping torque + m.tau_friction = tau_friction; // [+] Coulomb friction torque + + // Hip reaction compensation (if base is free-floating, apply a fraction of the last measured base forward force as a counter-torque to the hip pitch joint to help stabilise the base) + if (_baseIsFree && joint.name.find("hip_pitch") != std::string::npos) { + const double hipReactionGain = 0.7; + m.tau -= hipReactionGain * _lastBaseForwardForce; + } + + // Torque saturation and velocity soft limits only in CONTROLLED mode + if (_torqueMode == eTorqueMode::CONTROLLED) { + double tau_preSat = m.tau; + + // Effort clamp + if (joint.limits.maxEffort > 0.0f) { + const double E_max = joint.limits.maxEffort; + /*m.tau = std::clamp(m.tau, -E_max, E_max);*/ + } + m.tau_sat = tau_preSat - m.tau; + m.sat_flag = (m.tau_sat != 0.0); + + // Velocity soft limit + const double wMax_hw = std::abs(joint.limits.maxOmegaRad_s); + const double wMax_traj = std::abs(joint.limits.omegaRefMaxRad_s); // or derived from trajectory manager + + double tau_preBarrier = m.tau; + + // Apply soft velocity barrier + /*applyOmegaBarrier(m.tau, omega, wMax_hw, m.I_eff);*/ + + m.tau_barrier = tau_preBarrier - m.tau; + + m.wMax_hw = wMax_hw; + m.wMax_traj = wMax_traj; + m.traj_overspeed = std::max(0.0, std::abs(qd) - wMax_traj); + m.traj_overspeed_flag = (m.traj_overspeed > 0.05); // 0.05 rad/s threshold + } + + return m; + } + + // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration + mathlib::VecX RobotDynamics::derivative( + double /*t*/, + const mathlib::VecX& x + ) const { + const size_t n = static_cast(_robot.joints.size()); + mathlib::VecX dx(3 * n); + + // Extract state + std::vector q(n), qd(n), eta(n); + for (size_t i = 0; i < n; ++i) { + q[i] = x[i]; + qd[i] = x[i + n]; + eta[i] = x[i + 2 * n]; + } + + // Compute forward kinematics to get the pose of each link in the world frame + std::vector T_world = _kinematics->computeForwardKinematics_fromState(x); + // Compute world poses of each joint for inertia calculations + std::vector jointWorldPose = _kinematics->calcJointWorldPoses(T_world, _robot.joints); + + // Compute effective inertia for each joint based on current configuration + std::vector I_eff(n, 0.0); + for (size_t i = 0; i < n; ++i) { + for (size_t k = i + 1; k < _robot.links.size(); ++k) { + I_eff[i] += computeJointInertiaContribution( + _robot.joints[i], + _robot.links[k], + jointWorldPose[i], // pose of joint i in world frame + T_world[k] + ); + } + I_eff[i] = std::max(I_eff[i], 1e-6); + } + + // Compute mass matrix M(q) + MatX M_full = computeMassMatrix(q, T_world); + + // Compute gravity torques for each joint + std::vector tau_gravity(n, 0.0); + // Compute gravity torques if in a torque mode that requires it + if (_torqueMode != eTorqueMode::NONE) { + // Compute gravity torque + tau_gravity = computeGravityTorque(q, T_world, x); + } + + // Compute applied torques based on control mode and current state + VecX tau = VecX::Zero(n); + if (_torqueMode != eTorqueMode::NONE) { + // Compute applied torques based on control mode + tau = computeAppliedTorques(q, qd, eta, T_world, I_eff, tau_gravity); + } + + // Build list of active (non-fixed) joints + std::vector active; + for (size_t i = 0; i < n; ++i) { + if (_robot.joints[i].type != eJointType::FIXED) { + active.push_back((int)i); + } + } + const size_t m = active.size(); + + // Build reduced system + MatX M(m, m); + VecX tau_r = VecX::Zero(m); + VecX G_r = VecX::Zero(m); + + // Fill reduced mass matrix and torque vectors for active joints + for (size_t r = 0; r < m; ++r) { + int i = active[r]; + + tau_r[r] = tau[i]; // applied torque for active joint i + G_r[r] = tau_gravity[i]; // gravity torque for active joint i + + for (size_t c = 0; c < m; ++c) { + int j = active[c]; + M(r, c) = M_full(i, j); + } + } + + // Debugging info about the mass matrix + for (int i = 0; i < M.rows(); ++i) { + double rowNorm = M.row(i).norm(); + LOG_INFO_ONCE("Row %d norm = %.6e", i, rowNorm); + } + + // Debugging info about the reduced system + LOG_INFO_ONCE("Reduced system size = %zu", m); + + double rcond = M.fullPivLu().rcond(); + LOG_INFO_ONCE("Reduced M rcond: %.6e", rcond); + + Eigen::JacobiSVD svd(M); + LOG_INFO_ONCE("Reduced min singular value: %.6e", + svd.singularValues().minCoeff()); + + // Solved for qdd + Eigen::CompleteOrthogonalDecomposition cod(M); + VecX qdd_r; + if (_torqueMode == eTorqueMode::NONE) { + qdd_r = cod.solve(tau_r); // tau_r = 0, so checks for consistency of M + } + else { + qdd_r = cod.solve(tau_r - G_r); // M qdd = tau - G -> qdd = M^-1 (tau - G) + } + + // Expand qdd back to full size, filling zeros for fixed joints + Eigen::VectorXd qdd = Eigen::VectorXd::Zero(n); + for (size_t r = 0; r < m; ++r) { + qdd[active[r]] = qdd_r[r]; + } + + LOG_INFO_ONCE("Rank(M) = %d", (int)cod.rank()); + LOG_INFO_ONCE("||tau|| = %.6e", tau.norm()); + LOG_INFO_ONCE("||G|| = %.6e", G_r.norm()); + LOG_INFO_ONCE("||qdd|| = %.6e", qdd.norm()); + + // Fill in derivatives for all joints + for (size_t i = 0; i < n; ++i) { + const RobotJoint& joint = _robot.joints[i]; + + // For fixed joints, the derivative of angle and velocity is zero + if (joint.type == eJointType::FIXED) { + dx[i] = 0.0; + dx[i + n] = 0.0; + dx[i + 2 * n] = 0.0; + continue; + } + + // Compute error for integral term + double err_i = _robot.joints[i].thetaRefRad - q[i]; + + // For revolute and prismatic joints, fill in the derivatives + dx[i] = qd[i]; + dx[i + n] = qdd[i]; + dx[i + 2 * n] = err_i; // integrate error + } + + return dx; + } + + // Set the robot model reference for dynamics calculations + void RobotDynamics::setRobot(RobotModel& robot) { + _robot = robot; + _kinematics->setRobot(robot); + } +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Robots/RobotKinematics.cpp b/Sim_Engine/EngineLib/src/Robots/RobotKinematics.cpp new file mode 100644 index 0000000..f0290b9 --- /dev/null +++ b/Sim_Engine/EngineLib/src/Robots/RobotKinematics.cpp @@ -0,0 +1,99 @@ +#include "pch.h" +// File: RobotKinematics.cpp +// GitHub: SaltyJoss +#include "Robots/RobotKinematics.h" +#include "Robots/RobotModel.h" +#include +#include + +#include "EngineLib/LogMacros.h" + +using namespace mathlib; +using namespace constants; + +namespace robots { + // Constructor + RobotKinematics::RobotKinematics(RobotModel& robot) + : _robot(robot) { + } + + // Computes the forward kinematics for the robot based on the current state and robot configuration + std::vector RobotKinematics::computeForwardKinematics_fromState(const mathlib::VecX& x) const { + const auto& joints = _robot.joints; + const auto& links = _robot.links; + const size_t n = (size_t)joints.size(); + + std::vector T_world; + T_world.reserve((size_t)links.size()); + + Pose T = Pose::Identity(); // world -> base + T_world.push_back(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 double theta = 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, theta); // rotation about joint axis + } + else if (joint.type == eJointType::PRISMATIC) { + T_motion.block<3, 1>(0, 3) = joint.axis.normalized() * theta; // translation along joint axis + } + + // compose transforms + T = T * T_origin * T_motion; // parent -> joint -> motion -> child + T_world.push_back(T); // link i+1 pose in world frame + } + return T_world; // poses of all links in world frame + } + + // 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 std::vector& joints + ) { + std::vector jointWorldPoses; + jointWorldPoses.reserve(joints.size()); + for (size_t i = 0; i < joints.size(); ++i) { + const Pose& T = T_world[i + 1]; // joint i is at the end of link i, which is at T_world[i+1] + jointWorldPoses.push_back(T); + } + 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 theta + ) const { + Pose T = Pose::Identity(); // homogeneous transformation matrix (4x4) + + Eigen::AngleAxisd aa(theta, 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(); + } + + // Accessor for the robot model + void RobotKinematics::setRobot(RobotModel& robot) { _robot = robot; } +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp index ec0b0bf..c37aa83 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp @@ -2,21 +2,24 @@ // File: RobotSystem.cpp // GitHub: SaltyJoss #include "Robots/RobotSystem.h" -#include "Robots/RobotLoader.h" #include "Scene/Object.h" #include "Scene/Mesh.h" +#include +#include "Robots/RobotKinematics.h" +#include "Robots/RobotDynamics.h" +#include "Robots/RobotLoader.h" +#include "Robots/RobotCommon.h" + #include #include #include #include -#include #include "Robots/TrajectoryManager.h" #include "Platform/Paths.h" #include "EngineLib/LogMacros.h" - #include "Platform/DataManager.h" using namespace mathlib; @@ -25,10 +28,13 @@ using namespace constants; namespace robots { // Constructor RobotSystem::RobotSystem(std::vector>& objects, spawnFn meshLoader) - : _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), - _objects(objects), _loadMeshReturn(std::move(meshLoader)) { + : _objects(objects), _loadMeshReturn(std::move(meshLoader)), _torqueMode(eTorqueMode::CONTROLLED), + _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), + _kinematics(std::make_unique(_robot)), _dynamics(std::make_unique(_robot, _torqueMode)) { if (!_integrator) { LOG_WARN("RobotSystem got null IntegrationService*"); } } + // Destructor + RobotSystem::~RobotSystem() = default; // --- 'toGlm' OVERLOADS --- @@ -68,22 +74,6 @@ namespace robots { return g; // (4x4) } - // --- OTHER CONVERSIONS --- - - // tf2::Quaternion::setRPY(roll,pitch,yaw) corresponds to q = qz * qy * qx. - static Quat rpyRadToQuat(const 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(); - } - // --- HELPER METHODS --- // Method to clamp a joint angle to its limits @@ -92,20 +82,6 @@ namespace robots { else { return glm::clamp(angleRad, joint.limits.minAngle, joint.limits.maxAngle); } } - // Method to wrap an angle in radians to the range [-pi, pi] - double RobotSystem::wrapToPi(double angleRad) { - angleRad = std::fmod(angleRad + PI_d, TWO_PI_d); - if (angleRad < 0.0f) { angleRad += TWO_PI_d; } - return angleRad - PI_d; // [rad] - } - - // Method to wrap an angle in radians to the range [0, 2pi] - double RobotSystem::wrapRad(double angleRad) { - angleRad = fmod(angleRad, TWO_PI_d); - if (angleRad < 0.0f) { angleRad += TWO_PI_d; } - return angleRad; // [rad] - } - // Method to apply a soft velocity barrier to joint torque using a quadratic "wall" function (basically a softer version of a hard velocity limit) static void applyOmegaBarrier(double& tau, double omega, double wMax, double I_eff) { if (wMax <= 0.0) return; @@ -125,70 +101,8 @@ namespace robots { tau -= wall * (omega >= 0.0 ? 1.0 : -1.0); } - // Method to compute the inertia tensor of a robot link - static Mat3 computeLinkInertiaTensor(const RobotLink& link) { - 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 - } - - // Method to compute the transformation matrix for a joint motion given its axis and angle - static Pose jointMotionTransform(const Vec3& axis_joint, double theta) { - Pose T = Pose::Identity(); // homogeneous transformation matrix (4x4) - - Eigen::AngleAxisd aa(theta, 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 - } - - // Method to compute the contribution of a single joint and its child link to the effective inertia I_eff of the joint - static double computeJointInertiaContribution( - const RobotJoint& joint, - const RobotLink& link, - const Pose& jointWorldPose, // pose of joint frame in world - const Pose& linkWorldPose // pose of the link in world - ) { - 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 - } - // --- ROBOT STATE INTEGRATION METHODS --- - // + // Method to create Object instances for each robot link void RobotSystem::instantiateRobotLinks() { // For each link, load its visual mesh(es), apply the visual origin transform, and create a scene::Object @@ -405,486 +319,6 @@ namespace robots { } } - // Method to compute forward kinematics for all links given joint angles - std::vector RobotSystem::computeForwardKinematics_fromState(const VecX& x) const { - const auto& joints = _robot.joints; - const auto& links = _robot.links; - const size_t n = (size_t)joints.size(); - - std::vector T_world; - T_world.reserve((size_t)links.size()); - - Pose T = Pose::Identity(); // world -> base - T_world.push_back(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 double theta = 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, theta); // rotation about joint axis - } - else if (joint.type == eJointType::PRISMATIC) { - T_motion.block<3, 1>(0, 3) = joint.axis.normalized() * theta; // translation along joint axis - } - - // compose transforms - T = T * T_origin * T_motion; // parent -> joint -> motion -> child - T_world.push_back(T); // link i+1 pose in world frame - } - return T_world; // poses of all links in world frame - } - - // Method to compute the full mass matrix M(q) for the robot using the composite rigid body algorithm - MatX RobotSystem::computeMassMatrix( - const std::vector& q, - const std::vector& T_world - ) const { - const size_t n = _robot.joints.size(); - MatX M = MatX::Zero(n, n); // mass matrix to be computed - - // Compute its contribution to the mass matrix for each link - based on its mass, inertia, and Jacobian columns for each joint - for (size_t k = 0; k < _robot.links.size(); ++k) { - const RobotLink& link = _robot.links[k]; - const double m = link.inertial.mass; - - // Skip massless links - if (m <= 0.0) { continue; } - - // Rotation and position of the link in world frame - 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 - - // Inertia tensor of the link 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]; - // Skip fixed joints since they don't contribute to the mass matrix - if (j_i.type == eJointType::FIXED) { continue; } - - // Rotation from joint i frame to world frame - const Mat3 R_i = T_world[i + 1].block<3, 3>(0, 0); // rotation from joint i frame to world frame - const Vec3 z_i = R_i * j_i.axis; // joint axis in world frame - const Vec3 p_i = T_world[i + 1].block<3, 1>(0, 3); // joint position in world frame - - // Only include contribution if joint i affects link k - if (k <= i) continue; - - // Jacobian columns for joint i - 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]; - // Skip fixed joints since they don't contribute to the mass matrix - if (j_j.type == eJointType::FIXED) { continue; } - - // Rotation from joint j frame to world frame - const Mat3 R_j = T_world[j + 1].block<3, 3>(0, 0); // rotation from joint j frame to world frame - const Vec3 z_j = R_j * j_j.axis; // joint axis in world frame - const Vec3 p_j = T_world[j + 1].block<3, 1>(0, 3); // joint position in world frame - - // Only include contribution if joint i affects link k - if (k <= j) continue; - - // Jacobian columns for joints i and j - 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 - // Mass matrix contribution from this link for joints i and j - - // Mass matrix contribution from this link for joints i and j - M(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; - } - } - } - return M; // [kg*m^2], mass matrix for the robot at configuration q - } - - // Method to compute the gravity torque for each joint - std::vector RobotSystem::computeGravityTorque(const std::vector& theta, const std::vector& T_world) const { - const size_t n = _robot.joints.size(); - std::vector tau_G(n, 0.0); // [Nm], gravity torque for each joint - double g{ _gravity }; // [m/s^2], gravity acceleration magnitude - - // Create state vector with current joint angles - VecX x = packState(); - for (size_t k = 0; k < theta.size(); ++k) { - x[k] = theta[k]; // [rad] - } - - // 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 Vec3 p_i = T_world[i + 1].block<3, 1>(0, 3); - const Mat3 R_i = T_world[i + 1].block<3, 3>(0, 0); - 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; } - - // 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 - } - - // Method to compute the applied torques for each joint based on the current state, control mode, and dynamics - VecX RobotSystem::computeAppliedTorques( - const std::vector& q, - const std::vector& qd, - const std::vector& eta, - const std::vector& T_world, - std::vector I_eff, - std::vector tau_gravity - ) const { - const size_t n = _robot.joints.size(); - VecX tau = VecX::Zero(n); // [Nm], torque for each joint - - switch (_torqueMode) { - case eTorqueMode::PASSIVE: - // Compute passive damping and friction torques - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = _robot.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - - const double c = j.dynamics.damping; - const double mu = j.dynamics.friction; - const double v_eps = 1e-2; // small velocity threshold for friction model - - tau[i] -= c * qd[i]; // viscous damping - tau[i] -= mu * std::tanh(qd[i] / v_eps); // Coulomb friction with a small velocity threshold - } - break; - case eTorqueMode::CONTROLLED: - // State-Consistent effective inertia - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = _robot.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - - //LOG_INFO("Joint %zu: I_eff = %.4f kg*m^2", i, I_eff[i]); - - // Compute control torque using the computed metrics for this joint - RobotMetrics m = computeJointMetrics( - j, _robot.links[i + 1], - I_eff[i], - q[i], qd[i], eta[i], - j.thetaRefRad, j.omegaRefRad_s, j.alphaRefRad_s2, - 0.0, tau_gravity[i] - ); - tau[i] = m.tau; - } - break; - } - return tau; // [Nm], applied torques for each joint - } - - // Method to compute joint metrics for control - RobotMetrics RobotSystem::computeJointMetrics( - const RobotJoint& joint, const RobotLink& /*link*/, double I_eff, - double theta, double omega, double eta, - double thetaRef, double omegaRef, double alphaRef, - double tau_coriolis, double tau_gravity - ) const { - RobotMetrics m{}; - if (_torqueMode == eTorqueMode::NONE) { - // Current states - m.theta = theta; // [rad] - m.omega = omega; // [rad/s] - // Effective inertia - m.I_eff = I_eff; // [kg*m^2] - // Control parameters - m.tau = 0.0; - m.tau_fb = 0.0; - m.tau_coriolis = 0.0; - m.tau_gravity = 0.0; - m.tau_damping = 0.0; - m.tau_friction = 0.0; - m.tau_sat = 0.0; - m.tau_barrier = 0.0; - - return m; - } - - // Current states - m.theta = theta; // [rad] - m.omega = omega; // [rad/s] - // Reference states - double q_ref = thetaRef; // [rad] - double qd_ref = omegaRef; // [rad/s] - double qdd_ref = alphaRef; // [rad/s^2] - - // Errors - m.err = q_ref - theta; // [rad] - m.err_d = qd_ref - omega; // [rad/s] - - // Effective inertia - m.I_eff = I_eff; // [kg*m^2] - - // Control parameters - const double wn = joint.wn_target; // [rad/s], natural frequency - const double z = joint.zeta_target; // damping ratio - const double b = joint.beta_target; // overshoot ratio - - // Compute PID gains - double k_p = m.I_eff * wn * wn; // [Nm/rad], proportional gain - double k_i = b * k_p * wn; // [Nm/(rad*s)], integral gain - double k_d = 2.0 * z * m.I_eff * wn; // [Nm/(rad/s)], derivative gain - - // Integral term - double tau_i = k_i * eta; - - // Integral anti-windup - if (joint.limits.maxEffort > 0.0f) { - const double rho = 0.3; // fraction of max effort allocated to I-term - const double tau_i_max = rho * joint.limits.maxEffort; - tau_i = std::clamp(tau_i, -tau_i_max, tau_i_max); - } - - // Inverse dynamics control law (PD + feedforward) - double tau_fb = k_p * m.err + tau_i + k_d * m.err_d; - - // Feedforward term based on reference acceleration and passive dynamics compensation - double tau_ff = m.I_eff* qdd_ref + tau_coriolis; - - // Passive dynamics - const double c = joint.dynamics.damping; - const double mu = joint.dynamics.friction; - const double v_eps = 1e-2; // small velocity threshold - - // Friction model (viscous + Coulomb/Stribeck) - double tau_damping{ 0.0 }, tau_friction{ 0.0 }; - tau_damping = c * omega; // viscous damping - tau_friction = mu * std::tanh(omega / v_eps); // Coulomb friction - - // Net torque - m.tau = tau_fb + tau_ff - (tau_damping + tau_friction); // [Nm], net torque applied to the joint after passive dynamics - - // Cache torques in metrics - m.tau_fb = tau_fb; - m.tau_coriolis = tau_coriolis; - m.tau_gravity = tau_gravity; - m.tau_damping = tau_damping; // [+] viscous damping torque - m.tau_friction = tau_friction; // [+] Coulomb friction torque - - // Hip reaction compensation (if base is free-floating, apply a fraction of the last measured base forward force as a counter-torque to the hip pitch joint to help stabilise the base) - if (_baseIsFree && joint.name.find("hip_pitch") != std::string::npos) { - const double hipReactionGain = 0.7; - m.tau -= hipReactionGain * _lastBaseForwardForce; - } - - // Torque saturation and velocity soft limits only in CONTROLLED mode - if (_torqueMode == eTorqueMode::CONTROLLED) { - double tau_preSat = m.tau; - - // Effort clamp - if (joint.limits.maxEffort > 0.0f) { - const double E_max = joint.limits.maxEffort; - /*m.tau = std::clamp(m.tau, -E_max, E_max);*/ - } - m.tau_sat = tau_preSat - m.tau; - m.sat_flag = (m.tau_sat != 0.0); - - // Velocity soft limit - const double wMax_hw = std::abs(joint.limits.maxOmegaRad_s); - const double wMax_traj = std::abs(joint.limits.omegaRefMaxRad_s); // or derived from trajectory manager - - double tau_preBarrier = m.tau; - - // Apply soft velocity barrier - /*applyOmegaBarrier(m.tau, omega, wMax_hw, m.I_eff);*/ - - m.tau_barrier = tau_preBarrier - m.tau; - - m.wMax_hw = wMax_hw; - m.wMax_traj = wMax_traj; - m.traj_overspeed = std::max(0.0, std::abs(omega) - wMax_traj); - m.traj_overspeed_flag = (m.traj_overspeed > 0.05); // 0.05 rad/s threshold - } - - return m; - } - - // Method to calculate the world poses of each joint based on the forward kinematics results - std::vector calcJointWorldPoses(const std::vector& T_world, const std::vector& joints) { - std::vector jointWorldPoses; - jointWorldPoses.reserve(joints.size()); - for (size_t i = 0; i < joints.size(); ++i) { - const Pose& T = T_world[i + 1]; // joint i is at the end of link i, which is at T_world[i+1] - jointWorldPoses.push_back(T); - } - return jointWorldPoses; - } - - // Derivative function for ODE integration - mathlib::VecX RobotSystem::deriv(double /*t*/, const mathlib::VecX& x) const { - const size_t n = static_cast(_robot.joints.size()); - mathlib::VecX dx(3 * n); - - // Extract state - std::vector q(n), qd(n), eta(n); - for (size_t i = 0; i < n; ++i) { - q[i] = x[i]; - qd[i] = x[i + n]; - eta[i] = x[i + 2 * n]; - } - - // Compute forward kinematics to get the pose of each link in the world frame - std::vector T_world = computeForwardKinematics_fromState(x); - // Compute world poses of each joint for inertia calculations - std::vector jointWorldPose = calcJointWorldPoses(T_world, _robot.joints); - - // Compute effective inertia for each joint based on current configuration - std::vector I_eff(n, 0.0); - for (size_t i = 0; i < n; ++i) { - for (size_t k = i + 1; k < _robot.links.size(); ++k) { - I_eff[i] += computeJointInertiaContribution( - _robot.joints[i], - _robot.links[k], - jointWorldPose[i], // pose of joint i in world frame - T_world[k] - ); - } - I_eff[i] = std::max(I_eff[i], 1e-6); - } - - // Compute mass matrix M(q) - MatX M_full = computeMassMatrix(q, T_world); - - // Compute gravity torques for each joint - std::vector tau_gravity(n, 0.0); - // Compute gravity torques if in a torque mode that requires it - if (_torqueMode != eTorqueMode::NONE) { - // Compute gravity torque - tau_gravity = computeGravityTorque(q, T_world); - } - - // Compute applied torques based on control mode and current state - VecX tau = VecX::Zero(n); - if (_torqueMode != eTorqueMode::NONE) { - // Compute applied torques based on control mode - tau = computeAppliedTorques(q, qd, eta, T_world, I_eff, tau_gravity); - } - - // Build list of active (non-fixed) joints - std::vector active; - for (size_t i = 0; i < n; ++i) { - if (_robot.joints[i].type != eJointType::FIXED) { - active.push_back((int)i); - } - } - const size_t m = active.size(); - - // Build reduced system - MatX M(m, m); - VecX tau_r = VecX::Zero(m); - VecX G_r = VecX::Zero(m); - - // Fill reduced mass matrix and torque vectors for active joints - for (size_t r = 0; r < m; ++r) { - int i = active[r]; - - tau_r[r] = tau[i]; // applied torque for active joint i - G_r[r] = tau_gravity[i]; // gravity torque for active joint i - - for (size_t c = 0; c < m; ++c) { - int j = active[c]; - M(r, c) = M_full(i, j); - } - } - - // Debugging info about the mass matrix - for (int i = 0; i < M.rows(); ++i) { - double rowNorm = M.row(i).norm(); - LOG_INFO_ONCE("Row %d norm = %.6e", i, rowNorm); - } - - // Debugging info about the reduced system - LOG_INFO_ONCE("Reduced system size = %zu", m); - - double rcond = M.fullPivLu().rcond(); - LOG_INFO_ONCE("Reduced M rcond: %.6e", rcond); - - Eigen::JacobiSVD svd(M); - LOG_INFO_ONCE("Reduced min singular value: %.6e", - svd.singularValues().minCoeff()); - - // Solved for qdd - Eigen::CompleteOrthogonalDecomposition cod(M); - VecX qdd_r; - if (_torqueMode == eTorqueMode::NONE) { - qdd_r = cod.solve(tau_r); // tau_r = 0, so checks for consistency of M - } - else { - qdd_r = cod.solve(tau_r - G_r); // M qdd = tau - G -> qdd = M^-1 (tau - G) - } - - // Expand qdd back to full size, filling zeros for fixed joints - Eigen::VectorXd qdd = Eigen::VectorXd::Zero(n); - for (size_t r = 0; r < m; ++r) { - qdd[active[r]] = qdd_r[r]; - } - - LOG_INFO_ONCE("Rank(M) = %d", (int)cod.rank()); - LOG_INFO_ONCE("||tau|| = %.6e", tau.norm()); - LOG_INFO_ONCE("||G|| = %.6e", G_r.norm()); - LOG_INFO_ONCE("||qdd|| = %.6e", qdd.norm()); - - // Fill in derivatives for all joints - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = _robot.joints[i]; - - // For fixed joints, the derivative of angle and velocity is zero - if (joint.type == eJointType::FIXED) { - dx[i] = 0.0; - dx[i + n] = 0.0; - dx[i + 2 * n] = 0.0; - continue; - } - - // Compute error for integral term - double err_i = _robot.joints[i].thetaRefRad - q[i]; - - // For revolute and prismatic joints, fill in the derivatives - dx[i] = qd[i]; - dx[i + n] = qdd[i]; - dx[i + 2 * n] = err_i; // integrate error - } - - return dx; - } - // Method to enforce joint limits after integration void RobotSystem::enforceJointLimits(RobotJoint& j) { if (j.limits.continuous) { return; } @@ -906,7 +340,7 @@ namespace robots { mathlib::VecX x = packState(); // Define the derivative function - auto f = [&](double t, const mathlib::VecX& xIn) { return deriv(t, xIn); }; + auto f = [&](double t, const mathlib::VecX& xIn) { return _dynamics->derivative(t, xIn); }; auto step = _integrator->stepODE(_curIntMethod, x, simTime, dt, f); mathlib::VecX x_Next = step.x_next; @@ -922,14 +356,14 @@ namespace robots { // FK needed for inertia mathlib::VecX x_f = packState(); - std::vector T_world = computeForwardKinematics_fromState(x_f); - std::vector jointWorldPose = calcJointWorldPoses(T_world, _robot.joints); + std::vector T_world = _kinematics->computeForwardKinematics_fromState(x_f); + std::vector jointWorldPose = _kinematics->calcJointWorldPoses(T_world, _robot.joints); // compute gravity torques for new state so logs match dynamics std::vector tau_gravity(n, 0.0); std::vector theta(n); for (size_t i = 0; i < n; ++i) theta[i] = _robot.joints[i].thetaRad; - tau_gravity = computeGravityTorque(theta, T_world); + tau_gravity = _dynamics->computeGravityTorque(theta, T_world, x); // Compute effective inertia for each joint at the new state std::vector I_eff(n, 0.0); @@ -945,7 +379,7 @@ namespace robots { // Sum contributions to effective inertia from all links for joint i for (size_t k = i; k < _robot.links.size(); ++k) { - I_eff[i] += computeJointInertiaContribution( + I_eff[i] += _dynamics->computeJointInertiaContribution( _robot.joints[i], _robot.links[k], jointWorldPose[i], // pose of joint i in world frame @@ -971,7 +405,7 @@ namespace robots { const double alphaRef = joint.alphaRefRad_s2; // Compute joint metrics - RobotMetrics m = computeJointMetrics( + RobotMetrics m = _dynamics->computeJointMetrics( joint, link, I_eff[i], theta, omega, eta, thetaRef, omegaRef, alphaRef, @@ -1075,15 +509,8 @@ namespace robots { // Load robot model from JSON _robot = robots::RobotLoader::loadFromJSON(jsonPath.string()); - - //// Extract DOF joint indices (non-fixed joints) - //_dofJointIndices.clear(); - //for (size_t i = 0; i < _robot.joints.size(); ++i) { - // if (_robot.joints[i].type != eJointType::FIXED) { - // _dofJointIndices.push_back((int)i); - // } - //} - //LOG_INFO("DOF count = %zu", _dofJointIndices.size()); + _dynamics->setRobot(_robot); + _kinematics->setRobot(_robot); _loadedName = name; _baseIsFree = false; @@ -1305,7 +732,7 @@ namespace robots { glm::mat4 T_visual(1.0f); T_visual = glm::translate(T_visual, glm::vec3(vt.x(), vt.y(), vt.z())); - T_visual *= glm::mat4_cast(toGlm(rpyRadToQuat(vr))); + T_visual *= glm::mat4_cast(toGlm(_kinematics->rpyRadToQuat(vr))); glm::mat4 M = T_link * T_visual; @@ -1323,6 +750,8 @@ namespace robots { } } + // --- JOINT STATE GETTERS AND SETTERS --- + // Method to get the angle of a specific robot joint bool RobotSystem::tryGetJointAngleRad(const std::string& childLink, double& outAngle) const { if (!_hasRobot) { return false; } @@ -1385,7 +814,6 @@ namespace robots { // Modify actual state vector mathlib::VecX x = packState(); x[i + n] = omega; // velocity slot - LOG_INFO("Injecting omega=%.3f rad/s into joint '%s'", omega, childLink.c_str()); unpackState(x); return true; } @@ -1393,7 +821,6 @@ namespace robots { return false; } - // Method to get the target angle (reference) of a specific robot joint in radians bool RobotSystem::tryGetJointTargetRad(const std::string& childLink, double& outTargetRad) const { if (!_hasRobot) { return false; } @@ -1584,6 +1011,8 @@ namespace robots { return true; } + // --- ROBOT BASE INTEGRATION METHODS --- + // Method to set the default pose of the robot using joint angles in radians double RobotSystem::computeForwardDrive() const { double drive = 0.0; @@ -1645,4 +1074,18 @@ namespace robots { _robotRootPose = T * R * _robotRootHome; } + + // --- ROBOT SYSTEM CONFIGURATION METHODS --- + + // Method to set the gravity strength for the robot system + void RobotSystem::setGravity(double g) { + _gravity = g; + _dynamics->setGravity(g); + } + + // Set the torque mode for the robot system + void RobotSystem::setTorqueMode(eTorqueMode mode) { + _torqueMode = mode; + _dynamics->setTorqueMode(mode); + } } // namespace robots \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp index 20ee9c6..ece4eda 100644 --- a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp +++ b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp @@ -5,6 +5,7 @@ #include "Scene/Mesh.h" #include "ui/ControlPanel.h" #include "Robots/RobotSystem.h" +#include "Robots/RobotCommon.h" #include "Platform/Paths.h" #include #include diff --git a/Sim_Engine/External/MathLib/Release/MathLib.dll b/Sim_Engine/External/MathLib/Release/MathLib.dll index 6e7e4652ffb4f29f90a68106f0f68eb82cf958ed..022649a566c61822b2f227f5b08414e7937724be 100644 GIT binary patch delta 98 zcmZp8!rbtLd4m8WGk?sC&BBZy7gz%snG8U{1jIQ&?6V(;SAs+s7+`WdK(+`F?*Pi} YK*&wDUnB`OD0&eqJ0t7n#Fg_s0ET@RV*mgE delta 98 zcmZp8!rbtLd4m8W^Vys!n}r!aF0ckNG8uq?35aum*k?ZwuLOxOFu>$^fNT*U-T{=` ZfsmVQzeo~nQ1l{Jc1GsSi7V%O002-}8&d!P diff --git a/Sim_Engine/External/MathLib/include/core/Utils.h b/Sim_Engine/External/MathLib/include/core/Utils.h index 192e908..e4e4d4b 100644 --- a/Sim_Engine/External/MathLib/include/core/Utils.h +++ b/Sim_Engine/External/MathLib/include/core/Utils.h @@ -1,5 +1,4 @@ #pragma once -#pragma warning(disable : 4251) #include "MathLibAPI.h" #include "core/Types.h" @@ -11,82 +10,96 @@ using namespace constants; namespace mathlib { // Converts a non-eigen vector to Vec3 (if it has 3 elements) template - Vec3 toVec3(const T& vec) { + inline Vec3 toVec3(const T& vec) { static_assert(std::tuple_size::value == 3, "Input vector must have exactly 3 elements"); return Vec3(vec[0], vec[1], vec[2]); } // Converts a non-eigen vector to Vec4 (if it has 4 elements) template - Vec4 toVec4(const T& vec) { + inline Vec4 toVec4(const T& vec) { static_assert(std::tuple_size::value == 4, "Input vector must have exactly 4 elements"); return Vec4(vec[0], vec[1], vec[2], vec[3]); } // Converts a non-eigen 3x3 matrix to Mat3 (if it has 3 rows and 3 columns) template - Mat3 toMat3(const T& mat) { + inline Mat3 toMat3(const T& mat) { static_assert(std::tuple_size::value == 3 && std::tuple_size::value == 3, "Input matrix must be 3x3"); return Mat3(mat[0][0], mat[0][1], mat[0][2], - mat[1][0], mat[1][1], mat[1][2], - mat[2][0], mat[2][1], mat[2][2] + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2] ); } // Converts a non-eigen 4x4 matrix to Mat4 (if it has 4 rows and 4 columns) template - Mat4 toMat4(const T& mat) { + inline Mat4 toMat4(const T& mat) { static_assert(std::tuple_size::value == 4 && std::tuple_size::value == 4, "Input matrix must be 4x4"); return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], - mat[1][0], mat[1][1], mat[1][2], mat[1][3], - mat[2][0], mat[2][1], mat[2][2], mat[2][3], - mat[3][0], mat[3][1], mat[3][2], mat[3][3] + mat[1][0], mat[1][1], mat[1][2], mat[1][3], + mat[2][0], mat[2][1], mat[2][2], mat[2][3], + mat[3][0], mat[3][1], mat[3][2], mat[3][3] ); } // Convert degrees to radians - double deg2rad(double degrees) { return degrees * (PI_d / 180.0); } + inline double deg2rad(double degrees) { return degrees * (PI_d / 180.0); } // Convert radians to degrees - double rad2deg(double radians) { return radians * (180.0 / PI_d); } + inline double rad2deg(double radians) { return radians * (180.0 / PI_d); } // Clamp a value between min and max - double clamp(double value, double minVal, double maxVal) { + inline double clamp(double value, double minVal, double maxVal) { if (value < minVal) { return minVal; } if (value > maxVal) { return maxVal; } return value; } + // Method to wrap an angle in radians to the range [-pi, pi] + inline double wrapToPi(double angleRad) { + angleRad = std::fmod(angleRad + PI_d, TWO_PI_d); + if (angleRad < 0.0f) { angleRad += TWO_PI_d; } + return angleRad - PI_d; // [rad] + } + + // Method to wrap an angle in radians to the range [0, 2pi] + inline double wrapRad(double angleRad) { + angleRad = fmod(angleRad, TWO_PI_d); + if (angleRad < 0.0f) { angleRad += TWO_PI_d; } + return angleRad; // [rad] + } + // Linear interpolation between a and b by factor t (0 <= t <= 1) - double lerp(double a, double b, double t) { return a + t * (b - a); } + inline double lerp(double a, double b, double t) { return a + t * (b - a); } // Check if two doubles are approximately equal within a tolerance - bool approximatelyEqual(double a, double b, double tolerance = 1e-9) { return std::fabs(a - b) <= tolerance; } + inline bool approximatelyEqual(double a, double b, double tolerance = 1e-9) { return std::fabs(a - b) <= tolerance; } // Check if a value is within a specified range [minVal, maxVal] - bool isInRange(double value, double minVal, double maxVal) { return (value >= minVal) && (value <= maxVal); } + inline bool isInRange(double value, double minVal, double maxVal) { return (value >= minVal) && (value <= maxVal); } // Sign function: returns -1 for negative, 1 for positive, and 0 for zero - double sgn(double val) { return (val > 0) - (val < 0); } + inline double sgn(double val) { return (val > 0) - (val < 0); } // Returns a quadratic function f(x) = a*x^2 + b*x + c - std::function quadratic(double a, double b, double c) { return [a, b, c](double x) { return a * x * x + b * x + c; }; } + inline std::function quadratic(double a, double b, double c) { return [a, b, c](double x) { return a * x * x + b * x + c; }; } // Returns a cubic function f(x) = a*x^3 + b*x^2 + c*x + d - std::function cubic(double a, double b, double c, double d) { return [a, b, c, d](double x) { return a * x * x * x + b * x * x + c * x + d; }; } + inline std::function cubic(double a, double b, double c, double d) { return [a, b, c, d](double x) { return a * x * x * x + b * x * x + c * x + d; }; } // Dot product of two Vec3 - double dot(const Vec3& v1, const Vec3& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } + inline double dot(const Vec3& v1, const Vec3& v2) { return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); } // Natural frequency of a mass-spring system - double natural_freq(double k_p, double I) { + inline double natural_freq(double k_p, double I) { if (!std::isfinite(k_p) || !std::isfinite(I)) return std::numeric_limits::quiet_NaN(); if (k_p < 0.0 || I <= 0.0) return std::numeric_limits::quiet_NaN(); return std::sqrt(k_p / I); } // Damping ratio of a mass-spring-damper system - double damping_ratio(double k_d, double I, double k_p) { + inline double damping_ratio(double k_d, double I, double k_p) { if (!std::isfinite(k_p) || !std::isfinite(k_d) || !std::isfinite(I)) { return std::numeric_limits::quiet_NaN(); } if (k_p <= 0.0 || I <= 0.0) { return std::numeric_limits::quiet_NaN(); } double w_n = natural_freq(k_p, I); @@ -95,51 +108,51 @@ namespace mathlib { // Overloads for std::array and C arrays for Vec3 conversion template - std::enable_if_t toVec3(const std::array& arr) { + inline std::enable_if_t toVec3(const std::array& arr) { return Vec3(arr[0], arr[1], arr[2]); } template - Vec3 toVec3(const T arr[3]) { + inline Vec3 toVec3(const T arr[3]) { return Vec3(arr[0], arr[1], arr[2]); } // Overloads for std::array and C arrays for Vec4 conversion template - std::enable_if_t toVec4(const std::array& arr) { + inline std::enable_if_t toVec4(const std::array& arr) { return Vec4(arr[0], arr[1], arr[2], arr[3]); } template - Vec4 toVec4(const T arr[4]) { + inline Vec4 toVec4(const T arr[4]) { return Vec4(arr[0], arr[1], arr[2], arr[3]); } // Overloads for std::array and C arrays for Mat3 conversion template - std::enable_if_t toMat3(const std::array, N>& mat) { + inline std::enable_if_t toMat3(const std::array, N>& mat) { return Mat3(mat[0][0], mat[0][1], mat[0][2], - mat[1][0], mat[1][1], mat[1][2], - mat[2][0], mat[2][1], mat[2][2]); + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); } template - Mat3 toMat3(const T mat[3][3]) { + inline Mat3 toMat3(const T mat[3][3]) { return Mat3(mat[0][0], mat[0][1], mat[0][2], - mat[1][0], mat[1][1], mat[1][2], - mat[2][0], mat[2][1], mat[2][2]); + mat[1][0], mat[1][1], mat[1][2], + mat[2][0], mat[2][1], mat[2][2]); } // Overloads for std::array and C arrays for Mat4 conversion template - std::enable_if_t toMat4(const std::array, N>& mat) { + inline std::enable_if_t toMat4(const std::array, N>& mat) { return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], - mat[1][0], mat[1][1], mat[1][2], mat[1][3], - mat[2][0], mat[2][1], mat[2][2], mat[2][3], - mat[3][0], mat[3][1], mat[3][2], mat[3][3]); + mat[1][0], mat[1][1], mat[1][2], mat[1][3], + mat[2][0], mat[2][1], mat[2][2], mat[2][3], + mat[3][0], mat[3][1], mat[3][2], mat[3][3]); } template - Mat4 toMat4(const T mat[4][4]) { + inline Mat4 toMat4(const T mat[4][4]) { return Mat4(mat[0][0], mat[0][1], mat[0][2], mat[0][3], - mat[1][0], mat[1][1], mat[1][2], mat[1][3], - mat[2][0], mat[2][1], mat[2][2], mat[2][3], - mat[3][0], mat[3][1], mat[3][2], mat[3][3]); + mat[1][0], mat[1][1], mat[1][2], mat[1][3], + mat[2][0], mat[2][1], mat[2][2], mat[2][3], + mat[3][0], mat[3][1], mat[3][2], mat[3][3]); } } \ No newline at end of file From b5bb0caf80572170ffbe691bdd85175fed988265 Mon Sep 17 00:00:00 2001 From: Joss Salton <145990319+SaltyJoss@users.noreply.github.com> Date: Sat, 21 Feb 2026 11:39:06 +0000 Subject: [PATCH 06/20] Refactor/code seperation (#73) * feat: move robot kinematic and dynamic logic to seperate headers and compiler files respectively * fixes: fixed previous files to correctly function with new abstraction NOTES: - This whole refactor allowed me to reduce the size of robot system and actually create a better pipeline for the physics/mathematics backend. - In the future I aim to move most methods to mathlib, with general logic applicable to various systems, but until I safely KNOW how to do that, I decided thsi was still a good way to abstract the actual robotic system physics logic from the visual/simulator * fixes: fixed previous files to correctly function with new abstraction NOTES: - This whole refactor allowed me to reduce the size of robot system and actually create a better pipeline for the physics/mathematics backend. - In the future I aim to move most methods to mathlib, with general logic applicable to various systems, but until I safely KNOW how to do that, I decided thsi was still a good way to abstract the actual robotic system physics logic from the visual/simulator * refactor: moved all core simulator methods, logic, variables, and pointers into SimulationCore.cpp, making SimManage focused on actually MANAGING the simulator. TODO: - Move visual logic into new file (im thinking SceneRenderer.cpp/.h) * Tried this, but currently very heavy and needs fixing, will sort after this semester ends * fixes --- Sim_Engine/EngineLib/EngineLib.vcxproj | 3 + .../EngineLib/EngineLib.vcxproj.filters | 6 + .../EngineLib/include/Scene/SimulationCore.h | 132 ++++++ .../include/Scene/SimulationManager.h | 125 +++--- .../src/Interpreter/CommandContextMotion.cpp | 2 +- .../src/Interpreter/Commands/TrajClearCmd.cpp | 4 +- .../src/Interpreter/Commands/TrajSetCmd.cpp | 9 +- .../src/Interpreter/StoredProgram.cpp | 20 +- .../EngineLib/src/Interpreter/UIContext.cpp | 2 +- .../EngineLib/src/Scene/SimulationCore.cpp | 372 +++++++++++++++++ .../EngineLib/src/Scene/SimulationManager.cpp | 385 +++--------------- Sim_Engine/EngineLib/src/ui/ControlPanel.cpp | 4 +- 12 files changed, 662 insertions(+), 402 deletions(-) create mode 100644 Sim_Engine/EngineLib/include/Scene/SimulationCore.h create mode 100644 Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj b/Sim_Engine/EngineLib/EngineLib.vcxproj index 21f118f..05389b6 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj @@ -125,6 +125,7 @@ if "$(Configuration)"=="Release" ( + @@ -245,6 +246,8 @@ if "$(Configuration)"=="Release" ( + + diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters index e5275ab..8cba668 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters @@ -546,6 +546,9 @@ Header Files\include\Robots + + Header Files\include\Scene + @@ -767,5 +770,8 @@ Source Files\src\Robots + + Source Files\src\Scene + \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h new file mode 100644 index 0000000..9863a79 --- /dev/null +++ b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h @@ -0,0 +1,132 @@ +#pragma once +// File: SimulationCore.h +// GitHub: SaltyJoss +#include "EngineCore.h" +#include +#include +#include "Analysis/Telemetry.h" +#include "Analysis/MetricLogger.h" + +#include "Platform/Logger.h" + +// Forward Declarations +namespace integration { enum class eIntegrationMethod; } +namespace control { class ENGINE_API TrajectoryManager; } +namespace scene { class ENGINE_API Object; } +namespace physics { class ENGINE_API PhysicsSystem; } +namespace robots { class ENGINE_API RobotSystem; } +namespace interpreter { class ENGINE_API IStoredProgram; } + +namespace core { + class ENGINE_API SimulationCore { + public: + SimulationCore(); + + // Physics + void updatePhysics(double dt); + void tick(double frame_dt); + void stepFixed(double frame_dt); + + // Simulation Integrators + void setupSimulationIntegrator(); + + // Start or stop the simulation loop + void startSimulation(); + void stopSimulation(); + const bool isSimRunning() const { return _simRunning; } + + // Export logged telemetry data to HDF5 files + void exportLogsToHDF5(); + void exportRefsToHDF5(); + + // Setter and getter for current simulation time (seconds) + void setSimTime(double t) { _simTime = t; } + const double simTime() const { return _simTime; } + + // Increment simulation time by dt (used in the simulation loop) + void incrementSimTime(double dt) { _simTime += dt; } + + // Script Running State + void setScriptRunning(bool running) { _scriptRunning = running; } + const bool isScriptRunning() const { return _scriptRunning; } + + // Setter and getter for fixed timestep (seconds) + void setFixedDt(double dt) { _dt = dt; } + const double fixedDt() const { return _dt; } + + // Setter and getter for telemetry frequency (Hz) + void setTelemetryHz(double hz) { _telHz = hz; } + const double telemetryHz() const { return _telHz; } + + // Last script text (stored on run for comparison re-use) + void setLastScriptText(const std::string& text) { _lastScriptText = text; } + const std::string& lastScriptText() const { return _lastScriptText; } + + // Run a script to completion synchronously with a specific integrator + bool runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method); + + // Setter for the physics system + void setPhysicsSystem(physics::PhysicsSystem* physics); + + // Accessor for the physics system (non-const and const versions) + physics::PhysicsSystem* physicsSystem(); + const physics::PhysicsSystem* physicsSystem() const; + + // Setter and checker for Robot System + void setRobotSystem(robots::RobotSystem* robot); + bool hasRobot() const; + + // Accessor for the robot system (non-const and const versions) + robots::RobotSystem* robotSystem(); + const robots::RobotSystem* robotSystem() const; + + // Setter for the trajectory manager + void setTrajectoryManager(control::TrajectoryManager* traj); + + // Accessor for the trajectory manager (non-const and const versions) + control::TrajectoryManager* trajectoryManager(); + const control::TrajectoryManager* trajectoryManager() const; + + // Setters the scene objects pointer (used for object lookup by scripts) + void setObjects(std::vector>* objects); + + // Setters for the metric buffers + void setJointLogBuffer(robots::JointLogBuffer* buffer); + void setTrajRefBuffer(robots::TrajRefBuffer* buffer); + + // Accesors for the telemetry recorder (non-const and const versions) + diagnostics::TelemetryRecorder& telemetry(); + const diagnostics::TelemetryRecorder& telemetry() const; + + // Accesors for the active script program + void setActiveProgram(interpreter::IStoredProgram* p); + interpreter::IStoredProgram* activeProgram() const; + + private: + // Simulation Timing + double _dt = 1.0 / 180.0; // [seconds], fixed timestep duration for physics updates + double _telHz = 120.0; // [Hz], controls how often telemetry updates during simulation runs + double _accum = 0.0; // Accumulator for fixed timestep + double _simTime = 0.0; // Current simulation time + bool _simRunning = false; // Whether the simulation loop is currently running + bool _scriptRunning = false; // Whether a script is currently running + + // Core Systems + robots::RobotSystem* _robot = nullptr; + physics::PhysicsSystem* _physics = nullptr; + control::TrajectoryManager* _traj = nullptr; + std::vector>* _objects = nullptr; + + // Last script text for comparison re-use + std::string _lastScriptText; + + // Active Script Program + interpreter::IStoredProgram* _activeProgram = nullptr; + + // Telemetry + diagnostics::TelemetryRecorder _telemetry; // Dynamic telemetry recorder + robots::JointLogBuffer _jointLogBuffer; // Buffer for logging joint data each step + robots::TrajRefBuffer _trajRefBuffer; // Buffer for logging trajectory reference data each step + bool _telemetryBegun = false; + }; +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h index e6bf11c..29ed63a 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h @@ -1,5 +1,5 @@ #pragma once -// File: MeshLoader.h +// File: SimManager.h // GitHub: SaltyJoss #include "EngineCore.h" #include @@ -33,8 +33,12 @@ namespace scene { class ENGINE_API Input; class ENGINE_API Mesh; class ENGINE_API Object; + class ENGINE_API SceneRenderer; } +// Forward Declarations for Simulation Core +namespace core { class ENGINE_API SimulationCore; } + // Forward Declarations for Physics, Robots, Control, and Integration namespace interpreter { class ENGINE_API IStoredProgram; } namespace physics { class ENGINE_API PhysicsSystem; } @@ -170,14 +174,7 @@ namespace gui { scene::Object* getObject(); scene::Object* getObjectByID(scene::ObjectID id); - // Physics void updatePhysics(double dt); - void tick(double frame_dt); - void stepFixed(double frame_dt); - - // Access to Physics System -> my attempt to fix the control panel integrtation method selector issue - physics::PhysicsSystem& physicsSystem(); - const physics::PhysicsSystem& physicsSystem() const; // Robot System loading and management void loadRobot(const std::string& name); @@ -190,69 +187,70 @@ namespace gui { void setRobotRootPose(const glm::vec3& pos, const glm::quat& rot); void setRobotRootHome(const glm::vec3& pos, const glm::quat& rot); - // Getters for the robot system (non-const and const versions) + // Access to Physics System (non-const and const versions) + physics::PhysicsSystem* physicsSystem(); + const physics::PhysicsSystem* physicsSystem() const; + + // Accesors for the robot system (non-const and const versions) robots::RobotSystem* robotSystem(); const robots::RobotSystem* robotSystem() const; - // Trajectory Manager - control::TrajectoryManager& traj(); - const control::TrajectoryManager& traj() const; - - // Simulation Integrators - void setupReferenceIntegrator(); - void setupSimulationIntegrator(); - - // Input Handling - void processMovementKey(int key, float delta); - void handleContinuousMovement(GLFWwindow* window, float dt); - void handleMouseLook(GLFWwindow* window, double xpos, double ypos); - void onMouseMove(double x, double y, scene::eInputButton button); - void onMouseWheel(double delta); - void resetMouseDelta(); + // Accessors for the trajectory manager (non-const and const versions) + control::TrajectoryManager* traj(); + const control::TrajectoryManager* traj() const; - // Start or stop the simulation loop + // Simulation Control void startSimulation(); void stopSimulation(); - const bool isSimRunning() const { return _simRunning; } - // Export logged telemetry data to HDF5 files - void exportLogsToHDF5(); - void exportRefsToHDF5(); + // Simulation State + const bool isSimRunning() const; - // Setter and getter for current simulation time (seconds) - void setSimTime(double t) { _simTime = t; } - const double simTime() const { return _simTime; } + // Setter and getter for current simulation time (seconds) + void setSimTime(double t); + const double simTime() const; - // Increment simulation time by dt (used in the simulation loop) - void incrementSimTime(double dt) { _simTime += dt; } + // Setter and getter for fixed timestep (seconds) + void setFixedDt(double dt); + const double fixedDt() const; - // Script Running State - void setScriptRunning(bool running) { _scriptRunning = running; } - const bool isScriptRunning() const { return _scriptRunning; } + // Setter and getter for telemetry frequency (Hz) + void setTelemetryHz(double hz); + const double telemetryHz() const; - // Set and get the active script program - void setActiveProgram(interpreter::IStoredProgram* p) { _activeProgram = p; } - interpreter::IStoredProgram* activeProgram() const { return _activeProgram; } + // Setters for script and simulation running states + void setScriptRunning(bool running); + const bool isScriptRunning() const; - // Setter and getter for fixed timestep duration (seconds) - void setFixedDt(double dt) { _dt = dt; } - const double fixedDt() const { return _dt; } + // Setters and getters for last script text + void setLastScriptText(const std::string& text); + const std::string& lastScriptText() const; - // Telemetry - diagnostics::TelemetryRecorder& telemetry() { return _telemetry; } - const diagnostics::TelemetryRecorder& telemetry() const { return _telemetry; } + // Accessors for the last script text + void setActiveProgram(interpreter::IStoredProgram* program); + interpreter::IStoredProgram* activeProgram(); + const interpreter::IStoredProgram* activeProgram() const; + + // Run a script to completion synchronously with a specific integrator + bool runScriptToCompletion(const std::string& scriptText, integration::eIntegrationMethod method); + + // Accesors for the telemetry recorder (non-const and const versions) + diagnostics::TelemetryRecorder& telemetry(); + const diagnostics::TelemetryRecorder& telemetry() const; + + // Accesor for Simulation Core (non-const and const versions) + core::SimulationCore* simCore(); + const core::SimulationCore* simCore() const; - // Setter and getter for telemetry frequency (Hz) - void setTelemetryHz(double hz) { _telHz = hz; } - const double telemetryHz() const { return _telHz; } + // Input Handling + void processMovementKey(int key, float delta); + void handleContinuousMovement(GLFWwindow* window, float dt); + void handleMouseLook(GLFWwindow* window, double xpos, double ypos); + void onMouseMove(double x, double y, scene::eInputButton button); + void onMouseWheel(double delta); + void resetMouseDelta(); - // Last script text (stored on run for comparison re-use) - void setLastScriptText(const std::string& text) { _lastScriptText = text; } - const std::string& lastScriptText() const { return _lastScriptText; } - // Run a script to completion synchronously with a specific integrator - // Returns true if telemetry was captured successfully - bool runScriptToCompletion(const std::string& scriptText, integration::eIntegrationMethod method); private: // Rendering Pipeline Methods @@ -278,7 +276,6 @@ namespace gui { // Misc Settings bool _glReady = false; - bool _scriptRunning = false; // Sizes & Display glm::vec2 _internalSize = { 1920.0f, 1080.0f }; // Internal render target size @@ -290,12 +287,8 @@ namespace gui { float _gridInternalScale = 1.0f; float _backgroundAlpha = 1.0f; - // Simulation Timing - double _dt = 1.0 / 180.0; - double _telHz = 120.0; // [Hz], controls how often telemetry updates during simulation runs - double _accum = 0.0; // Accumulator for fixed timestep - double _simTime = 0.0; // Current simulation time - bool _simRunning = false; // Whether the simulation loop is currently running + // Last script text for comparison re-use + std::string _lastScriptText; // Ground Plane static constexpr float planeHeight = -2.5f; @@ -314,12 +307,6 @@ namespace gui { std::unordered_map _nameToId; std::unordered_map _idToPtr; - // Active Script Program - interpreter::IStoredProgram* _activeProgram = nullptr; - - // Last script text for comparison re-use - std::string _lastScriptText; - // Telemetry diagnostics::TelemetryRecorder _telemetry; // Dynamic telemetry recorder robots::JointLogBuffer _jointLogBuffer; // Buffer for logging joint data each step @@ -353,5 +340,7 @@ namespace gui { // Camera & Mouse glm::vec2 _lastMousePos{ 0.f, 0.f }; + + std::unique_ptr _core = nullptr; }; } // namespace gui \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp b/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp index 5d18107..264d01c 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp @@ -15,7 +15,7 @@ using namespace utils; namespace commands { // Constructor CommandContextMotion::CommandContextMotion(gui::SimManager* sim, scene::ObjectID objID) - : _sim(sim), _phys(sim ? &sim->physicsSystem() : nullptr), _robot(sim ? sim->robotSystem() : nullptr), + : _sim(sim), _phys(sim ? sim->physicsSystem() : nullptr), _robot(sim ? sim->robotSystem() : nullptr), _objID(objID), _defaultObjID(objID), _angularUnits(AngularUnits::DegPerSec) { } diff --git a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp index 5955a05..25a4bb2 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp @@ -30,8 +30,8 @@ namespace commands { gui::SimManager* sim = cntx.Sim(); if (!sim) return { CmdState::Failed, {}, "trajClear: no sim in context." }; - // Clear all trajectories - sim->traj().clearAll(); + auto trajMgr = sim->traj(); + trajMgr->clearAll(); // clear all trajectories // Zero qd/qdd refs for all joints so nothing lingers auto* robot = cntx.Robot(); diff --git a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp index 6e8accc..d76ec46 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp @@ -99,7 +99,8 @@ namespace commands { const double amax = degToRad(_params[2]); auto traj = std::make_unique(t0, q0, q1, vmax, amax); - sim->traj().set(_link, std::move(traj)); + auto trajMgr = sim->traj(); + trajMgr->set(_link, std::move(traj)); double wMax_est = std::abs(vmax); wMax_est = std::min(wMax_est, (double)wMax_hw); @@ -148,7 +149,8 @@ namespace commands { double phi = (_params.size() == 5) ? degToRad(_params[4]) : 0.0; // radians auto traj = std::make_unique(t0, t0 + dur, centre, amp, fHz, phi); - sim->traj().set(_link, std::move(traj)); + auto trajMgr = sim->traj(); + trajMgr->set(_link, std::move(traj)); double wMax_est = TWO_PI_d * fHz * amp; wMax_est = std::min(wMax_est, wMax_hw); @@ -217,7 +219,8 @@ namespace commands { } auto traj = std::make_unique(t0, t0 + dur, centre, std::move(comps)); - sim->traj().set(_link, std::move(traj)); + auto trajMgr = sim->traj(); + trajMgr->set(_link, std::move(traj)); SIM_SUCCESS("trajSet: MSINE link='%s' dur=%.6fs centre=%.6f nComps=%zu", _link.c_str(), centre, dur, nComps); diff --git a/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp b/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp index 3f5b945..5f61dcf 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp @@ -179,11 +179,13 @@ namespace interpreter { if (_sim) { if (_sim->hasRobot()) { robots::RobotSystem* robot = _sim->robotSystem(); - if (robot) { robot->setIntegrationMethod(static_cast(method)); } + if (!robot) { D_FAIL("No robot system found in simulation manager."); return; } + robot->setIntegrationMethod(static_cast(method)); } - physics::PhysicsSystem& phys = _sim->physicsSystem(); - phys.setIntegrationMethod(static_cast(method)); + physics::PhysicsSystem* phys = _sim->physicsSystem(); + if (!phys) { D_FAIL("No physics system found in simulation manager."); return; } + phys->setIntegrationMethod(static_cast(method)); } } // Get Integrator Method @@ -209,11 +211,13 @@ namespace interpreter { if (_sim) { if (_sim->hasRobot()) { robots::RobotSystem* robot = _sim->robotSystem(); + if (!robot) { D_FAIL("No robot system found in simulation manager."); return; } robot->setGravity(gravity); } - physics::PhysicsSystem& phys = _sim->physicsSystem(); - phys.setGravity(mathlib::Vec3(0.0f, static_cast(gravity), 0.0f)); + physics::PhysicsSystem* phys = _sim->physicsSystem(); + if (!phys) { D_FAIL("No physics system found in simulation manager."); return; } + phys->setGravity(mathlib::Vec3(0.0f, 0.0f, static_cast(gravity))); } } // Get Gravity @@ -221,11 +225,13 @@ namespace interpreter { if (_sim) { if (_sim->hasRobot()) { robots::RobotSystem* robot = _sim->robotSystem(); + if (!robot) { D_FAIL("No robot system found in simulation manager."); return _gravity; } return robot->getGravity(); } - physics::PhysicsSystem& phys = _sim->physicsSystem(); - return phys.getGravity().y(); + physics::PhysicsSystem* phys = _sim->physicsSystem(); + if (!phys) { D_FAIL("No physics system found in simulation manager."); return _gravity; } + return phys->getGravity().y(); } return _gravity; } diff --git a/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp b/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp index 3c01023..1b14e88 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp @@ -19,7 +19,7 @@ using namespace utils; namespace commands { // Constructor UIContext::UIContext(gui::SimManager* sim, scene::ObjectID objID) - : _sim(sim), _phys(sim ? &sim->physicsSystem() : nullptr), _robot(sim ? sim->robotSystem() : nullptr), + : _sim(sim), _phys(sim ? sim->physicsSystem() : nullptr), _robot(sim ? sim->robotSystem() : nullptr), _objID(objID), _defaultObjID(objID), _angularUnits(AngularUnits::DegPerSec) { } diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp new file mode 100644 index 0000000..faf60ef --- /dev/null +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -0,0 +1,372 @@ +#include "pch.h" +// File: SimulationCore.cpp +// GitHub: SaltyJoss +#include "Scene/SimulationCore.h" +#include "Physics/PhysicsSystem.h" +#include "Robots/RobotSystem.h" +#include "Robots/RobotModel.h" +#include "Robots/TrajectoryManager.h" + +#include "Interpreter/StoredProgram.h" +#include "Interpreter/Parser.h" + +#include "EngineLib/LogMacros.h" +#include "Platform/DataManager.h" + +namespace core { + // Constructor + SimulationCore::SimulationCore() { + } + + // Update physics for all objects in the scene using the physics system + void SimulationCore::updatePhysics(double dt) { + if (!_objects || !_physics) { return; } + for (auto& obj : *_objects) { + if (obj) { _physics->update(dt, obj.get()); } + } + } + + // Simulation System + void SimulationCore::setupSimulationIntegrator() { + if (!_robot) return; + auto* intgr = _robot->getIntegrator(); + intgr->resetAdaptiveState(); + intgr->setAdaptiveTolerances(1e-3, 1e-6); + intgr->setMaxStep(_dt); + } + + // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time + void SimulationCore::stepFixed(double frame_dt) { + _accum += frame_dt; + while (_accum >= _dt) { + if (_scriptRunning && _activeProgram) { + _activeProgram->step(_dt); + + const bool completed = _activeProgram->isCompleted(); + const bool stopped = _activeProgram->isStopped(); + const bool faulted = _activeProgram->isFaulted(); + + if (completed) { + D_SUCCESS("SCRIPT END: completed=%d (dt=%.6f s, simTime=%.3f s)", (int)completed, _dt, _simTime); + + _scriptRunning = false; + _activeProgram = nullptr; + + stopSimulation(); + D_RUNTIME("Program execution completed."); + } + else if (stopped || faulted) { + D_FAIL("SCRIPT END: stopped=%d faulted=%d (dt=%.6f s, simTime=%.3f s)", + (int)stopped, (int)faulted, _dt, _simTime); + + _scriptRunning = false; + _activeProgram = nullptr; + + stopSimulation(); + D_RUNTIME("Program execution completed."); + } + } + else if (_scriptRunning && !_activeProgram) { + D_FAIL("SCRIPT END: _scriptRunning=1 but _activeProgram=nullptr"); + _scriptRunning = false; + } + + if (_simRunning) { + _simTime += _dt; + + updatePhysics(_dt); + if (hasRobot()) { + // Update Trajector Inputs + _robot->updateTrajectoryInputs(*_traj, _simTime); + // Step robot system + _robot->step(_dt, _simTime); + + // Telemetry update + 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(_simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); + } + } + _accum -= _dt; + } + } + + // Start the simulation loop + void SimulationCore::startSimulation() { + if (_simRunning) return; + telemetry().clear(); + D_RUNTIME("starting simulation"); + + _simTime = 0.0; + _accum = 0.0; + + // Reset simulation system + if (_robot) { + _robot->resetRobot(); + + // Clear and reserve telemetry buffers based on expected simulation length and robot DOF + _trajRefBuffer.clear(); + _jointLogBuffer.clear(); + + // Calculate the total number of entries needed for the buffers + size_t steps = static_cast(300.0 / _dt); // Assuming a max simulation length of 300 seconds + size_t joints = _robot->jointCount(); + size_t total = steps * joints; + + // Reserve capacity to avoid reallocations during the run + _trajRefBuffer.reserve(total); + _jointLogBuffer.reserve(total); + + // IMPORTANT: inject buffer into robot + _robot->setRefBuffer(&_trajRefBuffer); + _robot->setLogBuffer(&_jointLogBuffer); + } + + // Ensure reference sim system have their integrators configured for the new run + setupSimulationIntegrator(); + SET_SIM_INTEGRATOR(_robot->getIntegratorName()); + + _simRunning = true; + _telemetryBegun = false; + DATA_CAPTURE_ENABLE(true); + } + + // Stop the simulation loop + void SimulationCore::stopSimulation() { + if (!_simRunning) return; + D_RUNTIME("stopping simulation"); + + D_RUNTIME("JointLogBuffer size = %zu", _jointLogBuffer.size()); + D_RUNTIME("TrajRefBuffer size = %zu", _trajRefBuffer.size()); + + // Only export ref if we actually have samples + if (_trajRefBuffer.size() > 0) { + exportRefsToHDF5(); + } + // Only export sim if we actually have samples + if (_jointLogBuffer.size() > 0) { + exportLogsToHDF5(); + } + + // Clear buffers to free memory and prepare for next run + DATA_CAPTURE_ENABLE(false); + _simRunning = false; + _telemetryBegun = false; + } + + // Exporst the logged joint data to HDF5 format using the custom macro for each log entry + void SimulationCore::exportLogsToHDF5() { + // Construct a header for the HDF5 dataset based on the robot and integrator names + const std::string intName = _robot->getIntegratorName(); + const std::string robotName = _robot->hasRobot() ? _robot->robotName() : "no_robot"; + const std::string header = robotName + "_sim_" + intName; + + // Check if there are any log entries + const size_t N = _jointLogBuffer.size(); + if (N == 0) return; + + // For each log entry, create a field list and write to HDF5 + for (size_t i = 0; i < N; ++i) { + // Create a list of fields for this log entry + data::FieldList fields; + // Sim Metadata + fields.emplace_back("sim_time", (double)_jointLogBuffer.sim_time[i]); + fields.emplace_back("dt_taken", (double)_jointLogBuffer.dt_taken[i]); + fields.emplace_back("dt_sug", (double)_jointLogBuffer.dt_sug[i]); + // States + fields.emplace_back("theta", (double)_jointLogBuffer.theta[i]); + fields.emplace_back("omega", (double)_jointLogBuffer.omega[i]); + fields.emplace_back("alpha", (double)_jointLogBuffer.alpha[i]); + fields.emplace_back("err", (double)_jointLogBuffer.err[i]); + fields.emplace_back("err_d", (double)_jointLogBuffer.err_d[i]); + // Dynamics + fields.emplace_back("I_eff", (double)_jointLogBuffer.I_eff[i]); + fields.emplace_back("tau", (double)_jointLogBuffer.tau[i]); + fields.emplace_back("tau_fb", (double)_jointLogBuffer.tau_fb[i]); + fields.emplace_back("tau_coriolis", (double)_jointLogBuffer.tau_coriolis[i]); + fields.emplace_back("tau_gravity", (double)_jointLogBuffer.tau_gravity[i]); + fields.emplace_back("tau_damping", (double)_jointLogBuffer.tau_damping[i]); + fields.emplace_back("tau_friction", (double)_jointLogBuffer.tau_friction[i]); + fields.emplace_back("tau_barrier", (double)_jointLogBuffer.tau_barrier[i]); + fields.emplace_back("tau_sat", (double)_jointLogBuffer.tau_sat[i]); + // Limit flags and info + fields.emplace_back("clamp_theta", (double)_jointLogBuffer.clamp_theta[i]); + fields.emplace_back("clamp_omega", (double)_jointLogBuffer.clamp_omega[i]); + fields.emplace_back("sat_flag", (double)_jointLogBuffer.sat_flag[i]); + // Joint info + fields.emplace_back("joint_index", (double)_jointLogBuffer.joint_index[i]); + + // Write this entry to HDF5 + HDF5_SIM_DATA(header, fields); + } + } + + // Exports the reference trajectory data to HDF5 format using the custom macro for each ref entry + void SimulationCore::exportRefsToHDF5() { + const std::string robotName = _robot->hasRobot() ? _robot->robotName() : "no_robot"; + const std::string header = robotName + "_traj_ref"; + + // Check if there are any log entries + const size_t N = _trajRefBuffer.size(); + if (N == 0) return; + + // For each ref entry, create a field list and write to HDF5 + for (size_t i = 0; i < N; ++i) { + // Create a list of fields for this log entry + data::FieldList fields; + // Sim Metadata + fields.emplace_back("sim_time", (double)_trajRefBuffer.sim_time[i]); + // Reference values + fields.emplace_back("theta_ref", (double)_trajRefBuffer.theta_ref[i]); + fields.emplace_back("omega_ref", (double)_trajRefBuffer.omega_ref[i]); + fields.emplace_back("alpha_ref", (double)_trajRefBuffer.alpha_ref[i]); + // Joint info + fields.emplace_back("joint_index", (double)_trajRefBuffer.joint_index[i]); + + // Write this entry to HDF5 + HDF5_REF_DATA(header, fields); + } + + } + + // -------------------------------------------------- + // SYNCHRONOUS SCRIPT EXECUTION + // -------------------------------------------------- + + // Run a script synchronously to completion, blocking the main thread. Returns true if completed successfully. + // NOTE: this is a blocking call that runs a tight loop until the script finishes, so it should only be used for testing or non-interactive scenarios. + // IMPORTANT: I want to make this REALLY clear: + // ---> I have implemented this for short (<5 minute) test scripts where blocking is acceptable + // ---> IT IS NOT intended for general use and WILL CAUSE THE UI TO FREEZE if used with long-running scripts + bool SimulationCore::runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) { + if (!hasRobot()) { return false; } + + // Map method enum to string name, purely for logging purposes + static const char* names[] = { "euler", "midpoint", "heun", "ralston", "rk4", "rk45" }; + const std::string methodName = names[static_cast(method)]; + + // Reset robot state + _robot->resetRobot(); + _traj->clearAll(); + + // Clear telemetry and log buffers + _trajRefBuffer.clear(); + _jointLogBuffer.clear(); + + // Inject buffers BEFORE any stepping happens + _robot->setRefBuffer(&_trajRefBuffer); + _robot->setLogBuffer(&_jointLogBuffer); + + _simTime = 0.0; + _simRunning = false; + _telemetryBegun = false; + _accum = 0.0; + + // Set integrator on both physics and robot systems + _robot->setIntegrationMethod(method); + _physics->setIntegrationMethod(method); + + _activeProgram = program; + _scriptRunning = true; + + // enable sim stepping and telemetry for synchronous run + startSimulation(); + + // Run tight simulation loop until program completes + 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) + + // 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()) { + break; + } + + // Step the program (DSL command execution) + program->step(dt); + + // Step physics and robot if sim is running + if (_simRunning) { + _simTime += dt; + + updatePhysics(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); + } + } + } + // Clean up + stopSimulation(); + + _activeProgram = nullptr; + _scriptRunning = false; + _simRunning = false; + _telemetryBegun = false; + + D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", + methodName.c_str(), _simTime, _telemetry.ring.size()); + + return (_telemetry.ring.size() >= 2); + } + + // Method to step the simulation with a fixed timestep + void SimulationCore::tick(double frame_dt) { stepFixed(frame_dt); } + + // --- Setters and Getters for Systems and State --- + + // Setter for the physics system + void SimulationCore::setPhysicsSystem(physics::PhysicsSystem* physics) { _physics = physics; } + + // Accessor for the physics system (non-const and const versions) + physics::PhysicsSystem* SimulationCore::physicsSystem() { return _physics; } + const physics::PhysicsSystem* SimulationCore::physicsSystem() const { return _physics; } + + // Setter and checker for Robot System + void SimulationCore::setRobotSystem(robots::RobotSystem* robot) { _robot = robot; } + bool SimulationCore::hasRobot() const { return _robot && _robot->hasRobot(); } + + // Accessor for the robot system (non-const and const versions) + robots::RobotSystem* SimulationCore::robotSystem() { return _robot; } + const robots::RobotSystem* SimulationCore::robotSystem() const { return _robot; } + + // Setter for the trajectory manager + void SimulationCore::setTrajectoryManager(control::TrajectoryManager* traj) { _traj = traj; } + + // Accessor for the trajectory manager (non-const and const versions) + control::TrajectoryManager* SimulationCore::trajectoryManager() { return _traj; } + const control::TrajectoryManager* SimulationCore::trajectoryManager() const { return _traj; } + + // Setter for the scene objects pointer (used for script object lookup) + void SimulationCore::setObjects(std::vector>* objects) { _objects = objects; } + + // Setters for the metric buffers + void SimulationCore::setJointLogBuffer(robots::JointLogBuffer* buf) { _jointLogBuffer = *buf; } + void SimulationCore::setTrajRefBuffer(robots::TrajRefBuffer* buf) { _trajRefBuffer = *buf; } + + // Accessor for the telemetry recorder (non-const and const versions) + diagnostics::TelemetryRecorder& SimulationCore::telemetry() { return _telemetry; } + const diagnostics::TelemetryRecorder& SimulationCore::telemetry() const { return _telemetry; } + + // Setter and getter for the active script program + void SimulationCore::setActiveProgram(interpreter::IStoredProgram* p) { _activeProgram = p; } + interpreter::IStoredProgram* SimulationCore::activeProgram() const { return _activeProgram; } +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp index 85344d8..37e9c17 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp @@ -3,6 +3,7 @@ // GitHub: SaltyJoss #include "Scene/Object.h" #include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" #ifdef __gl_h_ #undef __gl_h_ @@ -19,8 +20,6 @@ #include "Scene/AxisOrientator.h" #include "Physics/PhysicsSystem.h" -#include "Robots/RobotLoader.h" -#include "Robots/RobotModel.h" #include "Robots/RobotSystem.h" #include "Robots/TrajectoryManager.h" @@ -562,7 +561,11 @@ namespace gui { // -------------------------------------------------- SimManager::SimManager() : _internalSize(1920, 1080), _displaySize(1.0f, 1.0f), _backgroundColour(0.18f, 0.18f, 0.20f), - _backgroundAlpha(1.0f), _impl(std::make_unique(*this)) { + _backgroundAlpha(1.0f), _impl(std::make_unique(*this)), _core(std::make_unique()) { + _core->setPhysicsSystem(_impl->_physics.get()); + _core->setRobotSystem(_impl->_robotSystem.get()); + _core->setObjects(&_impl->_objects); + _core->setTrajectoryManager(&_impl->_traj); } void SimManager::initGL() { @@ -882,7 +885,7 @@ namespace gui { // Main render function called by the application void SimManager::render() { ImGuiIO& io = ImGui::GetIO(); - tick(io.DeltaTime); + _core->tick(io.DeltaTime); _fpsCounter.update(); drawMainDockspace(); @@ -1055,19 +1058,10 @@ namespace gui { LOG_INFO("Resized SimManager INTERNAL RT to %dx%d", width, height); } - // -------------------------------------------------- - // PHYSICS - // -------------------------------------------------- - void gui::SimManager::updatePhysics(double dt) { - // Update each object's physics state - for (auto& obj : _impl->_objects) { - if (obj) { _impl->_physics->update(dt, obj.get()); } - } - } - - // -------------------------------------------------- - // ROBOTS - // -------------------------------------------------- + // Accessor to core's updatePhysics for use in the main application loop + void SimManager::updatePhysics(double dt) { _core->updatePhysics(dt); } + + // Load a robot by name from the robot system void SimManager::loadRobot(const std::string& name) { // Clear any existing robot first if (hasRobot()) { clearRobot(); } @@ -1126,237 +1120,81 @@ namespace gui { } const bool SimManager::hasRobot() const { return _impl->_robotSystem && _impl->_robotSystem->hasRobot(); } - // Access the robot system (non-const and const versions) - robots::RobotSystem* SimManager::robotSystem() { return _impl->_robotSystem.get(); } - const robots::RobotSystem* SimManager::robotSystem() const { return _impl->_robotSystem.get(); } - - // Simulation System - void SimManager::setupSimulationIntegrator() { - if (!_impl->_robotSystem) return; - auto* integ = _impl->_robotSystem->getIntegrator(); - integ->resetAdaptiveState(); - integ->setAdaptiveTolerances(1e-3, 1e-6); - integ->setMaxStep(_dt); - } - - // -------------------------------------------------- - // SIMULATION LOOP - // -------------------------------------------------- - - // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time - void SimManager::stepFixed(double frame_dt) { - _accum += frame_dt; - while (_accum >= _dt) { - if (_scriptRunning && _activeProgram) { - _activeProgram->step(_dt); - - const bool completed = _activeProgram->isCompleted(); - const bool stopped = _activeProgram->isStopped(); - const bool faulted = _activeProgram->isFaulted(); - - if (completed) { - D_SUCCESS("SCRIPT END: completed=%d (dt=%.6f s, simTime=%.3f s)", (int)completed, _dt, _simTime); - - _scriptRunning = false; - _activeProgram = nullptr; - - stopSimulation(); - D_RUNTIME("Program execution completed."); - } - else if (stopped || faulted) { - D_FAIL("SCRIPT END: stopped=%d faulted=%d (dt=%.6f s, simTime=%.3f s)", - (int)stopped, (int)faulted, _dt, _simTime); - - _scriptRunning = false; - _activeProgram = nullptr; + // Access the physics system (non-const and const versions) + physics::PhysicsSystem* SimManager::physicsSystem() { return _core->physicsSystem(); } + const physics::PhysicsSystem* SimManager::physicsSystem() const { return _core->physicsSystem(); } - stopSimulation(); - D_RUNTIME("Program execution completed."); - } - } - else if (_scriptRunning && !_activeProgram) { - D_FAIL("SCRIPT END: _scriptRunning=1 but _activeProgram=nullptr"); - _scriptRunning = false; - } + // Access the robot system (non-const and const versions) + robots::RobotSystem* SimManager::robotSystem() { return _core->robotSystem(); } + const robots::RobotSystem* SimManager::robotSystem() const { return _core->robotSystem(); } - if (_simRunning) { - _simTime += _dt; - - updatePhysics(_dt); - if (hasRobot()) { - // Update Trajector Inputs - _impl->_robotSystem->updateTrajectoryInputs(_impl->_traj, _simTime); - // Step robot system - _impl->_robotSystem->step(_dt, _simTime); - - // Telemetry update - 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(_simTime, *_impl->_robotSystem, &_impl->_traj, diagnostics::eTelemetryLevel::FULL); - } - } - _accum -= _dt; - } - } + // Access the trajectory manager (non-const and const versions) + control::TrajectoryManager* SimManager::traj() { return _core->trajectoryManager(); } + const control::TrajectoryManager* SimManager::traj() const { return _core->trajectoryManager(); } - // Start the simulation loop + // Start the simulation void SimManager::startSimulation() { - if (_simRunning) return; - telemetry().clear(); - D_RUNTIME("starting simulation"); - - _simTime = 0.0; - _accum = 0.0; - - // Reset simulation system - if (_impl->_robotSystem) { - _impl->_robotSystem->resetRobot(); - - // Clear and reserve telemetry buffers based on expected simulation length and robot DOF - _trajRefBuffer.clear(); - _jointLogBuffer.clear(); - - // Calculate the total number of entries needed for the buffers - size_t steps = static_cast(120.0 / _dt); - size_t joints = _impl->_robotSystem->jointCount(); - size_t total = steps * joints; - - // Reserve capacity to avoid reallocations during the run - _trajRefBuffer.reserve(total); - _jointLogBuffer.reserve(total); - - // IMPORTANT: inject buffer into robot - _impl->_robotSystem->setRefBuffer(&_trajRefBuffer); - _impl->_robotSystem->setLogBuffer(&_jointLogBuffer); + if (!hasRobot()) { + LOG_WARN("Cannot start simulation: no robot loaded"); + return; } - - // Ensure reference sim system have their integrators configured for the new run - setupSimulationIntegrator(); - SET_SIM_INTEGRATOR(_impl->_robotSystem->getIntegratorName()); - - _simRunning = true; - _telemetryBegun = true; - DATA_CAPTURE_ENABLE(true); + _core->startSimulation(); } + // Stop the simulation + void SimManager::stopSimulation() { _core->stopSimulation(); } - // Stop the simulation loop - void SimManager::stopSimulation() { - if (!_simRunning) return; - D_RUNTIME("stopping simulation"); + // Check if the simulation is currently running + const bool SimManager::isSimRunning() const { return _core->isSimRunning(); } - D_RUNTIME("JointLogBuffer size = %zu", _jointLogBuffer.size()); - D_RUNTIME("TrajRefBuffer size = %zu", _trajRefBuffer.size()); + // Setter for current simulation time (in seconds) + void SimManager::setSimTime(double time) { _core->setSimTime(time); } + const double SimManager::simTime() const { return _core->simTime(); } - // Only export ref if we actually have samples - if (_trajRefBuffer.size() > 0) { - exportRefsToHDF5(); - } - // Only export sim if we actually have samples - if (_jointLogBuffer.size() > 0) { - exportLogsToHDF5(); - } + // Setter and gettter for fixed timstep (in seconds) + void SimManager::setFixedDt(double dt) { _core->setFixedDt(dt); } + const double SimManager::fixedDt() const { return _core->fixedDt(); } - // Clear buffers to free memory and prepare for next run - DATA_CAPTURE_ENABLE(false); - _simRunning = false; - _telemetryBegun = false; - } + // Setter and getter for telemetry frequency (in Hz) + void SimManager::setTelemetryHz(double hz) { _core->setTelemetryHz(hz); } + const double SimManager::telemetryHz() const { return _core->telemetryHz(); } - // Export the logged joint data to HDF5 format using the custom macro for each log entry - void SimManager::exportLogsToHDF5() { - // Construct a header for the HDF5 dataset based on the robot and integrator names - const std::string intName = _impl->_robotSystem->getIntegratorName(); - const std::string robotName = _impl->_robotSystem->hasRobot() ? _impl->_robotSystem->robotName() : "no_robot"; - const std::string header = robotName + "_sim_" + intName; - - // Check if there are any log entries - const size_t N = _jointLogBuffer.size(); - if (N == 0) return; - - // For each log entry, create a field list and write to HDF5 - for (size_t i = 0; i < N; ++i) { - // Create a list of fields for this log entry - data::FieldList fields; - // Sim Metadata - fields.emplace_back("sim_time", (double)_jointLogBuffer.sim_time[i]); - fields.emplace_back("dt_taken", (double)_jointLogBuffer.dt_taken[i]); - fields.emplace_back("dt_sug", (double)_jointLogBuffer.dt_sug[i]); - // States - fields.emplace_back("theta", (double)_jointLogBuffer.theta[i]); - fields.emplace_back("omega", (double)_jointLogBuffer.omega[i]); - fields.emplace_back("alpha", (double)_jointLogBuffer.alpha[i]); - fields.emplace_back("err", (double)_jointLogBuffer.err[i]); - fields.emplace_back("err_d", (double)_jointLogBuffer.err_d[i]); - // Dynamics - fields.emplace_back("I_eff", (double)_jointLogBuffer.I_eff[i]); - fields.emplace_back("tau", (double)_jointLogBuffer.tau[i]); - fields.emplace_back("tau_fb", (double)_jointLogBuffer.tau_fb[i]); - fields.emplace_back("tau_coriolis", (double)_jointLogBuffer.tau_coriolis[i]); - fields.emplace_back("tau_gravity", (double)_jointLogBuffer.tau_gravity[i]); - fields.emplace_back("tau_damping", (double)_jointLogBuffer.tau_damping[i]); - fields.emplace_back("tau_friction", (double)_jointLogBuffer.tau_friction[i]); - fields.emplace_back("tau_barrier", (double)_jointLogBuffer.tau_barrier[i]); - fields.emplace_back("tau_sat", (double)_jointLogBuffer.tau_sat[i]); - // Limit flags and info - fields.emplace_back("clamp_theta", (double)_jointLogBuffer.clamp_theta[i]); - fields.emplace_back("clamp_omega", (double)_jointLogBuffer.clamp_omega[i]); - fields.emplace_back("sat_flag", (double)_jointLogBuffer.sat_flag[i]); - // Joint info - fields.emplace_back("joint_index", (double)_jointLogBuffer.joint_index[i]); - - // Write this entry to HDF5 - HDF5_SIM_DATA(header, fields); - } - } + // Set whether a script is currently running (used to disable UI elements, etc.) + void SimManager::setScriptRunning(bool running) { _core->setScriptRunning(running); } + const bool SimManager::isScriptRunning() const { return _core->isScriptRunning(); } + // Setters and getters for last script text + void SimManager::setLastScriptText(const std::string& text) { _core->setLastScriptText(text); } + const std::string& SimManager::lastScriptText() const { return _core->lastScriptText(); } - void SimManager::exportRefsToHDF5() { - const std::string robotName = _impl->_robotSystem->hasRobot() ? _impl->_robotSystem->robotName() : "no_robot"; - const std::string header = robotName + "_traj_ref"; - - // Check if there are any log entries - const size_t N = _trajRefBuffer.size(); - if (N == 0) return; - - // For each ref entry, create a field list and write to HDF5 - for (size_t i = 0; i < N; ++i) { - // Create a list of fields for this log entry - data::FieldList fields; - // Sim Metadata - fields.emplace_back("sim_time", (double)_trajRefBuffer.sim_time[i]); - // Reference values - fields.emplace_back("theta_ref", (double)_trajRefBuffer.theta_ref[i]); - fields.emplace_back("omega_ref", (double)_trajRefBuffer.omega_ref[i]); - fields.emplace_back("alpha_ref", (double)_trajRefBuffer.alpha_ref[i]); - // Joint info - fields.emplace_back("joint_index", (double)_trajRefBuffer.joint_index[i]); - - // Write this entry to HDF5 - HDF5_REF_DATA(header, fields); - } + // Accessors for the Simulation Core's telemetry data + diagnostics::TelemetryRecorder& SimManager::telemetry() { return _core->telemetry(); } + const diagnostics::TelemetryRecorder& SimManager::telemetry() const { return _core->telemetry(); } - } + // Accesors for the active program (if any) + void SimManager::setActiveProgram(interpreter::IStoredProgram* program) { _core->setActiveProgram(program); } + interpreter::IStoredProgram* SimManager::activeProgram() { return _core->activeProgram(); } + const interpreter::IStoredProgram* SimManager::activeProgram() const { return _core->activeProgram(); } - // -------------------------------------------------- - // SYNCHRONOUS SCRIPT EXECUTION - // -------------------------------------------------- + // Access the simulation core for advanced users who want to run custom programs, etc. + core::SimulationCore* SimManager::simCore() { return _core.get(); } + const core::SimulationCore* SimManager::simCore() const { return _core.get(); } // Helper to replace the integrator method in the script text // A hacky approach my idea, but it works for me and honestly im starting to write up the dissertation so IT WILL DO :) // PS:If anyone has any better solution msg me static std::string replaceIntegratorInScript(const std::string& script, const std::string& methodName) { std::string result = script; + // Find set(integrator, ...) and replace the method name const std::string prefix = "set(integrator,"; auto pos = result.find(prefix); + + // If not found, also check for "set(integrator, " with a space after the comma if (pos == std::string::npos) { - // Also try with space variations const std::string prefix2 = "set(integrator, "; pos = result.find(prefix2); } + // If we found a set(integrator, ...), replace the method name inside the parentheses if (pos != std::string::npos) { auto end = result.find(')', pos); if (end != std::string::npos) { @@ -1366,122 +1204,33 @@ namespace gui { return result; } - // Run a script synchronously to completion, blocking the main thread. Returns true if completed successfully. - // NOTE: this is a blocking call that runs a tight loop until the script finishes, so it should only be used for testing or non-interactive scenarios. - // IMPORTANT: I want to make this REALLY clear: - // ---> I have implemented this for short (<5 minute) test scripts where blocking is acceptable - // ---> IT IS NOT intended for general use and WILL CAUSE THE UI TO FREEZE if used with long-running scripts + // Run a script to completion synchronously with a specific integrator bool SimManager::runScriptToCompletion(const std::string& scriptText, integration::eIntegrationMethod method) { - if (!hasRobot()) return false; + if (!hasRobot()) { return false; } // Map method enum to string name static const char* names[] = { "euler", "midpoint", "heun", "ralston", "rk4", "rk45" }; const std::string methodName = names[static_cast(method)]; - // Replace the integrator in the script text + // Replace the integrator method in the script text std::string modifiedScript = replaceIntegratorInScript(scriptText, methodName); - // Reset robot state - resetRobot(); - _impl->_traj.clearAll(); - - // Clear telemetry and log buffers - _trajRefBuffer.clear(); - _jointLogBuffer.clear(); - - // Inject buffers BEFORE any stepping happens - _impl->_robotSystem->setRefBuffer(&_trajRefBuffer); - _impl->_robotSystem->setLogBuffer(&_jointLogBuffer); - - _simTime = 0.0; - _simRunning = false; - _telemetryBegun = false; - _accum = 0.0; - - // Set integrator on both physics and robot systems - _impl->_robotSystem->setIntegrationMethod(method); - _impl->_physics->setIntegrationMethod(method); - - // Create and parse program + // Create program and parser auto program = std::make_unique(this); program->setDefaultObject(getObject()); auto parser = std::make_unique(program.get()); + // Clear any existing program state program->clear(); + // Parse the modified script parser->parse(modifiedScript); + // Start the program program->start(); - _activeProgram = program.get(); - _scriptRunning = true; - - // enable sim stepping and telemetry for synchronous run - startSimulation(); - - // Run tight simulation loop until program completes - 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) - - // 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()) { - break; - } - - // Step the program (DSL command execution) - program->step(dt); - - // Step physics and robot if sim is running - if (_simRunning) { - _simTime += dt; - - updatePhysics(dt); - - if (hasRobot()) { - // Update Trajectory Inputs - _impl->_robotSystem->updateTrajectoryInputs(_impl->_traj, _simTime); - - // Step robot system - _impl->_robotSystem->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, *_impl->_robotSystem, &_impl->_traj, diagnostics::eTelemetryLevel::FULL ); - } - } - } - - // Clean up - stopSimulation(); - - _activeProgram = nullptr; - _scriptRunning = false; - _simRunning = false; - _telemetryBegun = false; - - D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", - methodName.c_str(), _simTime, _telemetry.ring.size()); - - return (_telemetry.ring.size() >= 2); + // Run to completion + return _core->runScriptToCompletion(program.get(), method); } - // Method to step the simulation with a fixed timestep - void SimManager::tick(double frame_dt) { /*D_DEBUG("tick frame_dt=%.6f", frame_dt);*/ stepFixed(frame_dt); } - - // Access the physics system (non-const and const versions) - physics::PhysicsSystem& SimManager::physicsSystem() { return *_impl->_physics; } // mutable - const physics::PhysicsSystem& SimManager::physicsSystem() const { return *_impl->_physics; } // const - - // Access the robot system (non-const and const versions) - control::TrajectoryManager& SimManager::traj() { return _impl->_traj; } - const control::TrajectoryManager& SimManager::traj() const { return _impl->_traj; } - // -------------------------------------------------- // INTERNAL REDNDERING PIPELINE // -------------------------------------------------- diff --git a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp index ece4eda..98c82a7 100644 --- a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp +++ b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp @@ -249,7 +249,7 @@ namespace gui { void ControlPanel::drawMenus(SimManager* sim) { _sim = sim; - _phys = &_sim->physicsSystem(); + _phys = _sim->physicsSystem(); _obj = _sim->getObject(); if (ImGui::BeginMenu("File")) { @@ -304,7 +304,7 @@ namespace gui { _light = _sim->getLight(); _hasRobot = _sim->hasRobot(); - _phys = &_sim->physicsSystem(); + _phys = _sim->physicsSystem(); ImGui::SetNextWindowPos(ImVec2(10, 10), ImGuiCond_FirstUseEver); ImGui::SetNextWindowSize(ImVec2(300, 400), ImGuiCond_FirstUseEver); From 2f836731e6a36ee7fbdba6fa573c611ea6e1ea9f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 21 Feb 2026 15:09:19 +0000 Subject: [PATCH 07/20] refactor: refactored robot joint state naming and add energy metrics NOTES: - Renamed joint state and reference variables for clarity (`thetaRad` -> `q`, `omegaRad_s` -> `qd`) - Moved and integrated `eTorqueMode` enum into `RobotModel` - Added energy, work, and power metrics to `RobotMetrics` and logging - Updated telemetry, CSV export, and GUI to use new names and metrics - Improved clamp event plots and results window UI - Updated mouse capture logic and build system files - Removed all references to old joint state names --- Sim_Engine/EngineLib/EngineLib.vcxproj | 1 - .../EngineLib/EngineLib.vcxproj.filters | 6 +- .../EngineLib/include/Analysis/MetricLogger.h | 21 +++ .../EngineLib/include/Analysis/Telemetry.h | 52 ++++--- .../Interpreter/CommandContextMotion.h | 6 +- .../include/Platform/WindowManager.h | 2 +- .../EngineLib/include/Robots/RobotCommon.h | 15 -- .../EngineLib/include/Robots/RobotDynamics.h | 14 +- .../EngineLib/include/Robots/RobotModel.h | 41 +++-- .../EngineLib/include/Robots/RobotSystem.h | 4 +- .../include/Scene/SimulationManager.h | 4 - .../EngineLib/include/ui/ControlPanel.h | 4 +- .../EngineLib/src/Analysis/Telemetry.cpp | 52 ++++--- .../src/Interpreter/CommandContextMotion.cpp | 14 +- .../EngineLib/src/Platform/WindowManager.cpp | 19 ++- .../EngineLib/src/Robots/RobotDynamics.cpp | 104 +++++++------ .../EngineLib/src/Robots/RobotLoader.cpp | 27 +--- .../EngineLib/src/Robots/RobotSystem.cpp | 143 +++++++++--------- .../EngineLib/src/Scene/SimulationCore.cpp | 7 + .../EngineLib/src/Scene/SimulationManager.cpp | 8 +- Sim_Engine/EngineLib/src/ui/ControlPanel.cpp | 120 ++++++++------- 21 files changed, 348 insertions(+), 316 deletions(-) delete mode 100644 Sim_Engine/EngineLib/include/Robots/RobotCommon.h diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj b/Sim_Engine/EngineLib/EngineLib.vcxproj index 05389b6..4af62b2 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj @@ -228,7 +228,6 @@ if "$(Configuration)"=="Release" ( - diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters index 8cba668..5864ed6 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters @@ -543,12 +543,12 @@ Header Files\include\Robots - - Header Files\include\Robots - Header Files\include\Scene + + Header Files + diff --git a/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h b/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h index 8dfae46..ac695b1 100644 --- a/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h +++ b/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h @@ -26,6 +26,13 @@ namespace robots { std::vector tau_friction; std::vector tau_barrier; std::vector tau_sat; + // Energy, Work, & Power + std::vector KE; + std::vector PE; + std::vector E_total; + std::vector W_actuator; + std::vector P_damping; + std::vector P_friction; // Limit flags and info std::vector clamp_theta; std::vector clamp_omega; @@ -55,6 +62,13 @@ namespace robots { tau_friction.clear(); tau_barrier.clear(); tau_sat.clear(); + // Energy, Work, & Power + KE.clear(); + PE.clear(); + E_total.clear(); + W_actuator.clear(); + P_damping.clear(); + P_friction.clear(); // Limit flags and info clamp_theta.clear(); clamp_omega.clear(); @@ -85,6 +99,13 @@ namespace robots { tau_friction.reserve(n); tau_barrier.reserve(n); tau_sat.reserve(n); + // Energy, Work, & Power reserve + KE.reserve(n); + PE.reserve(n); + E_total.reserve(n); + W_actuator.reserve(n); + P_damping.reserve(n); + P_friction.reserve(n); // Limit flags and info reserve clamp_theta.reserve(n); clamp_omega.reserve(n); diff --git a/Sim_Engine/EngineLib/include/Analysis/Telemetry.h b/Sim_Engine/EngineLib/include/Analysis/Telemetry.h index 666969d..0bddcf7 100644 --- a/Sim_Engine/EngineLib/include/Analysis/Telemetry.h +++ b/Sim_Engine/EngineLib/include/Analysis/Telemetry.h @@ -14,51 +14,53 @@ namespace control { class ENGINE_API TrajectoryManager; } namespace diagnostics { // Enum for telemetry levels enum class eTelemetryLevel { - NONE = 0, // No telemetry data - BASIC = 1, // Basic telemetry data - FULL = 2 // Detailed telemetry data + NONE = 0, // No telemetry data + BASIC = 1, // Basic telemetry data + FULL = 2 // Detailed telemetry data }; // Struct for joint telemetry data struct ENGINE_API JointTelemetry{ // Actual data - double thetaRad = 0.0f; // Joint angle in radians - double omegaRad_s = 0.0f; // Joint angular velocity in radians per second - double eta = 0.0f; // + double q = 0.0f; // Joint angle in radians + double qd = 0.0f; // Joint angular velocity in radians per second + double eta = 0.0f; // // 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.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) // Reference data - double thetaRefRad = 0.0f; // Reference joint angle in radians - double omegaRefRad_s = 0.0f; // Reference joint angular velocity in radians per second - double alphaRefRad_s2 = 0.0f; // Reference joint angular acceleration in radians per second squared + 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 // 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 - bool traj_active = false; // Trajectory active state for a joint + double traj_q = 0.0f; // Trajectory joint position + double traj_qd = 0.0f; // Trajectory joint velocity + double traj_qdd = 0.0f; // Trajectory joint acceleration + bool traj_active = false; // Trajectory active state for a joint // Limit clamping flags - bool clampTheta = false; // Whether the joint angle is clamped to limits - bool clampOmega = false; // Whether the joint velocity is clamped to limits + bool clampTheta = false; // Whether the joint angle is clamped to limits + bool clampOmega = false; // Whether the joint velocity is clamped to limits }; // Struct for a single telemetry sample struct ENGINE_API TelemetrySample { // Timestamp and joint data - double timeSec = 0.0; // Timestamp of the sample in seconds - std::vector j; // Vector of joint telemetry data + double timeSec = 0.0; // Timestamp of the sample in seconds + 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 - int clamp_sum = 0; // Sum of clamping events across all joints - int worst_joint = -1; // Index of the joint with max |e| + double err_rms = 0.0f; // RMS error across all joints + double err_max = 0.0f; // 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 + int worst_joint = -1; // Index of the joint with max |e| }; // Ring buffer for storing telemetry samples diff --git a/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h b/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h index 62bac0a..65f062b 100644 --- a/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h +++ b/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h @@ -82,9 +82,9 @@ namespace commands { utils::OpResult setJointTargetRad(const std::string& link, double thetaTargetRad); utils::OpResult setJointTargetDeltaRad(const std::string& link, double deltaRad); - utils::OpResult setJointMaxOmegaRad(const std::string& link, double maxOmegaRad_s); - utils::OpResult setJointOmegaRefRad(const std::string& link, double omegaRefRad_s); - utils::OpResult setJointAlphaRefRad(const std::string& link, double alphaRefRad_s2); + utils::OpResult setJointMaxOmegaRad(const std::string& link, double maxqd); + utils::OpResult setJointOmegaRefRad(const std::string& link, double qd_ref); + utils::OpResult setJointAlphaRefRad(const std::string& link, double qdd_ref); utils::OpResult updateRigidRotateTo(double dt); utils::OpResult updateJointRotateTo(double dt); diff --git a/Sim_Engine/EngineLib/include/Platform/WindowManager.h b/Sim_Engine/EngineLib/include/Platform/WindowManager.h index e113a27..e0ade3d 100644 --- a/Sim_Engine/EngineLib/include/Platform/WindowManager.h +++ b/Sim_Engine/EngineLib/include/Platform/WindowManager.h @@ -70,7 +70,7 @@ namespace window { std::unique_ptr _cmdEditor; bool _isHovered = false; - bool _mouseCaptured = false; + bool _mouseCaptured = true; // Window properties int _width = 0; diff --git a/Sim_Engine/EngineLib/include/Robots/RobotCommon.h b/Sim_Engine/EngineLib/include/Robots/RobotCommon.h deleted file mode 100644 index 83cdc39..0000000 --- a/Sim_Engine/EngineLib/include/Robots/RobotCommon.h +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once -// File: RobotCommon.h -// GitHub: SaltyJoss -#include "EngineCore.h" -#include "MathLibAPI.h" -#include "core/Types.h" - -namespace robots { - // Dynamics Mode - enum class eTorqueMode { - NONE, // No physics simulation, just kinematics (e.g., for testing) - PASSIVE, // Physics simulation with passive joints (e.g., for observing natural dynamics or testing underactuated behavior) - CONTROLLED // Full physics simulation with active control (e.g., for testing control algorithms, trajectory tracking, or simulating real-world behavior) - }; -} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h index 1bb2ffa..004c966 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h @@ -60,8 +60,7 @@ namespace robots { // Computes control and dynamics metrics for a specific joint based on the current state and reference RobotMetrics computeJointMetrics( - const RobotJoint& joint, const RobotLink& link, - double I_eff, + const RobotJoint& joint, double I_eff, double q, double qd, double eta, double q_ref, double qd_ref, double qdd_ref, double tau_coriolis, double tau_g @@ -73,22 +72,23 @@ namespace robots { const mathlib::VecX& x ) const; - // Set the torque mode for the robot system - void setTorqueMode(eTorqueMode mode) { _torqueMode = mode; } - eTorqueMode getTorqueMode() const { return _torqueMode; } - // Accessor for the robot model void setRobot(RobotModel& robot); // Set the gravity strength for the robot system void setGravity(double gravity) { _gravity = gravity; } + // Set the timestep for dynamics updates (used for energy calculations and integration) + void setDt(double dt) { _dt = dt; } + const double dt() const { return _dt; } + private: // References and pointers RobotModel& _robot; std::unique_ptr _kinematics; - eTorqueMode _torqueMode; + double _dt = 1.0 / 180.0; // default timestep for dynamics updates + double _gravity{ 0.0 }; bool _baseIsFree = false; double _lastBaseForwardForce{ 0.0 }; diff --git a/Sim_Engine/EngineLib/include/Robots/RobotModel.h b/Sim_Engine/EngineLib/include/Robots/RobotModel.h index e52cdf8..72a595f 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotModel.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotModel.h @@ -16,20 +16,26 @@ namespace robots { URDF, DH }; - /// --- URDF Joint Types --- + // --- URDF Joint Types --- enum class eJointType { FIXED = 0, REVOLUTE = 1, PRISMATIC = 2, FREE = 3 }; - /// --- Visual Frame Options --- + // --- Visual Frame Options --- enum class eVisualFrame { NONE, JOINT, LINK, WORLD }; + // --- Torque Modes for Simulation --- + enum class eTorqueMode { + NONE, // No physics simulation, just kinematics (e.g., for testing) + PASSIVE, // Physics simulation with passive joints (e.g., for observing natural dynamics or testing underactuated behavior) + CONTROLLED // Full physics simulation with active control (e.g., for testing control algorithms, trajectory tracking, or simulating real-world behavior) + }; // --- Robot Model Links --- @@ -104,7 +110,7 @@ namespace robots { bool continuous = false; double minAngle = 0.0f; double maxAngle = 0.0f; - double maxOmegaRad_s = glm::radians(180.0f); + double maxqd = glm::radians(180.0f); double maxEffort = 0.0f; // max torque/force // Soft limits double omegaRefMaxRad_s = 0.0; @@ -143,19 +149,17 @@ namespace robots { JointDynamics dynamics; // --- State --- - double thetaRad = 0.0f; // rad - double omegaRad_s = 0.0f; // rad/s + double q = 0.0f; // rad + double qd = 0.0f; // rad/s double torque = 0.0f; // Nm or N double eta = 0.0f; // Integral state // --- Control --- - double thetaRefRad = 0.0f; // rad - double omegaRefRad_s = 0.0f; // rad/s - double alphaRefRad_s2 = 0.0f; // rad/s^2 - double k_p = 10.0f; // position gain (rad) - double k_i = 0.0f; // integral gain (rad*s) - double k_d = 10.0f; // velocity gain (rad/s) + double q_ref = 0.0f; // rad + double qd_ref = 0.0f; // rad/s + double qdd_ref = 0.0f; // rad/s^2 + // double wn_target = 5.0f; // rad/s double beta_target = 0.1f; // overshoot ratio double zeta_target = 1.1f; // damping ratio @@ -176,6 +180,9 @@ namespace robots { std::vector links; std::vector joints; + // Torque Mode for simulation + eTorqueMode torqueMode = eTorqueMode::CONTROLLED; + // Kinematics model (URDF or DH) eKinematicsModel kinematicsModel = eKinematicsModel::URDF; std::vector dhParams; @@ -192,7 +199,7 @@ namespace robots { const int n = static_cast(joints.size()); LOG_INFO_ONCE("Making joint vector of size %d", n); VecX q(n); - for (int i = 0; i < n; ++i) { q(i) = joints[i].thetaRad; } + for (int i = 0; i < n; ++i) { q(i) = joints[i].q; } return q; } @@ -206,7 +213,7 @@ namespace robots { } for (int i = 0; i < n; ++i) { double a = q(i); - joints[i].thetaRad = a; + joints[i].q = a; } } }; @@ -238,6 +245,14 @@ namespace robots { double wMax_traj{ 0.0 }; double traj_overspeed{ 0.0 }; + // Energy, Work, & Power + double KE{ 0.0 }; + double PE{ 0.0 }; + double E_total{ 0.0 }; + double W_actuator{ 0.0 }; + double P_damping{ 0.0 }; + double P_friction{ 0.0 }; + // Stability flags bool sat_flag{ false }; bool traj_overspeed_flag{ false }; diff --git a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h index e20702f..5584520 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h @@ -131,7 +131,7 @@ namespace robots { // Setter and getter the torque mode for the robot system void setTorqueMode(eTorqueMode mode); - eTorqueMode getTorqueMode() const { return _torqueMode; } + eTorqueMode getTorqueMode() const { return _robot.torqueMode; } private: void instantiateRobotLinks(); @@ -201,7 +201,7 @@ namespace robots { // Robot model, and robot mode RobotModel _robot; - eTorqueMode _torqueMode; + eTorqueMode _torqueMode = _robot.torqueMode; // Buffers for logging and reference state (not owned by RobotSystem) robots::JointLogBuffer* _logBuffer = nullptr; diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h index 29ed63a..ad66691 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h @@ -147,7 +147,6 @@ namespace gui { ShaderMode currentShaderMode = ShaderMode::PBR; // default // Setter and getter for the current shader - void setCurrentShader(shaders::Shader* shader); const shaders::Shader* getCurrentShader() const; // Render Settings & Profiles @@ -271,9 +270,6 @@ namespace gui { void beginSimManager(const char* id); void endSimManager(); - // Telemetry - void prepareLogBuffer(size_t expectedSteps, size_t jointCount); - // Misc Settings bool _glReady = false; diff --git a/Sim_Engine/EngineLib/include/ui/ControlPanel.h b/Sim_Engine/EngineLib/include/ui/ControlPanel.h index 19915cf..fb16380 100644 --- a/Sim_Engine/EngineLib/include/ui/ControlPanel.h +++ b/Sim_Engine/EngineLib/include/ui/ControlPanel.h @@ -108,7 +108,7 @@ namespace gui { // Follow View Methods void selectJointAndFollow(int jointIndex); - void drawTelemetryPlots(const diagnostics::TelemetryRecorder& rec); + //void drawTelemetryPlots(const diagnostics::TelemetryRecorder& rec); void drawTrajectoryInspector(const diagnostics::TelemetryRecorder& rec, int jointCount, int& selectedJoint); // Results Methods @@ -157,7 +157,7 @@ namespace gui { std::vector errRms; std::vector errMax; std::vector> jointErr; - int jointCount = 0; + size_t jointCount = 0; }; std::vector _comparisonResults; bool _comparisonReady = false; diff --git a/Sim_Engine/EngineLib/src/Analysis/Telemetry.cpp b/Sim_Engine/EngineLib/src/Analysis/Telemetry.cpp index c9b4a37..67bbf20 100644 --- a/Sim_Engine/EngineLib/src/Analysis/Telemetry.cpp +++ b/Sim_Engine/EngineLib/src/Analysis/Telemetry.cpp @@ -19,10 +19,12 @@ namespace diagnostics { s.j.resize(joints.size()); // Accumulators for error statistics - double sum_e2 = 0.0; // sum of squared errors - double max_abs_e = 0.0f; // max absolute error - int worstJ = -1; // index of worst joint - int clampSum = 0; // sum of clamping events + double sum_e2 = 0.0; // sum of squared errors + double max_abs_e = 0.0; // max absolute error + int clampThetaSum = 0; // sum of angle clamping events + int clampOmegaSum = 0; // sum of velocity clamping events + int clampSum = 0; // sum of clamping events + int worstJ = -1; // index of worst joint // Collect telemetry for each joint for (int i = 0; i < n; ++i) { @@ -31,22 +33,28 @@ namespace diagnostics { JointTelemetry jt; // Joint data - jt.eta = j.eta; - jt.thetaRad = j.thetaRad; - jt.omegaRad_s = j.omegaRad_s; - jt.torqueNm = j.torque; - jt.damping = j.dynamics.damping; - jt.friction = j.dynamics.friction; - jt.effort = (j.limits.maxEffort > 0.0f) ? (j.torque / j.limits.maxEffort) : 0.0f; + jt.q = j.q; + jt.qd = j.qd; + jt.eta = j.eta; // Reference data - jt.thetaRefRad = j.thetaRefRad; - jt.omegaRefRad_s = j.omegaRefRad_s; - jt.alphaRefRad_s2 = j.alphaRefRad_s2; + jt.q_ref = j.q_ref; + jt.qd_ref = j.qd_ref; + jt.qdd_ref = j.qdd_ref; + + // Dynamics Data + jt.torqueNm = j.torque; + jt.damping = j.dynamics.damping; + jt.friction = j.dynamics.friction; + jt.effort = (j.limits.maxEffort > 0.0f) ? (j.torque / j.limits.maxEffort) : 0.0f; // Clamping flags - jt.clampTheta = (j.thetaRad <= j.limits.minAngle) || (j.thetaRad >= j.limits.maxAngle); - jt.clampOmega = (j.omegaRad_s <= 0.0f) || (j.omegaRad_s >= j.limits.maxOmegaRad_s); + jt.clampTheta = (j.q <= j.limits.minAngle) || (j.q >= j.limits.maxAngle); + jt.clampOmega = (std::abs(j.qd) >= j.limits.maxqd); + + // Accumulate clamping events + clampThetaSum += (int)jt.clampTheta; + clampOmegaSum += (int)jt.clampOmega; clampSum += (int)jt.clampTheta + (int)jt.clampOmega; // Trajectory data @@ -61,7 +69,7 @@ namespace diagnostics { } // Joint error - const double e = jt.thetaRefRad - jt.thetaRad; + const double e = jt.q_ref - jt.q; sum_e2 += e * e; // Max absolute error and worst joint @@ -72,10 +80,12 @@ namespace diagnostics { } // Error statistics - s.err_rms = (n > 0) ? std::sqrt(sum_e2 / (double)n) : 0.0f; // RMS error - s.err_max = max_abs_e; // max error - s.worst_joint = worstJ; // index of worst joint - s.clamp_sum = clampSum; // total clamping events + s.err_rms = (n > 0) ? std::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 + s.clamp_sum = clampSum; // total clamping events + s.worst_joint = worstJ; // index of worst joint // Finalize write ring.endWrite(); diff --git a/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp b/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp index 264d01c..2384cb9 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp @@ -123,28 +123,28 @@ namespace commands { return setJointTargetRad(link, targetRad); } - utils::OpResult CommandContextMotion::setJointMaxOmegaRad(const std::string& link, double maxOmegaRad_s) { + utils::OpResult CommandContextMotion::setJointMaxOmegaRad(const std::string& link, double maxqd) { if (!_robot) { return OpResult::Failure("No robot loaded."); } - if (maxOmegaRad_s <= 0.0) { return OpResult::Failure("Max omega must be positive."); } - if (!_robot->trySetJointOmegaMaxRad(link, maxOmegaRad_s)) { + if (maxqd <= 0.0) { return OpResult::Failure("Max omega must be positive."); } + if (!_robot->trySetJointOmegaMaxRad(link, maxqd)) { return OpResult::Failure("Failed to set joint max omega -> Joint not found or invalid value."); } return OpResult::Success(true); } // Sets the reference angular velocity for a joint (rad/s) - utils::OpResult CommandContextMotion::setJointOmegaRefRad(const std::string& link, double omegaRefRad_s) { + utils::OpResult CommandContextMotion::setJointOmegaRefRad(const std::string& link, double qd_ref) { if (!_robot) { return OpResult::Failure("No robot loaded."); } - if (!_robot->trySetJointOmegaRefRad(link, omegaRefRad_s)) { + if (!_robot->trySetJointOmegaRefRad(link, qd_ref)) { return OpResult::Failure("Failed to set joint omega ref -> Joint not found or invalid value."); } return OpResult::Success(true); } // Sets the reference angular acceleration for a joint (rad/s^2) - utils::OpResult CommandContextMotion::setJointAlphaRefRad(const std::string& link, double alphaRefRad_s2) { + utils::OpResult CommandContextMotion::setJointAlphaRefRad(const std::string& link, double qdd_ref) { if (!_robot) { return OpResult::Failure("No robot loaded."); } - if (!_robot->trySetJointAlphaRefRad(link, alphaRefRad_s2)) { + if (!_robot->trySetJointAlphaRefRad(link, qdd_ref)) { return OpResult::Failure("Failed to set joint alpha ref -> Joint not found or invalid value."); } return OpResult::Success(true); diff --git a/Sim_Engine/EngineLib/src/Platform/WindowManager.cpp b/Sim_Engine/EngineLib/src/Platform/WindowManager.cpp index e01a193..1ff5508 100644 --- a/Sim_Engine/EngineLib/src/Platform/WindowManager.cpp +++ b/Sim_Engine/EngineLib/src/Platform/WindowManager.cpp @@ -152,21 +152,27 @@ namespace window { // Input handling void window::GLWindow::setMouseCaptured(bool captured) { _mouseCaptured = captured; - GLFWwindow* w = _window; - if (!w) return; + if (!w) { return; } + // When mouse is captured, disable the cursor and enable raw mouse motion for high-precision input if (_mouseCaptured) { + // When mouse is captured, disable the cursor and enable raw mouse motion for high-precision input glfwSetInputMode(w, GLFW_CURSOR, GLFW_CURSOR_DISABLED); - if (glfwRawMouseMotionSupported()) + if (glfwRawMouseMotionSupported()) { glfwSetInputMode(w, GLFW_RAW_MOUSE_MOTION, GLFW_TRUE); + } + ImGui::GetIO().ConfigFlags |= ImGuiConfigFlags_NoMouse; // tell SimManager to reset its first-mouse state - if (_sim) _sim->resetMouseDelta(); + if (_sim) { _sim->resetMouseDelta(); } } else { + // When mouse is released, show the cursor and disable raw mouse motion glfwSetInputMode(w, GLFW_CURSOR, GLFW_CURSOR_NORMAL); - if (glfwRawMouseMotionSupported()) + if (glfwRawMouseMotionSupported()) { glfwSetInputMode(w, GLFW_RAW_MOUSE_MOTION, GLFW_FALSE); + } + ImGui::GetIO().ConfigFlags &= ~ImGuiConfigFlags_NoMouse; } } @@ -174,9 +180,8 @@ namespace window { void window::GLWindow::onKey(int key, int /*scancode*/, int action, int /*mods*/) { if (action == GLFW_PRESS && key == GLFW_KEY_ESCAPE) { setMouseCaptured(!_mouseCaptured); - // close control panel when key is pressed again controlPanelOpen = !controlPanelOpen; - ImGui::SetWindowFocus(nullptr); // clear focus + ImGui::SetWindowFocus(nullptr); return; } } diff --git a/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp index ebd2c5b..e633c19 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp @@ -4,7 +4,6 @@ #include "Robots/RobotDynamics.h" #include "Robots/RobotKinematics.h" #include "Robots/RobotModel.h" -#include "Robots/RobotCommon.h" #include "Robots/TrajectoryManager.h" #include #include @@ -14,8 +13,7 @@ namespace robots { // Constructor RobotDynamics::RobotDynamics(RobotModel& robot, eTorqueMode mode) - : _robot(robot), _torqueMode(mode) { - _kinematics = std::make_unique(robot); + : _robot(robot), _kinematics(std::make_unique(robot)) { } // Computes the inertia tensor of a robot link @@ -198,12 +196,12 @@ namespace robots { const std::vector& eta, const std::vector& /*T_world*/, std::vector I_eff, - std::vector tau_gravity + std::vector tau_g ) const { const size_t n = _robot.joints.size(); VecX tau = VecX::Zero(n); // [Nm], torque for each joint - switch (_torqueMode) { + switch (_robot.torqueMode) { case eTorqueMode::PASSIVE: // Compute passive damping and friction torques for (size_t i = 0; i < n; ++i) { @@ -226,11 +224,10 @@ namespace robots { // Compute control torque using the computed metrics for this joint RobotMetrics m = computeJointMetrics( - j, _robot.links[i + 1], - I_eff[i], + j, I_eff[i], q[i], qd[i], eta[i], - j.thetaRefRad, j.omegaRefRad_s, j.alphaRefRad_s2, - 0.0, tau_gravity[i] + j.q_ref, j.qd_ref, j.qdd_ref, + 0.0, tau_g[i] ); tau[i] = m.tau; } @@ -241,14 +238,13 @@ namespace robots { // Computes control and dynamics metrics for a specific joint based on the current state and reference RobotMetrics RobotDynamics::computeJointMetrics( - const RobotJoint& joint, const RobotLink& link, - double I_eff, + const RobotJoint& joint, double I_eff, double q, double qd, double eta, double q_ref, double qd_ref, double qdd_ref, - double tau_coriolis, double tau_g + double tau_c, double tau_g ) const { RobotMetrics m{}; - if (_torqueMode == eTorqueMode::NONE) { + if (_robot.torqueMode == eTorqueMode::NONE) { // Current states m.theta = q; // [rad] m.omega = qd; // [rad/s] @@ -268,29 +264,33 @@ namespace robots { } // Current states - m.theta = qd; // [rad] - m.omega = qd; // [rad/s] + m.theta = q; // [rad] + m.omega = qd; // [rad/s] // Errors - m.err = q_ref - q; // [rad] + m.err = q_ref - q; // [rad] m.err_d = qd_ref - qd; // [rad/s] // Effective inertia m.I_eff = I_eff; // [kg*m^2] + // Energy metrics + m.KE = 0.5 * I_eff * qd * qd; // [J], kinetic energy of the joint + double P_grav = tau_g * qd; // [W], power due to gravity torque + m.PE += -P_grav * dt(); // [J], potential energy proxy based on gravity power (scaled down for interpretability) + m.E_total = m.KE + m.PE; // [J], total mechanical energy of the joint + // Control parameters const double wn = joint.wn_target; // [rad/s], natural frequency - const double z = joint.zeta_target; // damping ratio - const double b = joint.beta_target; // overshoot ratio + const double z = joint.zeta_target; // damping ratio + const double b = joint.beta_target; // overshoot ratio // Compute PID gains double k_p = m.I_eff * wn * wn; // [Nm/rad], proportional gain double k_i = b * k_p * wn; // [Nm/(rad*s)], integral gain double k_d = 2.0 * z * m.I_eff * wn; // [Nm/(rad/s)], derivative gain - // Integral term + // Integral term with anti-windup double tau_i = k_i * eta; - - // Integral anti-windup if (joint.limits.maxEffort > 0.0f) { const double rho = 0.3; // fraction of max effort allocated to I-term const double tau_i_max = rho * joint.limits.maxEffort; @@ -301,7 +301,8 @@ namespace robots { double tau_fb = k_p * m.err + tau_i + k_d * m.err_d; // Feedforward term based on reference acceleration and passive dynamics compensation - double tau_ff = m.I_eff * qdd_ref + tau_coriolis; + double tau_ff = m.I_eff * qdd_ref + tau_c; + if (_robot.torqueMode == eTorqueMode::CONTROLLED) { tau_ff += tau_g; } // Passive dynamics const double c = joint.dynamics.damping; @@ -310,27 +311,33 @@ namespace robots { // Friction model (viscous + Coulomb/Stribeck) double tau_damping{ 0.0 }, tau_friction{ 0.0 }; - tau_damping = c * qd; // viscous damping - tau_friction = mu * std::tanh(qd / v_eps); // Coulomb friction + tau_damping = c * qd; + tau_friction = mu * std::tanh(qd / v_eps); // Net torque m.tau = tau_fb + tau_ff - (tau_damping + tau_friction); // [Nm], net torque applied to the joint after passive dynamics // Cache torques in metrics - m.tau_fb = tau_fb; - m.tau_coriolis = tau_coriolis; - m.tau_gravity = tau_g; - m.tau_damping = tau_damping; // [+] viscous damping torque - m.tau_friction = tau_friction; // [+] Coulomb friction torque - - // Hip reaction compensation (if base is free-floating, apply a fraction of the last measured base forward force as a counter-torque to the hip pitch joint to help stabilise the base) + m.tau_fb = tau_fb; // [Nm], feedback control torque + m.tau_coriolis = tau_c; // [Nm], Coriolis and centrifugal torque + m.tau_gravity = tau_g; // [Nm], gravity torque + m.tau_damping = tau_damping; // [Nm], viscous damping torque + m.tau_friction = tau_friction; // [Nm], coulomb friction torque + + // Cache Work and Power metrics + m.W_actuator = m.tau * qd; // [W], actuator power (positive for power generation, negative for power consumption) + m.P_damping = tau_damping * qd; // [W], power dissipated by damping + m.P_friction = tau_friction * qd; // [W], power dissipated by friction + + // Hip reaction compensation if (_baseIsFree && joint.name.find("hip_pitch") != std::string::npos) { const double hipReactionGain = 0.7; + // If base is free-floating, apply a fraction of the last measured base forward force as a counter-torque to the hip pitch joint to help stabilise the base m.tau -= hipReactionGain * _lastBaseForwardForce; } // Torque saturation and velocity soft limits only in CONTROLLED mode - if (_torqueMode == eTorqueMode::CONTROLLED) { + if (_robot.torqueMode == eTorqueMode::CONTROLLED) { double tau_preSat = m.tau; // Effort clamp @@ -338,11 +345,12 @@ namespace robots { const double E_max = joint.limits.maxEffort; /*m.tau = std::clamp(m.tau, -E_max, E_max);*/ } + m.tau_sat = tau_preSat - m.tau; m.sat_flag = (m.tau_sat != 0.0); // Velocity soft limit - const double wMax_hw = std::abs(joint.limits.maxOmegaRad_s); + const double wMax_hw = std::abs(joint.limits.maxqd); const double wMax_traj = std::abs(joint.limits.omegaRefMaxRad_s); // or derived from trajectory manager double tau_preBarrier = m.tau; @@ -350,8 +358,8 @@ namespace robots { // Apply soft velocity barrier /*applyOmegaBarrier(m.tau, omega, wMax_hw, m.I_eff);*/ + // Cache barrier torque and overspeed metrics m.tau_barrier = tau_preBarrier - m.tau; - m.wMax_hw = wMax_hw; m.wMax_traj = wMax_traj; m.traj_overspeed = std::max(0.0, std::abs(qd) - wMax_traj); @@ -397,28 +405,26 @@ namespace robots { } // Compute mass matrix M(q) - MatX M_full = computeMassMatrix(q, T_world); + MatX M_full = computeMassMatrix(q, T_world); // [kg*m^2], full mass matrix for the robot at configuration q // Compute gravity torques for each joint std::vector tau_gravity(n, 0.0); + // Compute applied torques based on control mode and current state + VecX tau = VecX::Zero(n); + // Compute gravity torques if in a torque mode that requires it - if (_torqueMode != eTorqueMode::NONE) { + if (_robot.torqueMode != eTorqueMode::NONE) { // Compute gravity torque tau_gravity = computeGravityTorque(q, T_world, x); - } - - // Compute applied torques based on control mode and current state - VecX tau = VecX::Zero(n); - if (_torqueMode != eTorqueMode::NONE) { // Compute applied torques based on control mode tau = computeAppliedTorques(q, qd, eta, T_world, I_eff, tau_gravity); } // Build list of active (non-fixed) joints - std::vector active; + std::vector active; for (size_t i = 0; i < n; ++i) { if (_robot.joints[i].type != eJointType::FIXED) { - active.push_back((int)i); + active.push_back(i); } } const size_t m = active.size(); @@ -430,11 +436,12 @@ namespace robots { // Fill reduced mass matrix and torque vectors for active joints for (size_t r = 0; r < m; ++r) { - int i = active[r]; + size_t i = active[r]; tau_r[r] = tau[i]; // applied torque for active joint i G_r[r] = tau_gravity[i]; // gravity torque for active joint i + // Fill the reduced mass matrix row for active joint i for (size_t c = 0; c < m; ++c) { int j = active[c]; M(r, c) = M_full(i, j); @@ -449,18 +456,15 @@ namespace robots { // Debugging info about the reduced system LOG_INFO_ONCE("Reduced system size = %zu", m); - double rcond = M.fullPivLu().rcond(); LOG_INFO_ONCE("Reduced M rcond: %.6e", rcond); - Eigen::JacobiSVD svd(M); - LOG_INFO_ONCE("Reduced min singular value: %.6e", - svd.singularValues().minCoeff()); + LOG_INFO_ONCE("Reduced min singular value: %.6e", svd.singularValues().minCoeff()); // Solved for qdd Eigen::CompleteOrthogonalDecomposition cod(M); VecX qdd_r; - if (_torqueMode == eTorqueMode::NONE) { + if (_robot.torqueMode == eTorqueMode::NONE) { qdd_r = cod.solve(tau_r); // tau_r = 0, so checks for consistency of M } else { @@ -491,7 +495,7 @@ namespace robots { } // Compute error for integral term - double err_i = _robot.joints[i].thetaRefRad - q[i]; + double err_i = _robot.joints[i].q_ref - q[i]; // For revolute and prismatic joints, fill in the derivatives dx[i] = qd[i]; diff --git a/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp b/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp index 94c4bcf..2306485 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp @@ -281,7 +281,7 @@ namespace robots { joint.limits.continuous = false; joint.limits.minAngle = 0.0f; joint.limits.maxAngle = 0.0f; - joint.limits.maxOmegaRad_s = 0.0f; + joint.limits.maxqd = 0.0f; joint.limits.maxEffort = 0.0f; if (!jointData.contains("limits") || !jointData["limits"].is_object()) { LOG_WARN("Joint %s missing 'limits' block", joint.name.c_str()); return; } @@ -289,7 +289,7 @@ namespace robots { const auto& L = jointData["limits"]; joint.limits.continuous = L.value("continuous", false); - joint.limits.maxOmegaRad_s = L.value("velocity", joint.limits.maxOmegaRad_s); + joint.limits.maxqd = L.value("velocity", joint.limits.maxqd); joint.limits.maxEffort = L.value("effort", joint.limits.maxEffort); if (!joint.limits.continuous) { @@ -322,24 +322,6 @@ namespace robots { if (joint.dynamics.friction < 0.0f) { joint.dynamics.friction = 0.0f; } } - // Parse joint control parameters - static void parseJointControl(const json& jointData, RobotJoint& joint) { - joint.k_p = 25.0f; - joint.k_d = 8.0f; - joint.thetaRefRad = 0.0f; - - if (!jointData.contains("control") || !jointData["control"].is_object()) return; - - const auto& C = jointData["control"]; - joint.k_p = C.value("k_p", joint.k_p); - joint.k_d = C.value("k_d", joint.k_d); - - if (C.contains("maxOmega") && C["maxOmega"].is_number()) { - double maxOmega = C["maxOmega"].get(); - if (maxOmega > 0.0f) joint.limits.maxOmegaRad_s = maxOmega; - } - } - // Check if a joint is fixed based on its type string static bool isFixedJoint(const json& jointData) { if (!jointData.contains("type")) return false; @@ -472,7 +454,6 @@ namespace robots { parseJointAxis(jointData, joint); parseJointLimits(jointData, joint); parseJointDynamics(jointData, joint); - parseJointControl(jointData, joint); } robot.joints.push_back(joint); @@ -491,9 +472,9 @@ namespace robots { } 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.maxOmegaRad_s, joint.limits.minAngle, joint.limits.maxAngle); + 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.maxOmegaRad_s, joint.limits.minAngle, joint.limits.maxAngle); + 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); } diff --git a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp index c37aa83..ef0ba10 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp @@ -9,7 +9,6 @@ #include "Robots/RobotKinematics.h" #include "Robots/RobotDynamics.h" #include "Robots/RobotLoader.h" -#include "Robots/RobotCommon.h" #include #include @@ -234,8 +233,8 @@ namespace robots { auto& j = _robot.joints[i]; // Current states - x[i] = j.thetaRad; - x[i + n] = j.omegaRad_s; + x[i] = j.q; + x[i + n] = j.qd; x[i + 2 * n] = j.eta; // integral state } return x; // state vector @@ -262,7 +261,7 @@ namespace robots { double theta_out = clampJointAngle(j, theta_in); // max |omega| - double wMax_hw = std::abs(j.limits.maxOmegaRad_s); + double wMax_hw = std::abs(j.limits.maxqd); double omega_out = omega_in; // Velocity limit clamping @@ -287,8 +286,8 @@ namespace robots { _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; // Update joint states - j.thetaRad = theta_out; - j.omegaRad_s = omega_out; + j.q = theta_out; + j.qd = omega_out; j.eta = eta_in; } } @@ -301,8 +300,8 @@ namespace robots { auto& j = _robot.joints[i]; // Pack reference angles and velocities - x[i] = (double)j.thetaRefRad; - x[i + n] = (double)j.omegaRefRad_s; + x[i] = (double)j.q_ref; + x[i + n] = (double)j.qd_ref; } return x; // reference state vector } @@ -312,10 +311,10 @@ namespace robots { const size_t n = (int)_robot.joints.size(); for (size_t i = 0; i < n; ++i) { auto& j = _robot.joints[i]; - j.thetaRefRad = x[i]; // [rad] - j.omegaRefRad_s = x[i + n]; // [rad/s] + j.q_ref = x[i]; // [rad] + j.qd_ref = x[i + n]; // [rad/s] // Clamp reference angle to joint limits - j.thetaRefRad = clampJointAngle(j, j.thetaRefRad); // [rad] + j.q_ref = clampJointAngle(j, j.q_ref); // [rad] } } @@ -326,8 +325,8 @@ namespace robots { const double lo = j.limits.minAngle; const double hi = j.limits.maxAngle; - if (j.thetaRad < lo) { j.thetaRad = lo; if (j.omegaRad_s < 0.0f) { j.omegaRad_s = 0.0f; }} - if (j.thetaRad > hi) { j.thetaRad = hi; if (j.omegaRad_s > 0.0f) { j.omegaRad_s = 0.0f; }} + if (j.q < lo) { j.q = lo; if (j.qd < 0.0f) { j.qd = 0.0f; }} + if (j.q > hi) { j.q = hi; if (j.qd > 0.0f) { j.qd = 0.0f; }} } // Method to advance the robot state by dt using the selected integrator @@ -344,13 +343,15 @@ namespace robots { auto step = _integrator->stepODE(_curIntMethod, x, simTime, dt, f); mathlib::VecX x_Next = step.x_next; + _dynamics->setDt(step.dt_taken); + // Unpack new state unpackState(x_Next); // Enforce joint limits for (auto& j : _robot.joints) { - const double wMax = j.limits.maxOmegaRad_s; - /*if (wMax > 0.0f) { j.omegaRad_s = glm::clamp(j.omegaRad_s, -wMax, wMax); }*/ + const double wMax = j.limits.maxqd; + /*if (wMax > 0.0f) { j.qd = glm::clamp(j.qd, -wMax, wMax); }*/ enforceJointLimits(j); } @@ -360,10 +361,10 @@ namespace robots { std::vector jointWorldPose = _kinematics->calcJointWorldPoses(T_world, _robot.joints); // compute gravity torques for new state so logs match dynamics - std::vector tau_gravity(n, 0.0); - std::vector theta(n); - for (size_t i = 0; i < n; ++i) theta[i] = _robot.joints[i].thetaRad; - tau_gravity = _dynamics->computeGravityTorque(theta, T_world, x); + std::vector tau_g(n, 0.0); + std::vector q(n); + for (size_t i = 0; i < n; ++i) q[i] = _robot.joints[i].q; + tau_g = _dynamics->computeGravityTorque(q, T_world, x); // Compute effective inertia for each joint at the new state std::vector I_eff(n, 0.0); @@ -390,26 +391,16 @@ namespace robots { I_eff[i] = std::max(I_eff[i], 1e-6); } + // Compute and log metrics for each joint at the new state for (size_t i = 0; i < n; ++i) { - const auto& joint = _robot.joints[i]; - const auto& link = _robot.links[i + 1]; - - // Current states - const double theta = joint.thetaRad; - const double omega = joint.omegaRad_s; - const double eta = joint.eta; - - // Use integrated reference (baseline truth) - const double thetaRef = joint.thetaRefRad; - const double omegaRef = joint.omegaRefRad_s; - const double alphaRef = joint.alphaRefRad_s2; + const auto& j = _robot.joints[i]; // Compute joint metrics RobotMetrics m = _dynamics->computeJointMetrics( - joint, link, I_eff[i], - theta, omega, eta, - thetaRef, omegaRef, alphaRef, - 0.0,tau_gravity[i] + j, I_eff[i], + j.q, j.qd, j.eta, + j.q_ref, j.qd_ref, j.qdd_ref, + 0.0,tau_g[i] ); // Log metrics to buffer if logging is enabled @@ -437,6 +428,13 @@ namespace robots { buf->tau_friction.push_back(m.tau_friction); buf->tau_barrier.push_back(m.tau_barrier); buf->tau_sat.push_back(m.tau_sat); + // Energy, Work, & Power + buf->KE.push_back(m.KE); + buf->PE.push_back(m.PE); + buf->E_total.push_back(m.E_total); + buf->W_actuator.push_back(m.W_actuator); + buf->P_damping.push_back(m.P_damping); + buf->P_friction.push_back(m.P_friction); // Limit flags and info buf->clamp_theta.push_back((double)_clampTheta[i]); buf->clamp_omega.push_back((double)_clampOmega[i]); @@ -469,14 +467,14 @@ namespace robots { control::TrajState s{}; // Try to evaluate trajectory if (traj.tryEval(std::string(j.child), t, s)) { - j.thetaRefRad = clampJointAngle(j, s.q); // set ref angle - j.omegaRefRad_s = s.qd; - j.alphaRefRad_s2 = s.qdd; + j.q_ref = clampJointAngle(j, s.q); // set ref angle + j.qd_ref = s.qd; + j.qdd_ref = s.qdd; } // Store inputs else { - j.alphaRefRad_s2 = 0.0f; - j.omegaRefRad_s = 0.0f; + j.qdd_ref = 0.0f; + j.qd_ref = 0.0f; } auto* buf = _refBuffer; @@ -484,9 +482,9 @@ namespace robots { // Sim Metadata buf->sim_time.push_back(t); // Reference states - buf->theta_ref.push_back(j.thetaRefRad); - buf->omega_ref.push_back(j.omegaRefRad_s); - buf->alpha_ref.push_back(j.alphaRefRad_s2); + buf->theta_ref.push_back(j.q_ref); + buf->omega_ref.push_back(j.qd_ref); + buf->alpha_ref.push_back(j.qdd_ref); // Joint Index buf->joint_index.push_back((int)i); } @@ -551,10 +549,10 @@ namespace robots { _robot.setJointVector(_robotQHome); for (auto& joint : _robot.joints) { - joint.omegaRad_s = 0.0f; - joint.thetaRefRad = joint.thetaRad; - joint.omegaRefRad_s = 0.0f; - joint.alphaRefRad_s2 = 0.0f; + joint.qd = 0.0f; + joint.q_ref = joint.q; + joint.qd_ref = 0.0f; + joint.qdd_ref = 0.0f; } // Reset base state if free-floating @@ -605,8 +603,8 @@ namespace robots { void RobotSystem::stopAll() { if (!_hasRobot) return; for (auto& joint : _robot.joints) { - joint.omegaRad_s = 0.0f; - joint.thetaRefRad = joint.thetaRad; + joint.qd = 0.0f; + joint.q_ref = joint.q; } } @@ -689,11 +687,11 @@ namespace robots { // Apply joint rotation for revolute joints if (j.type == eJointType::REVOLUTE) { - glm::mat4 R_q = glm::rotate(glm::mat4(1.0f), static_cast(j.thetaRad), glm::normalize(toGlm(j.axis))); + glm::mat4 R_q = glm::rotate(glm::mat4(1.0f), static_cast(j.q), glm::normalize(toGlm(j.axis))); T_child = T_child * R_q; } else if (j.type == eJointType::PRISMATIC) { - glm::mat4 T_q = glm::translate(glm::mat4(1.0f), glm::normalize(toGlm(j.axis)) * static_cast(j.thetaRad)); + glm::mat4 T_q = glm::translate(glm::mat4(1.0f), glm::normalize(toGlm(j.axis)) * static_cast(j.q)); T_child = T_child * T_q; } @@ -758,7 +756,7 @@ namespace robots { // Find joint child matching childLink for (const auto& joint : _robot.joints) { if (joint.child == childLink) { - outAngle = joint.thetaRad; + outAngle = joint.q; return true; } } @@ -771,7 +769,7 @@ namespace robots { // Find joint child matching childLink for (auto& joint : _robot.joints) { if (joint.child == childLink) { - joint.thetaRad = clampJointAngle(joint, angleRad); // clamp to joint limits + joint.q = clampJointAngle(joint, angleRad); // clamp to joint limits return true; } } @@ -784,7 +782,7 @@ namespace robots { // Find joint child matching childLink for (const auto& joint : _robot.joints) { if (joint.child == childLink) { - outOmega = joint.omegaRad_s; + outOmega = joint.qd; return true; } } @@ -797,7 +795,7 @@ namespace robots { // Find joint child matching childLink for (auto& joint : _robot.joints) { if (joint.child == childLink) { - joint.omegaRad_s = omegaRad; + joint.qd = omegaRad; return true; } } @@ -826,7 +824,7 @@ namespace robots { if (!_hasRobot) { return false; } for (const auto& joint : _robot.joints) { if (joint.child == childLink) { - outTargetRad = joint.thetaRefRad; + outTargetRad = joint.q_ref; return true; } } @@ -838,8 +836,8 @@ namespace robots { if (!_hasRobot) { return false; } for (auto& joint : _robot.joints) { if (joint.child == childLink) { - if (joint.limits.continuous) { joint.thetaRefRad = wrapRad(targetRad); } - else { joint.thetaRefRad = clampJointAngle(joint, targetRad); } // clamp to joint + if (joint.limits.continuous) { joint.q_ref = wrapRad(targetRad); } + else { joint.q_ref = clampJointAngle(joint, targetRad); } // clamp to joint return true; } } @@ -851,7 +849,7 @@ namespace robots { if (!_hasRobot) { return false; } for (const auto& joint : _robot.joints) { if (joint.child == childLink) { - maxOmegaRad = joint.limits.maxOmegaRad_s; + maxOmegaRad = joint.limits.maxqd; return true; } } @@ -864,7 +862,7 @@ namespace robots { if (maxOmegaRad <= 0.0f) { return false; } for (auto& joint : _robot.joints) { if (joint.child == childLink) { - joint.limits.maxOmegaRad_s = maxOmegaRad; + joint.limits.maxqd = maxOmegaRad; return true; } } @@ -876,10 +874,10 @@ namespace robots { if (!_hasRobot) { return false; } for (auto& joint : _robot.joints) { if (joint.child == childLink) { - double t = joint.thetaRefRad + deltaRad; + double t = joint.q_ref + deltaRad; if (joint.limits.continuous) { t = wrapRad(t); } else { t = glm::clamp(t, joint.limits.minAngle, joint.limits.maxAngle); } - joint.thetaRefRad = t; // clamp to joint limits + joint.q_ref = t; // clamp to joint limits return true; } } @@ -891,7 +889,7 @@ namespace robots { if (!_hasRobot) { return false; } for (auto& joint : _robot.joints) { if (joint.child == childLink) { - joint.omegaRefRad_s = omegaRefRad; + joint.qd_ref = omegaRefRad; return true; } } @@ -903,7 +901,7 @@ namespace robots { if (!_hasRobot) { return false; } for (auto& joint : _robot.joints) { if (joint.child == childLink) { - joint.alphaRefRad_s2 = alphaRefRad; + joint.qdd_ref = alphaRefRad; return true; } } @@ -927,8 +925,8 @@ namespace robots { bool RobotSystem::tryZeroJointRefDerivatives() { if (!_hasRobot) { return false; } for (auto& joint : _robot.joints) { - joint.omegaRefRad_s = 0.0f; - joint.alphaRefRad_s2 = 0.0f; + joint.qd_ref = 0.0f; + joint.qdd_ref = 0.0f; } return true; } @@ -940,7 +938,7 @@ namespace robots { for (const auto& joint : _robot.joints) { if (joint.child == childLink) { - double err = joint.thetaRefRad - joint.thetaRad; + double err = joint.q_ref - joint.q; if (joint.limits.continuous) { err = wrapToPi(err); } err = std::abs(err); return err <= tolRad; @@ -961,7 +959,7 @@ namespace robots { for (const auto& joint : _robot.joints) { if (joint.child == childLink) { - double err = targetRad - joint.thetaRad; + double err = targetRad - joint.q; if (joint.limits.continuous) { err = wrapToPi(err); } return std::abs(err) <= tolRad; } @@ -980,7 +978,7 @@ namespace robots { bool RobotSystem::setRobotLinkRotation(const std::string& childLinkName, double angleDeg) { for (auto& j : _robot.joints) { if (j.child == childLinkName) { - j.thetaRad = glm::radians(angleDeg); + j.q = glm::radians(angleDeg); updateRobotKinematics(); return true; } @@ -1018,7 +1016,7 @@ namespace robots { double drive = 0.0; for (const auto& j : _robot.joints) { if (j.name.find("hip_pitch") != std::string::npos) { - drive += -j.omegaRad_s; + drive += -j.qd; } } return drive; @@ -1084,8 +1082,5 @@ namespace robots { } // Set the torque mode for the robot system - void RobotSystem::setTorqueMode(eTorqueMode mode) { - _torqueMode = mode; - _dynamics->setTorqueMode(mode); - } + void RobotSystem::setTorqueMode(eTorqueMode mode) { _robot.torqueMode = mode; } } // namespace robots \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index faf60ef..ac89f55 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -192,6 +192,13 @@ namespace core { fields.emplace_back("tau_friction", (double)_jointLogBuffer.tau_friction[i]); fields.emplace_back("tau_barrier", (double)_jointLogBuffer.tau_barrier[i]); fields.emplace_back("tau_sat", (double)_jointLogBuffer.tau_sat[i]); + // Energy, Work, & Power + fields.emplace_back("KE", (double)_jointLogBuffer.KE[i]); + fields.emplace_back("PE", (double)_jointLogBuffer.PE[i]); + fields.emplace_back("E_total", (double)_jointLogBuffer.E_total[i]); + fields.emplace_back("W_actuator", (double)_jointLogBuffer.W_actuator[i]); + fields.emplace_back("P_damping", (double)_jointLogBuffer.P_damping[i]); + fields.emplace_back("P_friction", (double)_jointLogBuffer.P_friction[i]); // Limit flags and info fields.emplace_back("clamp_theta", (double)_jointLogBuffer.clamp_theta[i]); fields.emplace_back("clamp_omega", (double)_jointLogBuffer.clamp_omega[i]); diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp index 37e9c17..1d4929a 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp @@ -1629,7 +1629,7 @@ namespace gui { // INPUT HANDLING // -------------------------------------------------- void gui::SimManager::processMovementKey(int key, float delta) { - if (_impl->viewMode == Impl::ViewMode::Quad) return; // No keyboard movement in quad view + if (_impl->viewMode == Impl::ViewMode::Quad) { return; }// No keyboard movement in quad view scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); if (ctrlMode == ControlMode::Camera) { cam->processKeyboard(key, delta); } else if (ctrlMode == ControlMode::Object && _impl->_mesh) { /*idea is to add multiple angles to switch between!*/ } @@ -1650,12 +1650,12 @@ namespace gui { } void gui::SimManager::handleMouseLook(GLFWwindow* window, double xpos, double ypos) { - if (_impl->viewMode == Impl::ViewMode::Quad) return; // No mouse look in quad view + if (_impl->viewMode == Impl::ViewMode::Quad) { return; } // No mouse look in quad view scene::Camera* cam = _impl->_views[static_cast(_impl->activeView)].cam.get(); auto* win = static_cast(glfwGetWindowUserPointer(window)); - if (!win || !win->isMouseCaptured()) return; + if (!win || !win->isMouseCaptured()) { return; } - bool captured = false; + bool captured = true; if (win == static_cast(glfwGetWindowUserPointer(window))) { captured = win->isMouseCaptured(); } if (!captured && !_isHovered) { diff --git a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp index 98c82a7..6315f68 100644 --- a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp +++ b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp @@ -5,11 +5,9 @@ #include "Scene/Mesh.h" #include "ui/ControlPanel.h" #include "Robots/RobotSystem.h" -#include "Robots/RobotCommon.h" #include "Platform/Paths.h" #include #include -#include #include "Platform/imguiWidgets.h" #include @@ -1013,8 +1011,8 @@ namespace gui { ImGui::TextUnformatted(joint.name.c_str()); if (ImGui::IsItemHovered()) { ImGui::BeginTooltip(); - ImGui::TextDisabled("Angle(rad): %.2f\nOmega(rad/s): %.2f\nk_p: %.2f\nk_d: %.2f\nDamping: %.2f\nFriction: %.2f", - joint.thetaRad, joint.omegaRad_s, joint.k_p, joint.k_d, joint.dynamics.damping, joint.dynamics.friction); + ImGui::TextDisabled("Angle(rad): %.2f\nOmega(rad/s): %.2f\nDamping: %.2f\nFriction: %.2f", + joint.q, joint.qd, joint.dynamics.damping, joint.dynamics.friction); ImGui::EndTooltip(); } if (jointSelected) ImGui::PopStyleColor(); @@ -1171,7 +1169,7 @@ namespace gui { } const diagnostics::JointTelemetry& j = s.j[selectedJoint]; - const float e = (float)(j.thetaRefRad - j.thetaRad); + const float e = (float)(j.q_ref - j.q); ImGui::Separator(); ImGui::TextDisabled("Robot loaded: %s", _requestedRobot.c_str()); @@ -1181,8 +1179,8 @@ namespace gui { ImGui::Spacing(); // Joint Info ImGui::SectionHeader("State:"); - ImGui::Text("theta: %.6f rad", j.thetaRad); - ImGui::Text("omega: %.6f rad/s", j.omegaRad_s); + ImGui::Text("theta: %.6f rad", j.q); + ImGui::Text("omega: %.6f rad/s", j.qd); ImGui::Text("damping: %.6f kg·m^2/s", j.damping); ImGui::Text("friction: %.6f N·m", j.friction); ImGui::Text("torque: %.6f N·m", j.torqueNm); @@ -1190,9 +1188,9 @@ namespace gui { ImGui::Spacing(); // Reference Info ImGui::SectionHeader("Reference:"); - ImGui::Text("theta_ref: %.6f rad", j.thetaRefRad); - ImGui::Text("omega_ref: %.6f rad/s", j.omegaRefRad_s); - ImGui::Text("alpha_ref: %.6f rad/s^2", j.alphaRefRad_s2); + ImGui::Text("theta_ref: %.6f rad", j.q_ref); + ImGui::Text("omega_ref: %.6f rad/s", j.qd_ref); + ImGui::Text("alpha_ref: %.6f rad/s^2", j.qdd_ref); ImGui::Text("error e: %.6f drad", e); ImGui::Spacing(); @@ -1216,7 +1214,7 @@ namespace gui { float worstErr = 0.0f; int worstIdx = 0; for (int i = 0; i < (int)s.j.size(); ++i) { - float err = (float)std::abs(s.j[i].thetaRefRad - s.j[i].thetaRad); + float err = (float)std::abs(s.j[i].q_ref - s.j[i].q); if (err > worstErr) { worstErr = err; worstIdx = i; } } selectedJoint = worstIdx; @@ -1239,26 +1237,26 @@ namespace gui { return; } - const int sampleCount = (int)ring.size(); - const int jointCount = (int)ring.at(sampleCount - 1).j.size(); + const size_t sampleCount = ring.size(); + const size_t jointCount = ring.at(sampleCount - 1).j.size(); // Header fprintf(f, "time_s,err_rms,err_max,clamp_sum"); - for (int j = 0; j < jointCount; ++j) { - fprintf(f, ",J%02d_theta,J%02d_omega,J%02d_torque,J%02d_theta_ref,J%02d_err", j+1, j+1, j+1, j+1, j+1); + for (size_t j = 0; j < jointCount; ++j) { + fprintf(f, ",J%02d_theta,J%02d_omega,J%02d_torque,J%02d_theta_ref,J%02d_err", (int)j+1, (int)j+1, (int)j+1, (int)j+1, (int)j+1); } fprintf(f, "\n"); // Data rows - for (int k = 0; k < sampleCount; ++k) { + for (size_t k = 0; k < sampleCount; ++k) { const auto& s = ring.at(k); fprintf(f, "%.6f,%.9f,%.9f,%d", s.timeSec, s.err_rms, s.err_max, s.clamp_sum); - const int m = std::min(jointCount, (int)s.j.size()); - for (int j = 0; j < m; ++j) { + const size_t m = std::min(jointCount, s.j.size()); + for (size_t j = 0; j < m; ++j) { const auto& jt = s.j[j]; fprintf(f, ",%.9f,%.9f,%.9f,%.9f,%.9f", - jt.thetaRad, jt.omegaRad_s, jt.torqueNm, jt.thetaRefRad, - jt.thetaRefRad - jt.thetaRad); + jt.q, jt.qd, jt.torqueNm, jt.q_ref, + jt.q_ref - jt.q); } fprintf(f, "\n"); } @@ -1297,7 +1295,10 @@ namespace gui { D_INFO("Starting integrator comparison (6 methods)..."); - for (int i = 0; i < 6; ++i) { + const size_t methodCount = sizeof(methods) / sizeof(methods[0]); + + // Loop through each integrator method, run the script, and capture telemetry for comparison + for (size_t i = 0; i < methodCount; ++i) { D_INFO(" Running: %s ...", names[i]); if (!_sim->runScriptToCompletion(scriptText, methods[i])) { @@ -1307,33 +1308,34 @@ namespace gui { // Snapshot telemetry into comparison result const auto& ring = _sim->telemetry().ring; - const int sampleCount = (int)ring.size(); - if (sampleCount < 2) continue; + const size_t sampleCount = ring.size(); + // Need at least 2 samples to plot error over time + if (sampleCount < 2) { continue; } + + // Create snapshot for this integrator ComparisonSnapshot snap; snap.integratorName = names[i]; - snap.jointCount = (int)ring.at(sampleCount - 1).j.size(); + snap.jointCount = ring.at(sampleCount - 1).j.size(); snap.time.resize(sampleCount); snap.errRms.resize(sampleCount); snap.errMax.resize(sampleCount); snap.jointErr.resize(snap.jointCount); - for (int j = 0; j < snap.jointCount; ++j) snap.jointErr[j].resize(sampleCount); + for (size_t j = 0; j < snap.jointCount; ++j) { snap.jointErr[j].resize(sampleCount); } - for (int k = 0; k < sampleCount; ++k) { + for (size_t k = 0; k < sampleCount; ++k) { const auto& s = ring.at(k); snap.time[k] = (float)s.timeSec; snap.errRms[k] = (float)s.err_rms; snap.errMax[k] = (float)s.err_max; - const int m = std::min(snap.jointCount, (int)s.j.size()); - for (int j = 0; j < m; ++j) { - snap.jointErr[j][k] = (float)(s.j[j].thetaRefRad - s.j[j].thetaRad); + const size_t m = std::min(snap.jointCount, s.j.size()); + for (size_t j = 0; j < m; ++j) { + snap.jointErr[j][k] = (float)(s.j[j].q_ref - s.j[j].q); } } - _comparisonResults.push_back(std::move(snap)); } - _comparisonReady = !_comparisonResults.empty(); D_SUCCESS("Integrator comparison complete: %d/%d methods captured.", (int)_comparisonResults.size(), 6); } @@ -1396,7 +1398,7 @@ namespace gui { // Find which joint has the most variation across integrators static int selectedCmpJoint = 0; ImGui::SetNextItemWidth(150.0f); - ImGui::SliderInt("Compare Joint##cmp", &selectedCmpJoint, 0, _comparisonResults.front().jointCount - 1, "J%02d"); + ImGui::SliderInt("Compare Joint##cmp", &selectedCmpJoint, 0, (int)_comparisonResults.front().jointCount - 1, "J%02d"); char plotLabel[64]; snprintf(plotLabel, sizeof(plotLabel), "J%02d Error - All Integrators##cmpjoint", selectedCmpJoint + 1); @@ -1433,12 +1435,12 @@ namespace gui { fprintf(f, "\n"); // Use longest time series - int maxSamples = 0; - for (const auto& res : _comparisonResults) { maxSamples = std::max(maxSamples, (int)res.time.size()); } + size_t maxSamples = 0; + for (const auto& res : _comparisonResults) { maxSamples = std::max(maxSamples, res.time.size()); } - for (int k = 0; k < maxSamples; ++k) { + for (size_t k = 0; k < maxSamples; ++k) { // Use first result's time as reference - float t = (k < (int)_comparisonResults[0].time.size()) ? _comparisonResults[0].time[k] : 0.0f; + float t = (k < _comparisonResults[0].time.size()) ? _comparisonResults[0].time[k] : 0.0f; fprintf(f, "%.6f", t); for (const auto& res : _comparisonResults) { if (k < (int)res.time.size()) { @@ -1505,8 +1507,8 @@ namespace gui { ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(8, 4)); ImGui::PushStyleColor(ImGuiCol_TitleBg, ImVec4(0.08f, 0.08f, 0.12f, 1.0f)); ImGui::PushStyleColor(ImGuiCol_TitleBgActive, ImVec4(0.15f, 0.18f, 0.28f, 1.0f)); - - ImGui::Begin("Simulation Results", &open, ImGuiWindowFlags_NoDocking); + ImGui::Begin("Simulation Results", &open, ImGuiWindowFlags_NoTitleBar); + if (!open) { _showResultsWindow = false; ImGui::End(); @@ -1564,19 +1566,21 @@ namespace gui { ImGui::Separator(); // --- Rebuild series from ring (reuse the same helpers) --- - static std::vector rX, rRms, rMax, rCs; + static std::vector rX, rRms, rMax, rCs, rCst, rCso; static std::vector> rY; - const int sampleCount = (int)ring.size(); - const int jointCount = (int)last.j.size(); + const size_t sampleCount = ring.size(); + const size_t jointCount = last.j.size(); rX.resize(sampleCount); rRms.resize(sampleCount); rMax.resize(sampleCount); rCs.resize(sampleCount); + rCst.resize(sampleCount); + rCso.resize(sampleCount); - if ((int)rY.size() != jointCount) rY.resize(jointCount); - for (int j = 0; j < jointCount; ++j) rY[j].resize(sampleCount); + if (rY.size() != jointCount) { rY.resize(jointCount); } + for (size_t j = 0; j < jointCount; ++j) { rY[j].resize(sampleCount); } for (int k = 0; k < sampleCount; ++k) { const auto& s = ring.at(k); @@ -1584,12 +1588,14 @@ namespace gui { rRms[k] = (float) s.err_rms; rMax[k] = (float)s.err_max; rCs[k] = (float)s.clamp_sum; + rCst[k] = (float)s.clamp_theta; + rCso[k] = (float)s.clamp_omega; - const int m = std::min(jointCount, (int)s.j.size()); - for (int j = 0; j < m; ++j) { - rY[j][k] = (float)(s.j[j].thetaRefRad - s.j[j].thetaRad); + const size_t m = std::min(jointCount, s.j.size()); + for (size_t j = 0; j < m; ++j) { + rY[j][k] = (float)(s.j[j].q_ref - s.j[j].q); } - for (int j = m; j < jointCount; ++j) { + for (size_t j = m; j < jointCount; ++j) { rY[j][k] = 0.0f; } } @@ -1613,8 +1619,8 @@ namespace gui { if (ImPlot::BeginPlot("Error (RMS, Max)##results", plotSz1)) { ImPlot::SetupAxes("t (s)", "error (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); ImPlot::SetupLegend(ImPlotLocation_NorthEast); - ImPlot::PlotLine("RMS", rX.data(), rRms.data(), sampleCount); - ImPlot::PlotLine("Max", rX.data(), rMax.data(), sampleCount); + ImPlot::PlotLine("RMS", rX.data(), rRms.data(), (int)sampleCount); + ImPlot::PlotLine("Max", rX.data(), rMax.data(), (int)sampleCount); ImPlot::EndPlot(); } @@ -1625,9 +1631,15 @@ namespace gui { // --- Clamp Events --- const ImVec2 tPlotSz2(_plotWidth, _plotH2); if (ImPlot::BeginPlot("Clamp Events##results", tPlotSz2)) { - ImPlot::SetupAxes("t (s)", "Clamp Sum", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); + ImPlot::SetupAxes("t (s)", "Sum", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); ImPlot::SetupLegend(ImPlotLocation_NorthEast); - ImPlot::PlotStairs("Sum", rX.data(), rCs.data(), sampleCount); + ImPlot::PlotStairs("Clamp Theta", rX.data(), rCst.data(), (int)sampleCount); + ImPlot::PlotStairs("Clamp Omega", rX.data(), rCso.data(), (int)sampleCount); + + ImPlot::PushStyleColor(ImPlotCol_Line, ImVec4(0.9f, 0.1f, 0.1f, 1.0f)); + ImPlot::PlotStairs("Clamp Sum", rX.data(), rCs.data(), (int)sampleCount); + ImPlot::PopStyleColor(); + ImPlot::EndPlot(); } @@ -1639,10 +1651,10 @@ namespace gui { if (ImPlot::BeginPlot("Joint Error Overlay##results", plotSz3)) { ImPlot::SetupAxes("t (s)", "e (rad)", ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); ImPlot::SetupLegend(ImPlotLocation_NorthEast); - for (int j = 0; j < jointCount; ++j) { + for (size_t j = 0; j < jointCount; ++j) { char label[16]; - snprintf(label, sizeof(label), "J%02d", j + 1); - ImPlot::PlotLine(label, rX.data(), rY[j].data(), sampleCount); + snprintf(label, sizeof(label), "J%02d", (int)j + 1); + ImPlot::PlotLine(label, rX.data(), rY[j].data(), (int)sampleCount); } ImPlot::EndPlot(); } From 60c7afc1e75988592812e1e2394d8899cc329d61 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 21 Feb 2026 15:48:03 +0000 Subject: [PATCH 08/20] update: going to add a basic double-buffer design to help testing runs --- .../EngineLib/include/Analysis/MetricLogger.h | 53 +++++++++++++---- .../EngineLib/include/Robots/RobotDynamics.h | 3 +- .../EngineLib/include/Robots/RobotSystem.h | 30 +--------- .../EngineLib/src/Robots/RobotDynamics.cpp | 10 ++-- .../EngineLib/src/Robots/RobotSystem.cpp | 57 +++++++------------ 5 files changed, 69 insertions(+), 84 deletions(-) diff --git a/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h b/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h index ac695b1..4942bc3 100644 --- a/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h +++ b/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h @@ -42,17 +42,14 @@ namespace robots { // Clear all logged data void clear() { - // Sim Metadata sim_time.clear(); dt_taken.clear(); dt_sug.clear(); - // States theta.clear(); omega.clear(); alpha.clear(); err.clear(); err_d.clear(); - // Dynamics I_eff.clear(); tau.clear(); tau_fb.clear(); @@ -62,34 +59,28 @@ namespace robots { tau_friction.clear(); tau_barrier.clear(); tau_sat.clear(); - // Energy, Work, & Power KE.clear(); PE.clear(); E_total.clear(); W_actuator.clear(); P_damping.clear(); P_friction.clear(); - // Limit flags and info clamp_theta.clear(); clamp_omega.clear(); sat_flag.clear(); - // Joint Index joint_index.clear(); } // Reserve space for a certain number of samples void reserve(size_t n) { - // Sim Metadata reserve sim_time.reserve(n); dt_taken.reserve(n); dt_sug.reserve(n); - // States reserve theta.reserve(n); omega.reserve(n); alpha.reserve(n); err.reserve(n); err_d.reserve(n); - // Dynamics reserve I_eff.reserve(n); tau.reserve(n); tau_fb.reserve(n); @@ -99,18 +90,15 @@ namespace robots { tau_friction.reserve(n); tau_barrier.reserve(n); tau_sat.reserve(n); - // Energy, Work, & Power reserve KE.reserve(n); PE.reserve(n); E_total.reserve(n); W_actuator.reserve(n); P_damping.reserve(n); P_friction.reserve(n); - // Limit flags and info reserve clamp_theta.reserve(n); clamp_omega.reserve(n); sat_flag.reserve(n); - // Joint index reserve joint_index.reserve(n); } @@ -118,6 +106,47 @@ namespace robots { size_t size() const { return theta.size(); } + + struct ENGINE_API JointLogEntry { + double sim_time; + double dt_taken; + double dt_sug; + double theta, omega, alpha, err, err_d; + double I_eff, tau, tau_fb, tau_coriolis, tau_gravity, tau_damping, tau_friction, tau_barrier, tau_sat; + double KE, PE, E_total, W_actuator, P_damping, P_friction; + double clamp_theta, clamp_omega, sat_flag; + int joint_index; + }; + + void push_entry(const JointLogEntry& e) { + sim_time.push_back(e.sim_time); + dt_sug.push_back(e.dt_sug); + dt_taken.push_back(e.dt_taken); + theta.push_back(e.theta); + omega.push_back(e.omega); + alpha.push_back(e.alpha); + err.push_back(e.err); + err_d.push_back(e.err_d); + I_eff.push_back(e.I_eff); + tau.push_back(e.tau); + tau_fb.push_back(e.tau_fb); + tau_coriolis.push_back(e.tau_coriolis); + tau_gravity.push_back(e.tau_gravity); + tau_damping.push_back(e.tau_damping); + tau_friction.push_back(e.tau_friction); + tau_barrier.push_back(e.tau_barrier); + tau_sat.push_back(e.tau_sat); + KE.push_back(e.KE); + PE.push_back(e.PE); + E_total.push_back(e.E_total); + W_actuator.push_back(e.W_actuator); + P_damping.push_back(e.P_damping); + P_friction.push_back(e.P_friction); + clamp_theta.push_back(e.clamp_theta); + clamp_omega.push_back(e.clamp_omega); + sat_flag.push_back(e.sat_flag); + joint_index.push_back(e.joint_index); + } }; // Struct for logging reference trajectory data diff --git a/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h index 004c966..0c2affc 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h @@ -63,7 +63,8 @@ namespace robots { const RobotJoint& joint, double I_eff, double q, double qd, double eta, double q_ref, double qd_ref, double qdd_ref, - double tau_coriolis, double tau_g + double tau_coriolis, double tau_g, + double dt ) const; // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration diff --git a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h index 5584520..eaec413 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h @@ -149,32 +149,6 @@ namespace robots { spawnFn _loadMeshReturn; - // Forward kinematics computation - std::vector computeForwardKinematics_fromState(const VecX& q) const; - - // Compute the full mass matrix M(q) based on the current state and robot configuration - mathlib::MatX computeMassMatrix(const std::vector& q, const std::vector& T_world) const; - // Compute the gravity torque for a joint based on the current state and robot configuration - std::vector computeGravityTorque(const std::vector& q, const std::vector& T_world) const; - - // Computes the control torque for a joint based on the current state, reference, and robot configuration - VecX computeAppliedTorques( - const std::vector& q, - const std::vector& qd, - const std::vector& eta, - const std::vector& T_world, - std::vector I_eff, - std::vector tau_gravity - ) const; - - // Compute control and dynamics metrics for a specific joint based on the current state and reference - RobotMetrics computeJointMetrics( - const RobotJoint& joint, const RobotLink& link, double I_eff, - double q, double qd, double eta, - double q_ref, double qd_ref, double qdd_ref, - double tau_coriolis, double tau_g - ) const; - // 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 @@ -190,9 +164,6 @@ namespace robots { mathlib::VecX packRefState() const; void unpackRefState(const mathlib::VecX& xr); - // Compute state derivatives - mathlib::VecX deriv(double t, const mathlib::VecX& x) const; - // Enforce joint limits after integration void enforceJointLimits(RobotJoint& j); @@ -205,6 +176,7 @@ namespace robots { // Buffers for logging and reference state (not owned by RobotSystem) robots::JointLogBuffer* _logBuffer = nullptr; + robots::TrajRefBuffer* _refBuffer = nullptr; // Flags and precomputed data diff --git a/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp index e633c19..dc04a9c 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp @@ -227,7 +227,8 @@ namespace robots { j, I_eff[i], q[i], qd[i], eta[i], j.q_ref, j.qd_ref, j.qdd_ref, - 0.0, tau_g[i] + 0.0, tau_g[i], + dt() ); tau[i] = m.tau; } @@ -241,7 +242,8 @@ namespace robots { const RobotJoint& joint, double I_eff, double q, double qd, double eta, double q_ref, double qd_ref, double qdd_ref, - double tau_c, double tau_g + double tau_c, double tau_g, + double dt ) const { RobotMetrics m{}; if (_robot.torqueMode == eTorqueMode::NONE) { @@ -259,7 +261,7 @@ namespace robots { m.tau_friction = 0.0; m.tau_sat = 0.0; m.tau_barrier = 0.0; - + // Return early return m; } @@ -276,7 +278,7 @@ namespace robots { // Energy metrics m.KE = 0.5 * I_eff * qd * qd; // [J], kinetic energy of the joint double P_grav = tau_g * qd; // [W], power due to gravity torque - m.PE += -P_grav * dt(); // [J], potential energy proxy based on gravity power (scaled down for interpretability) + m.PE += -P_grav * dt; // [J], potential energy proxy based on gravity power (scaled down for interpretability) m.E_total = m.KE + m.PE; // [J], total mechanical energy of the joint // Control parameters diff --git a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp index ef0ba10..aa520f5 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp @@ -379,10 +379,9 @@ namespace robots { } // Sum contributions to effective inertia from all links for joint i - for (size_t k = i; k < _robot.links.size(); ++k) { + for (size_t k = i + 1; k < _robot.links.size(); ++k) { I_eff[i] += _dynamics->computeJointInertiaContribution( - _robot.joints[i], - _robot.links[k], + _robot.joints[i], _robot.links[k], jointWorldPose[i], // pose of joint i in world frame T_world[k] ); @@ -400,47 +399,29 @@ namespace robots { j, I_eff[i], j.q, j.qd, j.eta, j.q_ref, j.qd_ref, j.qdd_ref, - 0.0,tau_g[i] + 0.0,tau_g[i], dt ); // Log metrics to buffer if logging is enabled auto* buf = _logBuffer; - // If logging is enabled, store metrics in the buffer for this joint + // inside joint loop if (buf) { - // Sim Metadata - buf->sim_time.push_back(simTime); - buf->dt_taken.push_back(step.dt_taken); - buf->dt_sug.push_back(step.dt_sug); - // States - buf->theta.push_back(m.theta); - buf->omega.push_back(m.omega); - buf->alpha.push_back(m.alpha); - buf->err.push_back(m.err); - buf->err_d.push_back(m.err_d); - // Dynamics - buf->I_eff.push_back(m.I_eff); - buf->tau.push_back(m.tau); - buf->tau_fb.push_back(m.tau_fb); - buf->tau_coriolis.push_back(m.tau_coriolis); - buf->tau_gravity.push_back(m.tau_gravity); - buf->tau_damping.push_back(m.tau_damping); - buf->tau_friction.push_back(m.tau_friction); - buf->tau_barrier.push_back(m.tau_barrier); - buf->tau_sat.push_back(m.tau_sat); - // Energy, Work, & Power - buf->KE.push_back(m.KE); - buf->PE.push_back(m.PE); - buf->E_total.push_back(m.E_total); - buf->W_actuator.push_back(m.W_actuator); - buf->P_damping.push_back(m.P_damping); - buf->P_friction.push_back(m.P_friction); - // Limit flags and info - buf->clamp_theta.push_back((double)_clampTheta[i]); - buf->clamp_omega.push_back((double)_clampOmega[i]); - buf->sat_flag.push_back(m.sat_flag); - // Joint Index - buf->joint_index.push_back((int)i); + JointLogBuffer::JointLogEntry e{}; + e.sim_time = simTime; + e.dt_taken = step.dt_taken; + e.dt_sug = step.dt_sug; + e.theta = m.theta; e.omega = m.omega; e.alpha = m.alpha; + e.err = m.err; e.err_d = m.err_d; + e.I_eff = m.I_eff; e.tau = m.tau; e.tau_fb = m.tau_fb; + e.tau_coriolis = m.tau_coriolis; e.tau_gravity = m.tau_gravity; + e.tau_damping = m.tau_damping; e.tau_friction = m.tau_friction; + e.tau_barrier = m.tau_barrier; e.tau_sat = m.tau_sat; + e.KE = m.KE; e.PE = m.PE; e.E_total = m.E_total; + e.W_actuator = m.W_actuator; e.P_damping = m.P_damping; e.P_friction = m.P_friction; + e.clamp_theta = (double)_clampTheta[i]; e.clamp_omega = (double)_clampOmega[i]; + e.sat_flag = m.sat_flag; e.joint_index = (int)i; + buf->push_entry(e); } } From 2b0b7cc21ce3ef6b382452e459741e1a5be11e66 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 21 Feb 2026 16:23:41 +0000 Subject: [PATCH 09/20] WIP: adding double-buffered joint metric logging to RobotSystem NOTES: - The idea is to introduce a thread-safe, double-buffered internal logging system for joint metrics, I am so fed up of the current logger. - Added JointLogEntry struct, push_entry, and validate methods to MetricLogger. - Implemented claimExportLogBuffer, useInternalLogBuffer, and reserveInternalLogBuffers to manage buffer swapping and allocation. (assisted by forums and AI to correct minor mistakes) - Updated simulation loop to use internal buffers by default, with support for external buffers if needed. --- .../EngineLib/include/Analysis/MetricLogger.h | 44 +++++++++++++++++++ .../EngineLib/include/Robots/RobotSystem.h | 27 +++++++++--- .../EngineLib/src/Robots/RobotSystem.cpp | 40 +++++++++++++++-- 3 files changed, 102 insertions(+), 9 deletions(-) diff --git a/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h b/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h index 4942bc3..a25fc23 100644 --- a/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h +++ b/Sim_Engine/EngineLib/include/Analysis/MetricLogger.h @@ -107,6 +107,7 @@ namespace robots { return theta.size(); } + // Struct representing a single log entry for a joint at a specific time step struct ENGINE_API JointLogEntry { double sim_time; double dt_taken; @@ -118,6 +119,7 @@ namespace robots { int joint_index; }; + // Push a new log entry into the buffer void push_entry(const JointLogEntry& e) { sim_time.push_back(e.sim_time); dt_sug.push_back(e.dt_sug); @@ -146,6 +148,48 @@ namespace robots { clamp_omega.push_back(e.clamp_omega); sat_flag.push_back(e.sat_flag); joint_index.push_back(e.joint_index); + } + + // Validate that all vectors have the same size + bool validate(std::string* outMsg) const { + size_t n = theta.size(); // ref size + // Lambda to check size of each vector against n + auto checkSize = [&](const auto& v, const char* name) -> bool { + if (v.size() != n) { + if (outMsg) { *outMsg = "Size mismatch for " + std::string(name) + ": expected " + std::to_string(n) + ", got " + std::to_string(v.size()); } + return false; + } + return true; + }; + // Check that all vectors have the same size + if (!checkSize(sim_time, "sim_time")) return false; + if (!checkSize(dt_taken, "dt_taken")) return false; + if (!checkSize(dt_sug, "dt_sug")) return false; + if (!checkSize(theta, "theta")) return false; + if (!checkSize(omega, "omega")) return false; + if (!checkSize(alpha, "alpha")) return false; + if (!checkSize(err, "err")) return false; + if (!checkSize(err_d, "err_d")) return false; + if (!checkSize(I_eff, "I_eff")) return false; + if (!checkSize(tau, "tau")) return false; + if (!checkSize(tau_fb, "tau_fb")) return false; + if (!checkSize(tau_coriolis, "tau_coriolis")) return false; + if (!checkSize(tau_gravity, "tau_gravity")) return false; + if (!checkSize(tau_damping, "tau_damping")) return false; + if (!checkSize(tau_friction, "tau_friction")) return false; + if (!checkSize(tau_barrier, "tau_barrier")) return false; + if (!checkSize(tau_sat, "tau_sat")) return false; + if (!checkSize(KE, "KE")) return false; + if (!checkSize(PE, "PE")) return false; + if (!checkSize(E_total, "E_total")) return false; + if (!checkSize(W_actuator, "W_actuator")) return false; + if (!checkSize(P_damping, "P_damping")) return false; + if (!checkSize(P_friction, "P_friction")) return false; + if (!checkSize(clamp_theta, "clamp_theta")) return false; + if (!checkSize(clamp_omega, "clamp_omega")) return false; + if (!checkSize(sat_flag, "sat_flag")) return false; + if (!checkSize(joint_index, "joint_index")) return false; + return true; // all sizes match } }; diff --git a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h index eaec413..8f48d1b 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotSystem.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotSystem.h @@ -133,6 +133,15 @@ namespace robots { void setTorqueMode(eTorqueMode mode); eTorqueMode getTorqueMode() const { return _robot.torqueMode; } + // Swap for the current log buffer, returning a ptr to new active buffer + robots::JointLogBuffer* claimExportLogBuffer(); + + // Method to enable or disable the use of internal log buffers + void useInternalLogBuffer(bool enable); + + // Reserve space in the internal log buffers for a certain number of samples (expected) + void reserveInternalLogBuffers(size_t expected); + private: void instantiateRobotLinks(); void buildLinkIndex(); @@ -174,11 +183,6 @@ namespace robots { RobotModel _robot; eTorqueMode _torqueMode = _robot.torqueMode; - // Buffers for logging and reference state (not owned by RobotSystem) - robots::JointLogBuffer* _logBuffer = nullptr; - - robots::TrajRefBuffer* _refBuffer = nullptr; - // Flags and precomputed data bool _hasRobot = false; glm::mat4 _robotRootPose = glm::mat4(1.0f); // current pose (meters) @@ -221,10 +225,21 @@ namespace robots { double _baseYawAcc = 0.0; // Tunables - double _baseMass = 62.0; // kg (H1 ≈ 60–65) + 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 + std::array _logBuffers{}; + std::atomic _activeLogBufIdx{ 0 }; // index of the currently active log buffer for writing (0 or 1) + std::mutex _logSwapMutex; // mutex to protect swapping log buffers between simulation and logging thread + bool _useInternalLogging = true; // flag to determine whether to use internal log buffers or external one provided by setLogBuffer + + // Pointers to external log and reference buffers (not owned by RobotSystem) + robots::JointLogBuffer* _logBuffer = nullptr; + robots::TrajRefBuffer* _refBuffer = nullptr; + }; } // namespace robot diff --git a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp index aa520f5..5a7523b 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp @@ -403,9 +403,17 @@ namespace robots { ); // Log metrics to buffer if logging is enabled - auto* buf = _logBuffer; + robots::JointLogBuffer* buf = nullptr; - // inside joint loop + // If using internal logging, get the active buffer + if (_useInternalLogging) { + int idx = _activeLogBufIdx.load(std::memory_order_acquire); + buf = &_logBuffers[idx]; + } + // If using external logging, use the user-provided buffer + else { buf = _logBuffer; /*external buffer provided by user*/ } + + // If we have a buffer, push the new entry if (buf) { JointLogBuffer::JointLogEntry e{}; e.sim_time = simTime; @@ -1044,7 +1052,6 @@ namespace robots { (float)_basePos.z() ) ); - glm::mat4 R = glm::rotate( glm::mat4(1.0f), (float)_baseYaw, @@ -1064,4 +1071,31 @@ namespace robots { // Set the torque mode for the robot system void RobotSystem::setTorqueMode(eTorqueMode mode) { _robot.torqueMode = mode; } + + // Method to claim the current active log buffer for exporting logged data (returns pointer to buffer active before swap) + robots::JointLogBuffer* RobotSystem::claimExportLogBuffer() { + // swap active buffer index + std::lock_guard lk(_logSwapMutex); // ensure thread safety during swap + int prev = _activeLogBufIdx.load(std::memory_order_acquire); // get current active buffer index + int next = 1 - prev; // compute next buffer index (toggle between 0 and 1) + _activeLogBufIdx.store(next, std::memory_order_release); // set next buffer as active for logging + return &_logBuffers[prev]; // returns ptr to buffer active before swap + } + + // Method to enable or disable the use of internal log buffers for recording joint metrics during simulation + void RobotSystem::useInternalLogBuffer(bool enable) { + _useInternalLogging = enable; + if (enable) { + _logBuffers[0].clear(); // clear both buffers to start fresh + _logBuffers[1].clear(); // clear both buffers to start fresh + _activeLogBufIdx.store(0); // reset active buffer index to 0 + } + } + + // Method to reserve capacity in the internal log buffers to optimize performance by avoiding reallocations during logging + void RobotSystem::reserveInternalLogBuffers(size_t expected) { + _logBuffers[0].reserve(expected); // reserve both buffers to avoid reallocations during logging + _logBuffers[1].reserve(expected); // reserve both buffers to avoid reallocations during logging + } + } // namespace robots \ No newline at end of file From 3230a4c4af6583a3c2d0f9c929a29bc2c63a84fc Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 21 Feb 2026 17:47:59 +0000 Subject: [PATCH 10/20] WIP: added simulation run modes and refactor log buffer handling NOTES: - IntroduceD Interactive/Synchronous run modes to SimulationCore WITH dynamical sizing log and reference buffers based on mode to prevent OOM (new acronym learnt) errors. - Moved joint log buffer management into the robot system, with export and clearing handled via new interfaces. - Refactored exportLogsToHDF5 for validation, timing, and robust logging. - Added buffer size validation to exportRefsToHDF5. TODO: - Now implementing batch parallelisation * I WANT TO VERY CLEAR -> this does not affect my current dissertation, but I really cannot be bothered waiting for all my tests to finish because my thread util is 100%. --- .../include/Platform/SimulationState.h | 13 +- .../EngineLib/include/Scene/SimulationCore.h | 9 + .../EngineLib/src/Scene/SimulationCore.cpp | 181 +++++++++++------- 3 files changed, 128 insertions(+), 75 deletions(-) diff --git a/Sim_Engine/EngineLib/include/Platform/SimulationState.h b/Sim_Engine/EngineLib/include/Platform/SimulationState.h index 33cf4b0..fd003a4 100644 --- a/Sim_Engine/EngineLib/include/Platform/SimulationState.h +++ b/Sim_Engine/EngineLib/include/Platform/SimulationState.h @@ -21,10 +21,21 @@ enum class SelectionSource { }; // Current selection state -struct Selection { +struct ENGINE_API Selection { SelectionType type = SelectionType::NONE; SelectionSource source = SelectionSource::NONE; int index = -1; // Index of the selected link or robot }; +// Current simulation run mode (e.g., interactive with real-time rendering vs. synchronous for +enum class eRunMode { + Interactive, + Synchronous +}; + +// Struct to hold the current simulation mode and related settings +struct ENGINE_API modes { + eRunMode _runMode = eRunMode::Interactive; +}; + diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h index 9863a79..2bf6438 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h @@ -4,6 +4,7 @@ #include "EngineCore.h" #include #include +#include "Platform/SimulationState.h" #include "Analysis/Telemetry.h" #include "Analysis/MetricLogger.h" @@ -18,6 +19,11 @@ namespace robots { class ENGINE_API RobotSystem; } namespace interpreter { class ENGINE_API 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 + class ENGINE_API SimulationCore { public: SimulationCore(); @@ -117,6 +123,9 @@ namespace core { control::TrajectoryManager* _traj = nullptr; std::vector>* _objects = nullptr; + // Run mode (interactive vs synchronous) + eRunMode _runMode = eRunMode::Interactive; + // Last script text for comparison re-use std::string _lastScriptText; diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index ac89f55..0191565 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -37,31 +37,26 @@ namespace core { // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { - _accum += frame_dt; + _accum += frame_dt; // accumulate frame time to step the simulation in fixed increments of _dt + // Step the simulation forward in fixed increments of _dt until we catch up to the current frame time while (_accum >= _dt) { + // Step the active script program if running and check for completion or faults if (_scriptRunning && _activeProgram) { _activeProgram->step(_dt); - const bool completed = _activeProgram->isCompleted(); const bool stopped = _activeProgram->isStopped(); const bool faulted = _activeProgram->isFaulted(); - if (completed) { D_SUCCESS("SCRIPT END: completed=%d (dt=%.6f s, simTime=%.3f s)", (int)completed, _dt, _simTime); - _scriptRunning = false; _activeProgram = nullptr; - stopSimulation(); D_RUNTIME("Program execution completed."); } else if (stopped || faulted) { - D_FAIL("SCRIPT END: stopped=%d faulted=%d (dt=%.6f s, simTime=%.3f s)", - (int)stopped, (int)faulted, _dt, _simTime); - + D_FAIL("SCRIPT END: stopped=%d faulted=%d (dt=%.6f s, simTime=%.3f s)", (int)stopped, (int)faulted, _dt, _simTime); _scriptRunning = false; _activeProgram = nullptr; - stopSimulation(); D_RUNTIME("Program execution completed."); } @@ -70,17 +65,14 @@ namespace core { D_FAIL("SCRIPT END: _scriptRunning=1 but _activeProgram=nullptr"); _scriptRunning = false; } - + // Update physics and robot system if sim is running if (_simRunning) { _simTime += _dt; - updatePhysics(_dt); + // Update robot trajectory inputs and step the robot forward in time if (hasRobot()) { - // Update Trajector Inputs _robot->updateTrajectoryInputs(*_traj, _simTime); - // Step robot system _robot->step(_dt, _simTime); - // Telemetry update if (!_telemetryBegun) { _telemetry.beginRun(_simTime, _telHz, 300.0); @@ -90,7 +82,7 @@ namespace core { _telemetry.update(_simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); } } - _accum -= _dt; + _accum -= _dt; // decrease accumulator by fixed timestep until we catch up to the current frame time } } @@ -106,23 +98,28 @@ namespace core { // Reset simulation system if (_robot) { _robot->resetRobot(); - - // Clear and reserve telemetry buffers based on expected simulation length and robot DOF _trajRefBuffer.clear(); - _jointLogBuffer.clear(); - // Calculate the total number of entries needed for the buffers - size_t steps = static_cast(300.0 / _dt); // Assuming a max simulation length of 300 seconds + // Determine expected number of entries based on run mode and cap it to prevent OOM + double expectedMinutes = (_runMode == eRunMode::Synchronous) ? DEFAULT_SYNC_MINUTES : DEFAULT_INTERACTIVE_MINUTES; + + // Convert minutes to steps size_t joints = _robot->jointCount(); - size_t total = steps * joints; + double expectedSeconds = expectedMinutes * 60.0; + size_t steps = static_cast(expectedSeconds / _dt); - // Reserve capacity to avoid reallocations during the run - _trajRefBuffer.reserve(total); - _jointLogBuffer.reserve(total); + // Guarding against overflow and capping + uint64_t total64 = static_cast(steps) * static_cast(joints); + size_t total = static_cast(std::min(total64, MAX_LOG_ENTRIES)); - // IMPORTANT: inject buffer into robot + // Internal double buf for high-rate joint log + _robot->useInternalLogBuffer(true); + _robot->reserveInternalLogBuffers(total); + + // Keeps external traj ref buffer for lower-rate traj ref (going to refactor this later) + _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); - _robot->setLogBuffer(&_jointLogBuffer); } // Ensure reference sim system have their integrators configured for the new run @@ -139,17 +136,14 @@ namespace core { if (!_simRunning) return; D_RUNTIME("stopping simulation"); - D_RUNTIME("JointLogBuffer size = %zu", _jointLogBuffer.size()); - D_RUNTIME("TrajRefBuffer size = %zu", _trajRefBuffer.size()); - - // Only export ref if we actually have samples + // Export references if (_trajRefBuffer.size() > 0) { exportRefsToHDF5(); + _trajRefBuffer.clear(); } - // Only export sim if we actually have samples - if (_jointLogBuffer.size() > 0) { - exportLogsToHDF5(); - } + + // Claims export buffer and writes it for joint logs + exportLogsToHDF5(); // Clear buffers to free memory and prepare for next run DATA_CAPTURE_ENABLE(false); @@ -159,56 +153,84 @@ namespace core { // Exporst the logged joint data to HDF5 format using the custom macro for each log entry void SimulationCore::exportLogsToHDF5() { + auto t0 = std::chrono::steady_clock::now(); // start timer for export duration measurement // Construct a header for the HDF5 dataset based on the robot and integrator names const std::string intName = _robot->getIntegratorName(); const std::string robotName = _robot->hasRobot() ? _robot->robotName() : "no_robot"; const std::string header = robotName + "_sim_" + intName; - // Check if there are any log entries - const size_t N = _jointLogBuffer.size(); - if (N == 0) return; + // Claim the export log buffer from the robot (swap is internal!) + robots::JointLogBuffer* exportBuf = _robot->claimExportLogBuffer(); + if (!exportBuf) { return; } + + // Validation check to ensure we have data to export + std::string vmsg; + if (!exportBuf->validate(&vmsg)) { + D_FAIL("ExportLogs -> validation failed for export buffer: %s", vmsg.c_str()); + // Not returning, data is exported even if validation fails + } + + // Check if there are any log entries to export + const size_t N = exportBuf->size(); + if (N == 0) { + D_RUNTIME("ExportLogs -> no data to export (buffer size is 0)"); // *REMNINDER* -> SHOULD I make a macro for ExportLogs? + exportBuf->clear(); + return; + } // For each log entry, create a field list and write to HDF5 for (size_t i = 0; i < N; ++i) { - // Create a list of fields for this log entry data::FieldList fields; // Sim Metadata - fields.emplace_back("sim_time", (double)_jointLogBuffer.sim_time[i]); - fields.emplace_back("dt_taken", (double)_jointLogBuffer.dt_taken[i]); - fields.emplace_back("dt_sug", (double)_jointLogBuffer.dt_sug[i]); + fields.emplace_back("sim_time", (double)exportBuf->sim_time[i]); + fields.emplace_back("dt_taken", (double)exportBuf->dt_taken[i]); + fields.emplace_back("dt_sug", (double)exportBuf->dt_sug[i]); // States - fields.emplace_back("theta", (double)_jointLogBuffer.theta[i]); - fields.emplace_back("omega", (double)_jointLogBuffer.omega[i]); - fields.emplace_back("alpha", (double)_jointLogBuffer.alpha[i]); - fields.emplace_back("err", (double)_jointLogBuffer.err[i]); - fields.emplace_back("err_d", (double)_jointLogBuffer.err_d[i]); + fields.emplace_back("theta", (double)exportBuf->theta[i]); + fields.emplace_back("omega", (double)exportBuf->omega[i]); + fields.emplace_back("alpha", (double)exportBuf->alpha[i]); + fields.emplace_back("err", (double)exportBuf->err[i]); + fields.emplace_back("err_d", (double)exportBuf->err_d[i]); // Dynamics - fields.emplace_back("I_eff", (double)_jointLogBuffer.I_eff[i]); - fields.emplace_back("tau", (double)_jointLogBuffer.tau[i]); - fields.emplace_back("tau_fb", (double)_jointLogBuffer.tau_fb[i]); - fields.emplace_back("tau_coriolis", (double)_jointLogBuffer.tau_coriolis[i]); - fields.emplace_back("tau_gravity", (double)_jointLogBuffer.tau_gravity[i]); - fields.emplace_back("tau_damping", (double)_jointLogBuffer.tau_damping[i]); - fields.emplace_back("tau_friction", (double)_jointLogBuffer.tau_friction[i]); - fields.emplace_back("tau_barrier", (double)_jointLogBuffer.tau_barrier[i]); - fields.emplace_back("tau_sat", (double)_jointLogBuffer.tau_sat[i]); + fields.emplace_back("I_eff", (double)exportBuf->I_eff[i]); + fields.emplace_back("tau", (double)exportBuf->tau[i]); + fields.emplace_back("tau_fb", (double)exportBuf->tau_fb[i]); + fields.emplace_back("tau_coriolis",(double)exportBuf->tau_coriolis[i]); + fields.emplace_back("tau_gravity", (double)exportBuf->tau_gravity[i]); + fields.emplace_back("tau_damping", (double)exportBuf->tau_damping[i]); + fields.emplace_back("tau_friction",(double)exportBuf->tau_friction[i]); + fields.emplace_back("tau_barrier", (double)exportBuf->tau_barrier[i]); + fields.emplace_back("tau_sat", (double)exportBuf->tau_sat[i]); // Energy, Work, & Power - fields.emplace_back("KE", (double)_jointLogBuffer.KE[i]); - fields.emplace_back("PE", (double)_jointLogBuffer.PE[i]); - fields.emplace_back("E_total", (double)_jointLogBuffer.E_total[i]); - fields.emplace_back("W_actuator", (double)_jointLogBuffer.W_actuator[i]); - fields.emplace_back("P_damping", (double)_jointLogBuffer.P_damping[i]); - fields.emplace_back("P_friction", (double)_jointLogBuffer.P_friction[i]); + fields.emplace_back("KE", (double)exportBuf->KE[i]); + fields.emplace_back("PE", (double)exportBuf->PE[i]); + fields.emplace_back("E_total", (double)exportBuf->E_total[i]); + fields.emplace_back("W_actuator", (double)exportBuf->W_actuator[i]); + fields.emplace_back("P_damping", (double)exportBuf->P_damping[i]); + fields.emplace_back("P_friction", (double)exportBuf->P_friction[i]); // Limit flags and info - fields.emplace_back("clamp_theta", (double)_jointLogBuffer.clamp_theta[i]); - fields.emplace_back("clamp_omega", (double)_jointLogBuffer.clamp_omega[i]); - fields.emplace_back("sat_flag", (double)_jointLogBuffer.sat_flag[i]); + fields.emplace_back("clamp_theta", (double)exportBuf->clamp_theta[i]); + fields.emplace_back("clamp_omega", (double)exportBuf->clamp_omega[i]); + fields.emplace_back("sat_flag", (double)exportBuf->sat_flag[i]); // Joint info - fields.emplace_back("joint_index", (double)_jointLogBuffer.joint_index[i]); + fields.emplace_back("joint_index", (double)exportBuf->joint_index[i]); - // Write this entry to HDF5 + // Write entry to HDF5 HDF5_SIM_DATA(header, fields); } + // Log export duration + auto dur = std::chrono::steady_clock::now() - t0; + LOG_INFO("ExportLogs -> wrote %zu samples in %.3f s", N, std::chrono::duration(dur).count()); + D_RUNTIME("ExportLogs -> wrote %zu samples in %.3f s", N, std::chrono::duration(dur).count()); + + // Clear exported buffer + exportBuf->clear(); + D_RUNTIME("ExportLogs -> export buffer cleared"); + LOG_INFO("ExportLogs -> export buffer cleared"); + + // Log success + D_SUCCESS("ExportLogs -> export completed successfully"); + LOG_INFO("ExportLogs -> export completed successfully"); } // Exports the reference trajectory data to HDF5 format using the custom macro for each ref entry @@ -220,6 +242,16 @@ namespace core { const size_t N = _trajRefBuffer.size(); if (N == 0) return; + // Simple validation of buffer sizes + if (_trajRefBuffer.theta_ref.size() != N || + _trajRefBuffer.omega_ref.size() != N || + _trajRefBuffer.alpha_ref.size() != N || + _trajRefBuffer.sim_time.size() != N || + _trajRefBuffer.joint_index.size() != N) { + D_FAIL("exportRefsToHDF5: TrajRefBuffer size mismatch"); + return; // Return as reference data is useless if sizes don't match + } + // For each ref entry, create a field list and write to HDF5 for (size_t i = 0; i < N; ++i) { // Create a list of fields for this log entry @@ -259,14 +291,12 @@ namespace core { _robot->resetRobot(); _traj->clearAll(); - // Clear telemetry and log buffers + // Clear reference buffer (external for now) _trajRefBuffer.clear(); - _jointLogBuffer.clear(); - - // Inject buffers BEFORE any stepping happens + // Inject reference buffer only _robot->setRefBuffer(&_trajRefBuffer); - _robot->setLogBuffer(&_jointLogBuffer); + // Reset simulation state _simTime = 0.0; _simRunning = false; _telemetryBegun = false; @@ -279,6 +309,9 @@ namespace core { _activeProgram = program; _scriptRunning = true; + // Set run mode to synchronous for the duration of this run + _runMode = eRunMode::Synchronous; + // enable sim stepping and telemetry for synchronous run startSimulation(); @@ -323,15 +356,15 @@ namespace core { } // Clean up stopSimulation(); + // Reset run mode to interactive (default) + _runMode = eRunMode::Interactive; _activeProgram = nullptr; _scriptRunning = false; _simRunning = false; _telemetryBegun = false; - D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", - methodName.c_str(), _simTime, _telemetry.ring.size()); - + D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", methodName.c_str(), _simTime, _telemetry.ring.size()); return (_telemetry.ring.size() >= 2); } From c303050ec9c58fd02ba2cbbaaabfd6c286b95fdc Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 21 Feb 2026 23:13:56 +0000 Subject: [PATCH 11/20] feat: added Headless core API, batch runner, and asset refactor NOTES: - Major refactor to decouple simulation core from GUI by introducing ISimulationCore, a headless API for scripting and batch operation. * This has taken ~7hours, learnt a lot though!! - Added StudyRunner for parallel/batch simulation runs and BatchMain for CLI batch mode. - Interpreter contexts now use ISimulationCore*, enabling headless scripting! - Moveded MeshLoader and VertexHolder to Assets namespace -> updated all mesh/vertex code accordingly. - Addeded integration method enum header. TODO: - Go through and MAKE SURE it is working (it loads, but cannot verify right now, commit is to save progress) --- Sim_Engine/Engine.vcxproj | 1 + Sim_Engine/Engine.vcxproj.filters | 3 + Sim_Engine/Engine/src/BatchMain.cpp | 75 ++++++++++ Sim_Engine/Engine/src/Main.cpp | 8 +- Sim_Engine/EngineLib/EngineLib.vcxproj | 11 +- .../EngineLib/EngineLib.vcxproj.filters | 33 +++-- .../include/{Scene => Assets}/MeshLoader.h | 4 +- .../include/{Scene => Assets}/VertexHolder.h | 2 +- Sim_Engine/EngineLib/include/EngineCore.h | 14 +- .../Interpreter/CommandContextMotion.h | 9 +- .../Interpreter/Commands/TrajClearCmd.h | 2 +- .../include/Interpreter/MainContext.h | 4 +- .../EngineLib/include/Interpreter/SimFwd.h | 2 +- .../include/Interpreter/StoredProgram.h | 4 +- .../EngineLib/include/Interpreter/UIContext.h | 6 +- .../include/Numerics/IntegrationMethods.h | 12 ++ .../include/Numerics/IntegrationService.h | 12 +- .../include/Platform/ISimulationCore.h | 56 +++++++ .../EngineLib/include/Platform/StudyRunner.h | 53 +++++++ .../include/Rendering/OpenGLBufferManager.h | 2 +- .../EngineLib/include/Rendering/RenderBase.h | 4 +- Sim_Engine/EngineLib/include/Scene/Mesh.h | 8 +- Sim_Engine/EngineLib/include/Scene/ObjectID.h | 10 +- .../EngineLib/include/Scene/SimulationCore.h | 112 +++++++------- .../include/Scene/SimulationManager.h | 14 +- .../src/{Scene => Assets}/MeshLoader.cpp | 8 +- Sim_Engine/EngineLib/src/Assets/importObj.cpp | 2 +- Sim_Engine/EngineLib/src/EngineCore.cpp | 17 +++ .../src/Interpreter/CommandContextMotion.cpp | 14 +- .../src/Interpreter/Commands/TrajClearCmd.cpp | 9 +- .../src/Interpreter/Commands/TrajSetCmd.cpp | 14 +- .../src/Interpreter/StoredProgram.cpp | 68 +++++---- .../EngineLib/src/Interpreter/UIContext.cpp | 42 +++--- .../EngineLib/src/Platform/StudyRunner.cpp | 138 ++++++++++++++++++ .../src/Rendering/OpenGLBufferManager.cpp | 10 +- .../EngineLib/src/Robots/RobotLoader.cpp | 2 +- Sim_Engine/EngineLib/src/Scene/Mesh.cpp | 8 +- .../EngineLib/src/Scene/SimulationCore.cpp | 90 +++++++++++- .../EngineLib/src/Scene/SimulationManager.cpp | 31 ++-- .../EngineLib/src/ui/CommandScriptEditor.cpp | 15 +- .../External/MathLib/include/core/Types.h | 2 +- 41 files changed, 699 insertions(+), 232 deletions(-) create mode 100644 Sim_Engine/Engine/src/BatchMain.cpp rename Sim_Engine/EngineLib/include/{Scene => Assets}/MeshLoader.h (96%) rename Sim_Engine/EngineLib/include/{Scene => Assets}/VertexHolder.h (97%) create mode 100644 Sim_Engine/EngineLib/include/Numerics/IntegrationMethods.h create mode 100644 Sim_Engine/EngineLib/include/Platform/ISimulationCore.h create mode 100644 Sim_Engine/EngineLib/include/Platform/StudyRunner.h rename Sim_Engine/EngineLib/src/{Scene => Assets}/MeshLoader.cpp (97%) create mode 100644 Sim_Engine/EngineLib/src/EngineCore.cpp create mode 100644 Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp diff --git a/Sim_Engine/Engine.vcxproj b/Sim_Engine/Engine.vcxproj index bdc1778..0eb4c4a 100644 --- a/Sim_Engine/Engine.vcxproj +++ b/Sim_Engine/Engine.vcxproj @@ -111,6 +111,7 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" + diff --git a/Sim_Engine/Engine.vcxproj.filters b/Sim_Engine/Engine.vcxproj.filters index a4e06fa..a25c818 100644 --- a/Sim_Engine/Engine.vcxproj.filters +++ b/Sim_Engine/Engine.vcxproj.filters @@ -7,6 +7,9 @@ src + + src + diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp new file mode 100644 index 0000000..abf1249 --- /dev/null +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -0,0 +1,75 @@ +// BatchMain.cpp +#include "pch.h" +#include +#include "Platform/StudyRunner.h" +#include "Numerics/IntegrationMethods.h" + +extern "C" core::ISimulationCore* CreateSimulationCore_v1(); +extern "C" void DestroySimulationCore(core::ISimulationCore*); + +// Helper: create CorePtr (unique_ptr with std::function deleter) +static CorePtr makeCoreFactory() { + core::ISimulationCore* raw = CreateSimulationCore_v1(); + if (!raw) { + return CorePtr(nullptr, [](core::ISimulationCore*) {}); + } + // std::function deleter is constructed from the lambda implicitly + return CorePtr(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); +} + +int runBatchMode() { + try { + // Define the configurations for the studies to run (combinations of integrator methods, timesteps, and run lengths) + std::vector configs; + std::vector methods = { + integration::eIntegrationMethod::Euler, + integration::eIntegrationMethod::Midpoint, + integration::eIntegrationMethod::Heun, + integration::eIntegrationMethod::Ralston, + integration::eIntegrationMethod::RK4, + integration::eIntegrationMethod::RK45 + }; + std::vector dts = { 0.001, 0.002, 0.005 }; + std::vector lengths_mins = { 0.5, 1.0, 5.0 }; + + for (auto m : methods) { + for (double dt : dts) { + for (double len : lengths_mins) { + StudyRunner::config c; + c.method = m; + c.dt = dt; + c.len_min = len; + c.tag = std::string("m") + std::to_string((int)m) + "_dt" + std::to_string(dt) + "_L" + std::to_string((int)len); + configs.push_back(c); + } + } + } + + // Build StudyRunner with factory + StudyRunner runner([]() { return makeCoreFactory(); }, /*maxConcurrency=*/ 0); + + // The script text for the run(s) + std::string scriptText = R"( + // your DSL script here, example: + set(integrator, rk4) + // ... more script ... + )"; + + // Run studies (note: runStudies signature expects std::string& in your header) + auto results = runner.runStudies(configs, scriptText); + + // Print summary + for (const auto& r : results) { + std::cout << "tag=" << r.tag + << " success=" << (r.success ? "yes" : "no") + << " int=" << r.intName + << " time=" << r.simTime + << " samples=" << r.samples << "\n"; + } + } + catch (const std::exception& e) { + std::cerr << "Batch mode exception: " << e.what() << std::endl; + return EXIT_FAILURE; + } + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/Sim_Engine/Engine/src/Main.cpp b/Sim_Engine/Engine/src/Main.cpp index 86cd4ca..2738276 100644 --- a/Sim_Engine/Engine/src/Main.cpp +++ b/Sim_Engine/Engine/src/Main.cpp @@ -1,8 +1,14 @@ #include "pch.h" +// File: Main.cpp +// GitHub: SaltyJoss #include #include -int main() { +int main(int argc, char** argv) { + if (argc > 1 && std::string(argv[1]) == "--batch") { + return runBatchMode(); + } + Application app("DSFE"); app.run(); return 0; diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj b/Sim_Engine/EngineLib/EngineLib.vcxproj index 4af62b2..079e562 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj @@ -122,7 +122,10 @@ if "$(Configuration)"=="Release" ( + + + @@ -176,6 +179,8 @@ if "$(Configuration)"=="Release" ( + + @@ -207,15 +212,18 @@ if "$(Configuration)"=="Release" ( + + + @@ -242,13 +250,11 @@ if "$(Configuration)"=="Release" ( - - @@ -384,7 +390,6 @@ if "$(Configuration)"=="Release" ( - diff --git a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters index 5864ed6..06d112b 100644 --- a/Sim_Engine/EngineLib/EngineLib.vcxproj.filters +++ b/Sim_Engine/EngineLib/EngineLib.vcxproj.filters @@ -231,9 +231,6 @@ Header Files\include\Scene - - Header Files\include\Scene - Header Files\include\Assets @@ -300,9 +297,6 @@ Header Files\include\Physics - - Header Files\include\Scene - Header Files\include\Rendering @@ -549,6 +543,21 @@ Header Files + + Header Files\include\Platform + + + Header Files\include\Platform + + + Header Files\include\Numerics + + + Header Files\include\Assets + + + Header Files\include\Analysis + @@ -629,9 +638,6 @@ Source Files\src\Physics - - Source Files\src\Scene - Source Files\src\Scene @@ -773,5 +779,14 @@ Source Files\src\Scene + + Source Files\src\Platform + + + Source Files\src + + + Source Files\src\Assets + \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Scene/MeshLoader.h b/Sim_Engine/EngineLib/include/Assets/MeshLoader.h similarity index 96% rename from Sim_Engine/EngineLib/include/Scene/MeshLoader.h rename to Sim_Engine/EngineLib/include/Assets/MeshLoader.h index 067043e..20c2c87 100644 --- a/Sim_Engine/EngineLib/include/Scene/MeshLoader.h +++ b/Sim_Engine/EngineLib/include/Assets/MeshLoader.h @@ -7,7 +7,7 @@ #pragma warning(disable : 4251) #include "EngineCore.h" - +#include "Assets/VertexHolder.h" #include #include #include @@ -23,7 +23,7 @@ namespace ai { // Forward Declarations for Mesh.h namespace scene { class ENGINE_API Mesh; } -namespace gui { +namespace assets { class ENGINE_API MeshLoader { public: // Loads a mesh from the specified file path and returns a vector of shared pointers to Mesh objects diff --git a/Sim_Engine/EngineLib/include/Scene/VertexHolder.h b/Sim_Engine/EngineLib/include/Assets/VertexHolder.h similarity index 97% rename from Sim_Engine/EngineLib/include/Scene/VertexHolder.h rename to Sim_Engine/EngineLib/include/Assets/VertexHolder.h index aa030ff..e4ed220 100644 --- a/Sim_Engine/EngineLib/include/Scene/VertexHolder.h +++ b/Sim_Engine/EngineLib/include/Assets/VertexHolder.h @@ -9,7 +9,7 @@ extern ENGINE_API Debug gLog; -namespace scene { +namespace assets { class ENGINE_API VertexHolder { public: VertexHolder() : _pos(), _normal(), _texCoord() {} diff --git a/Sim_Engine/EngineLib/include/EngineCore.h b/Sim_Engine/EngineLib/include/EngineCore.h index c4fe393..c0cc0e5 100644 --- a/Sim_Engine/EngineLib/include/EngineCore.h +++ b/Sim_Engine/EngineLib/include/EngineCore.h @@ -1,6 +1,8 @@ #pragma once // File: EngineCore.h // GitHub: SaltyJoss + +// Conditional compilation for cross-platform symbol export/import #ifdef _WIN32 #ifdef ENGINE_LIB_BUILD #define ENGINE_API __declspec(dllexport) @@ -9,4 +11,14 @@ #endif #else #define ENGINE_API -#endif \ No newline at end of file +#endif + +// Forward declarations for core components +namespace core { struct ISimulationCore; } + +// Factory functions +extern "C" { + // Create / destroy factory declarations (no definitions in header) + ENGINE_API core::ISimulationCore* CreateSimulationCore_v1(); + ENGINE_API void DestroySimulationCore(core::ISimulationCore* p); +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h b/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h index 65f062b..33f2b73 100644 --- a/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h +++ b/Sim_Engine/EngineLib/include/Interpreter/CommandContextMotion.h @@ -31,7 +31,7 @@ namespace commands { // Class representing the command context class ENGINE_API CommandContextMotion { public: - CommandContextMotion(gui::SimManager* sim, scene::ObjectID objID); + CommandContextMotion(core::ISimulationCore* core); // --- GLOBAL STATE METHODS --- @@ -52,10 +52,7 @@ namespace commands { utils::OpResult setJointOmega(const std::string& childLink, double omegaDegPerSec); // deg/s // --- HELPER METHODS --- - - - - gui::SimManager* Sim() const { return _sim; } + core::ISimulationCore* Core() const { return _core; } robots::RobotSystem* Robot() const { return _robot; } scene::ObjectID DefaultObjectID() const; scene::ObjectID ObjectID() const; @@ -104,7 +101,7 @@ namespace commands { private: - gui::SimManager* _sim = nullptr; + core::ISimulationCore* _core = nullptr; physics::PhysicsSystem* _phys = nullptr; robots::RobotSystem* _robot = nullptr; scene::ObjectID _objID; diff --git a/Sim_Engine/EngineLib/include/Interpreter/Commands/TrajClearCmd.h b/Sim_Engine/EngineLib/include/Interpreter/Commands/TrajClearCmd.h index fbbf527..08fd1ea 100644 --- a/Sim_Engine/EngineLib/include/Interpreter/Commands/TrajClearCmd.h +++ b/Sim_Engine/EngineLib/include/Interpreter/Commands/TrajClearCmd.h @@ -23,7 +23,7 @@ namespace commands { program_data::CmdResult update(CommandContextMotion& cntx, double dt) override; void execute() override; - gui::SimManager* _sim = nullptr; + core::ISimulationCore* _core = nullptr; bool _done = false; bool _started = false; diff --git a/Sim_Engine/EngineLib/include/Interpreter/MainContext.h b/Sim_Engine/EngineLib/include/Interpreter/MainContext.h index d12796c..6d36f25 100644 --- a/Sim_Engine/EngineLib/include/Interpreter/MainContext.h +++ b/Sim_Engine/EngineLib/include/Interpreter/MainContext.h @@ -10,8 +10,8 @@ namespace commands { // Main context combining motion and UI contexts class ENGINE_API MainContext { public: - MainContext(gui::SimManager* sim, scene::ObjectID objID) - : _motion(sim, objID), _ui(sim, objID) {} + MainContext(core::ISimulationCore* core) + : _motion(core), _ui(core) {} // Accessors commands::CommandContextMotion& motion() { return _motion; } diff --git a/Sim_Engine/EngineLib/include/Interpreter/SimFwd.h b/Sim_Engine/EngineLib/include/Interpreter/SimFwd.h index 27f3d24..872c699 100644 --- a/Sim_Engine/EngineLib/include/Interpreter/SimFwd.h +++ b/Sim_Engine/EngineLib/include/Interpreter/SimFwd.h @@ -16,6 +16,6 @@ namespace scene { enum class ObjectID : std::uint32_t; class ENGINE_API Object; } -namespace gui { class ENGINE_API SimManager; } +namespace core { struct ENGINE_API ISimulationCore; } namespace robots { class ENGINE_API RobotSystem; } namespace physics { class ENGINE_API PhysicsSystem; } diff --git a/Sim_Engine/EngineLib/include/Interpreter/StoredProgram.h b/Sim_Engine/EngineLib/include/Interpreter/StoredProgram.h index a80e78e..095eb7f 100644 --- a/Sim_Engine/EngineLib/include/Interpreter/StoredProgram.h +++ b/Sim_Engine/EngineLib/include/Interpreter/StoredProgram.h @@ -13,7 +13,7 @@ namespace interpreter { class ENGINE_API StoredProgram : public IStoredProgram { public: // Constructor - StoredProgram(gui::SimManager* sim); + StoredProgram(core::ISimulationCore* core); ~StoredProgram() override; // Delete copy constructor and assignment operator to prevent copies @@ -82,7 +82,7 @@ namespace interpreter { mathlib::Vec3 getColour() const override; private: - gui::SimManager* _sim = nullptr; + core::ISimulationCore* _core = nullptr; commands::MainContext _cntx; scene::Object* _defaultObj = nullptr; diff --git a/Sim_Engine/EngineLib/include/Interpreter/UIContext.h b/Sim_Engine/EngineLib/include/Interpreter/UIContext.h index d7281ad..a3a4e70 100644 --- a/Sim_Engine/EngineLib/include/Interpreter/UIContext.h +++ b/Sim_Engine/EngineLib/include/Interpreter/UIContext.h @@ -13,9 +13,9 @@ namespace interpreter { class ENGINE_API StoredProgram; } namespace commands { class ENGINE_API UIContext { public: - UIContext(gui::SimManager* sim, scene::ObjectID obj); + UIContext(core::ISimulationCore* core); - gui::SimManager* Sim() const { return _sim; } + core::ISimulationCore* Core() const { return _core; } robots::RobotSystem* Robot() const { return _robot; } scene::ObjectID DefaultObjectID() const; scene::ObjectID ObjectID() const; @@ -47,7 +47,7 @@ namespace commands { utils::OpResult startSim(); private: - gui::SimManager* _sim = nullptr; + core::ISimulationCore* _core = nullptr; physics::PhysicsSystem* _phys = nullptr; robots::RobotSystem* _robot = nullptr; scene::ObjectID _objID; diff --git a/Sim_Engine/EngineLib/include/Numerics/IntegrationMethods.h b/Sim_Engine/EngineLib/include/Numerics/IntegrationMethods.h new file mode 100644 index 0000000..cf89e1a --- /dev/null +++ b/Sim_Engine/EngineLib/include/Numerics/IntegrationMethods.h @@ -0,0 +1,12 @@ +#pragma once + +namespace integration { + enum class eIntegrationMethod { + Euler = 0, // First-Order Euler Method + Midpoint = 1, // Second-Order Runge-Kutta (Midpoint) + Heun = 2, // Second-Order Runge-Kutta (Heun) + Ralston = 3, // Second-Order Runge-Kutta (Ralston) + RK4 = 4, // Fourth-Order Runge-Kutta + RK45 = 5 // RK45 Method with Adaptive Step Size (Dormand-Prince) + }; +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Numerics/IntegrationService.h b/Sim_Engine/EngineLib/include/Numerics/IntegrationService.h index 21cbd0a..e5ca4c0 100644 --- a/Sim_Engine/EngineLib/include/Numerics/IntegrationService.h +++ b/Sim_Engine/EngineLib/include/Numerics/IntegrationService.h @@ -8,20 +8,11 @@ #include #include #include +#include "Numerics/IntegrationMethods.h" #include "Platform/Logger.h" namespace integration { - // Integrator Methods - enum class eIntegrationMethod { - Euler = 0, // First-Order Euler Method - Midpoint = 1, // Second-Order Runge-Kutta (Midpoint) - Heun = 2, // Second-Order Runge-Kutta (Heun) - Ralston = 3, // Second-Order Runge-Kutta (Ralston) - RK4 = 4, // Fourth-Order Runge-Kutta - RK45 = 5 // RK45 Method with Adaptive Step Size (Dormand-Prince) - }; - // Struct representing the result of a single integration step struct ENGINE_API StepOut { VecX x_next; // next state vector @@ -29,7 +20,6 @@ namespace integration { double dt_sug = 0.0; // suggested next step size }; - // Class representing the integration service class ENGINE_API IntegrationService { public: diff --git a/Sim_Engine/EngineLib/include/Platform/ISimulationCore.h b/Sim_Engine/EngineLib/include/Platform/ISimulationCore.h new file mode 100644 index 0000000..cf16cc9 --- /dev/null +++ b/Sim_Engine/EngineLib/include/Platform/ISimulationCore.h @@ -0,0 +1,56 @@ +#pragma once +// File: ISimulationCore.h +// GitHub: SaltyJoss +#include "EngineCore.h" +#include +#include + +// Forward Declarations +namespace integration { enum class eIntegrationMethod; } +namespace physics { class ENGINE_API PhysicsSystem; } +namespace robots { class ENGINE_API RobotSystem; } +namespace control { class ENGINE_API TrajectoryManager; } +namespace scene { class ENGINE_API Object; enum class ObjectID : std::uint32_t; } +namespace diagnostics { class ENGINE_API TelemetryRecorder; } +namespace interpreter { class ENGINE_API IStoredProgram; } + +namespace core { + // Headless API for the Simulation Core + // Aimed at allowing scripts and other systems to interact with the simulation without direct access to the full SimulationCore implementation + struct ENGINE_API ISimulationCore { + virtual ~ISimulationCore() = default; + + // Simulation control + virtual void startSimulation() = 0; + virtual void stopSimulation() = 0; + virtual bool isSimRunning() const = 0; + // Time stepping + virtual void updatePhysics(double dt) = 0; + virtual void setFixedDt(double dt) = 0; + virtual double fixedDt() const = 0; + virtual double simTime() const = 0; + // Integrator + virtual void setIntegrationMethod(integration::eIntegrationMethod method) = 0; + virtual std::string integrationMethodName() const = 0; + // Subsystems + virtual physics::PhysicsSystem* physicsSystem() = 0; + virtual robots::RobotSystem* robotSystem() = 0; + virtual control::TrajectoryManager* trajectoryManager() = 0; + // Robot management + virtual bool hasRobot() const = 0; + virtual void loadRobot(const std::string& name) = 0; + virtual void clearRobot() = 0; + // Scene objects management + virtual std::vector>& getObjects() = 0; + virtual void deleteObject(int index) = 0; + virtual std::vector loadMeshReturn(const std::string& path) = 0; + // Scene lookup (DSL access) + virtual scene::Object* getObject() = 0; + virtual scene::Object* getObjectByID(scene::ObjectID id) = 0; + // Script execution + virtual bool runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) = 0; + // Telemetry access + virtual diagnostics::TelemetryRecorder& telemetry() = 0; + virtual size_t telemetrySampleCount() const = 0; + }; +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Platform/StudyRunner.h b/Sim_Engine/EngineLib/include/Platform/StudyRunner.h new file mode 100644 index 0000000..f1c1680 --- /dev/null +++ b/Sim_Engine/EngineLib/include/Platform/StudyRunner.h @@ -0,0 +1,53 @@ +#pragma once +// File: StudyRunner.h +// GitHub: SaltyJoss +#include +#include +#include +#include +#include +#include + +#include "Platform/ISimulationCore.h" +#include "Interpreter/StoredProgram.h" + +// Type alias for a unique_ptr to ISimulationCore with a custom deleter (using std::function for flexibility) +using CorePtr = std::unique_ptr>; + +// Struct to hold the results of a study/research run +struct StudyResult { + bool success; + std::string tag; // user tag for this run (I am so fed up of renaming in file explorer) + std::string intName; // integration method name + double dt; + double simTime; + size_t samples; +}; + +// StudyRunner class to manage running batches of studies in parallel +class StudyRunner { +public: + // Config for a run + struct config { + integration::eIntegrationMethod method; + double dt; // timestep (secs) + double len_min; // length of run (mins) + std::string tag; // user-provided id for run + }; + + // Factory that creates a full configured SimulationCore for a run (its a fresh instance) + using MakeCoreFn = std::function; + + // Constructor and destructor + StudyRunner(MakeCoreFn makeCore, size_t maxConcurrency = 0); + ~StudyRunner() = default; + + // Run a batch of studies in parallel using the script string as the "run" for each sim, returning the results when all are complete + std::vector runStudies(const std::vector& configs, std::string& scriptText); + +private: + MakeCoreFn _makeCore; // Factory function to create SimulationCore instances for each run + size_t _maxConcurrency; // Maximum number of concurrent runs (0 for hardware concurrency - limit) +}; + +// Keeping out of a namespace so I can avoid avoid circular dependencies, also so it is easily accessed without many nested namespaces :) \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Rendering/OpenGLBufferManager.h b/Sim_Engine/EngineLib/include/Rendering/OpenGLBufferManager.h index 8a1be6f..6e0339e 100644 --- a/Sim_Engine/EngineLib/include/Rendering/OpenGLBufferManager.h +++ b/Sim_Engine/EngineLib/include/Rendering/OpenGLBufferManager.h @@ -12,7 +12,7 @@ namespace render { public: OpenGLVertexIndexBuffer() : VertexIndexBuffer() {} - void createBuffers(const std::vector& vertices, const std::vector& indices) override; + void createBuffers(const std::vector& vertices, const std::vector& indices) override; void deleteBuffers() override; void bind() override; void unbind() override; diff --git a/Sim_Engine/EngineLib/include/Rendering/RenderBase.h b/Sim_Engine/EngineLib/include/Rendering/RenderBase.h index 2abbd0e..3d9de73 100644 --- a/Sim_Engine/EngineLib/include/Rendering/RenderBase.h +++ b/Sim_Engine/EngineLib/include/Rendering/RenderBase.h @@ -4,7 +4,7 @@ #include "EngineCore.h" #include "Platform/Window.h" -#include "Scene/VertexHolder.h" +#include "Assets/VertexHolder.h" #include #include "Platform/Logger.h" @@ -22,7 +22,7 @@ namespace render { // Replaces and Centralises old VAO, VBO, EBO classes -> See OpenGLBufferManager VertexIndexBuffer() : _VAO{ 0 }, _VBO{ 0 }, _EBO{ 0 } {} - virtual void createBuffers(const std::vector& verticies, const std::vector& indices) = 0; + virtual void createBuffers(const std::vector& verticies, const std::vector& indices) = 0; virtual void deleteBuffers() = 0; virtual void bind() = 0; virtual void unbind() = 0; diff --git a/Sim_Engine/EngineLib/include/Scene/Mesh.h b/Sim_Engine/EngineLib/include/Scene/Mesh.h index ff288e1..b0f78eb 100644 --- a/Sim_Engine/EngineLib/include/Scene/Mesh.h +++ b/Sim_Engine/EngineLib/include/Scene/Mesh.h @@ -7,7 +7,7 @@ #include "EngineCore.h" #include "Rendering/RenderBase.h" -#include "Scene/VertexHolder.h" +#include "Assets/VertexHolder.h" #include "Scene/Element.h" #include "Scene/Face.h" @@ -26,11 +26,11 @@ namespace scene { bool load(const std::string& filepath); // CPU buffers - std::vector _vertices; + std::vector _vertices; std::vector _indices; // Utility methods for building mesh geometry - void addVertex(const VertexHolder& vertex) { _vertices.push_back(vertex); } + void addVertex(const assets::VertexHolder& vertex) { _vertices.push_back(vertex); } void addVertexIndex(unsigned int vertexIndx) { _indices.push_back(vertexIndx); } // GPU buffer management @@ -72,7 +72,7 @@ namespace scene { // Transform and append vertices for (const auto& v : other._vertices) { - VertexHolder out = v; + assets::VertexHolder out = v; // Apply the local transform to the vertex position and normal glm::vec4 p = T * glm::vec4(v._pos, 1.0f); diff --git a/Sim_Engine/EngineLib/include/Scene/ObjectID.h b/Sim_Engine/EngineLib/include/Scene/ObjectID.h index 332b136..cb7c99a 100644 --- a/Sim_Engine/EngineLib/include/Scene/ObjectID.h +++ b/Sim_Engine/EngineLib/include/Scene/ObjectID.h @@ -5,10 +5,10 @@ // ObjectID is a strongly-typed identifier for scene objects, using uint32_t as the underlying type. namespace scene { - enum class ObjectID : std::uint32_t { INVALID_OBJECT_ID = 0 }; // Special invalid ObjectID - inline constexpr ObjectID FIRST_VALID_OBJECT_ID = static_cast(1); // First valid ObjectID + enum class ObjectID : std::uint32_t { INVALID_OBJECT_ID = 0 }; // Special invalid ObjectID + inline constexpr ObjectID FIRST_VALID_OBJECT_ID = static_cast(1); // First valid ObjectID - inline std::uint32_t toUInt32(ObjectID id) { return static_cast(id); } // Helper to convert ObjectID to uint32_t - inline ObjectID fromUInt32(std::uint32_t value) { return static_cast(value); } // Helper to convert uint32_t to ObjectID - inline ObjectID nextID(ObjectID id) { return fromUInt32(toUInt32(id) + 1u); } // Helper to get the next ObjectID + inline std::uint32_t toUInt32(ObjectID id) { return static_cast(id); } // Helper to convert ObjectID to uint32_t + inline ObjectID fromUInt32(std::uint32_t value) { return static_cast(value); } // Helper to convert uint32_t to ObjectID + inline ObjectID nextID(ObjectID id) { return fromUInt32(toUInt32(id) + 1u); } // Helper to get the next ObjectID }// namespace scene \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h index 2bf6438..d65afbd 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h @@ -2,6 +2,7 @@ // File: SimulationCore.h // GitHub: SaltyJoss #include "EngineCore.h" +#include "Platform/ISimulationCore.h" #include #include #include "Platform/SimulationState.h" @@ -24,31 +25,72 @@ namespace core { 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 - class ENGINE_API SimulationCore { + class ENGINE_API SimulationCore : public ISimulationCore { public: SimulationCore(); - // Physics - void updatePhysics(double dt); - void tick(double frame_dt); - void stepFixed(double frame_dt); + // Simulation control + void startSimulation() override; + void stopSimulation() override; + bool isSimRunning() const override { return _simRunning; } + + // Time stepping + void updatePhysics(double dt) override; + void setFixedDt(double dt) override; + void setSimTime(double t) { _simTime = t; } + double fixedDt() const override; + double simTime() const override; - // Simulation Integrators + // Integrator void setupSimulationIntegrator(); + void setIntegrationMethod(integration::eIntegrationMethod method) override; + std::string integrationMethodName() const override; + + // Subsystems access + physics::PhysicsSystem* physicsSystem() override; + const physics::PhysicsSystem* physicsSystem() const; + robots::RobotSystem* robotSystem() override; + const robots::RobotSystem* robotSystem() const; + control::TrajectoryManager* trajectoryManager() override; + const control::TrajectoryManager* trajectoryManager() const; + + // Robot management + bool hasRobot() const override; + void loadRobot(const std::string& name) override; + void clearRobot() override; + + // Scene objects management + std::vector>& getObjects() override; + void deleteObject(int index) override; + std::vector loadMeshReturn(const std::string& path) override; + // Object lookup + scene::Object* getObject() override; + scene::Object* getObjectByID(scene::ObjectID id) override; + + // Run a script to completion synchronously with a specific integrator + bool runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) override; - // Start or stop the simulation loop - void startSimulation(); - void stopSimulation(); - const bool isSimRunning() const { return _simRunning; } + // Telemetry + diagnostics::TelemetryRecorder& telemetry() override; + const diagnostics::TelemetryRecorder& telemetry() const; + size_t telemetrySampleCount() const override; + + // Setters for subsystems and scene objects + void setPhysicsSystem(physics::PhysicsSystem* physics); + void setRobotSystem(robots::RobotSystem* robot); + void setTrajectoryManager(control::TrajectoryManager* traj); + void setObjects(std::vector>* objects); + void setJointLogBuffer(robots::JointLogBuffer* buffer); + void setTrajRefBuffer(robots::TrajRefBuffer* buffer); + + // Helpers + void tick(double frame_dt); + void stepFixed(double frame_dt); // Export logged telemetry data to HDF5 files void exportLogsToHDF5(); void exportRefsToHDF5(); - // Setter and getter for current simulation time (seconds) - void setSimTime(double t) { _simTime = t; } - const double simTime() const { return _simTime; } - // Increment simulation time by dt (used in the simulation loop) void incrementSimTime(double dt) { _simTime += dt; } @@ -56,10 +98,6 @@ namespace core { void setScriptRunning(bool running) { _scriptRunning = running; } const bool isScriptRunning() const { return _scriptRunning; } - // Setter and getter for fixed timestep (seconds) - void setFixedDt(double dt) { _dt = dt; } - const double fixedDt() const { return _dt; } - // Setter and getter for telemetry frequency (Hz) void setTelemetryHz(double hz) { _telHz = hz; } const double telemetryHz() const { return _telHz; } @@ -68,42 +106,6 @@ namespace core { void setLastScriptText(const std::string& text) { _lastScriptText = text; } const std::string& lastScriptText() const { return _lastScriptText; } - // Run a script to completion synchronously with a specific integrator - bool runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method); - - // Setter for the physics system - void setPhysicsSystem(physics::PhysicsSystem* physics); - - // Accessor for the physics system (non-const and const versions) - physics::PhysicsSystem* physicsSystem(); - const physics::PhysicsSystem* physicsSystem() const; - - // Setter and checker for Robot System - void setRobotSystem(robots::RobotSystem* robot); - bool hasRobot() const; - - // Accessor for the robot system (non-const and const versions) - robots::RobotSystem* robotSystem(); - const robots::RobotSystem* robotSystem() const; - - // Setter for the trajectory manager - void setTrajectoryManager(control::TrajectoryManager* traj); - - // Accessor for the trajectory manager (non-const and const versions) - control::TrajectoryManager* trajectoryManager(); - const control::TrajectoryManager* trajectoryManager() const; - - // Setters the scene objects pointer (used for object lookup by scripts) - void setObjects(std::vector>* objects); - - // Setters for the metric buffers - void setJointLogBuffer(robots::JointLogBuffer* buffer); - void setTrajRefBuffer(robots::TrajRefBuffer* buffer); - - // Accesors for the telemetry recorder (non-const and const versions) - diagnostics::TelemetryRecorder& telemetry(); - const diagnostics::TelemetryRecorder& telemetry() const; - // Accesors for the active script program void setActiveProgram(interpreter::IStoredProgram* p); interpreter::IStoredProgram* activeProgram() const; @@ -123,7 +125,7 @@ namespace core { control::TrajectoryManager* _traj = nullptr; std::vector>* _objects = nullptr; - // Run mode (interactive vs synchronous) + // Run mode eRunMode _runMode = eRunMode::Interactive; // Last script text for comparison re-use diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h index ad66691..0235774 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h @@ -237,6 +237,10 @@ namespace gui { diagnostics::TelemetryRecorder& telemetry(); const diagnostics::TelemetryRecorder& telemetry() const; + // Accessors for the Simulation Core interface (non-const and const versions) + core::ISimulationCore* simCoreInterface(); + const core::ISimulationCore* simCoreInterface() const; + // Accesor for Simulation Core (non-const and const versions) core::SimulationCore* simCore(); const core::SimulationCore* simCore() const; @@ -249,9 +253,9 @@ namespace gui { void onMouseWheel(double delta); void resetMouseDelta(); + private: + std::unique_ptr _core = nullptr; - - private: // Rendering Pipeline Methods void MeshRender(scene::Camera* cam); void WorldGridRender(scene::Camera* cam, int rtW); @@ -274,8 +278,8 @@ namespace gui { bool _glReady = false; // Sizes & Display - glm::vec2 _internalSize = { 1920.0f, 1080.0f }; // Internal render target size - glm::vec2 _displaySize = { 1920.0f, 1080.0f }; // Actual display size + glm::vec2 _internalSize{ 1920.0f, 1080.0f }; // Internal render target size + glm::vec2 _displaySize{ 1920.0f, 1080.0f }; // Actual display size glm::vec3 _backgroundColour{ 1.0f, 1.0f, 1.0f }; // Background colour (default white, but can be changed by user) // Cached display size for scaling calculations (updated on resize) @@ -336,7 +340,5 @@ namespace gui { // Camera & Mouse glm::vec2 _lastMousePos{ 0.f, 0.f }; - - std::unique_ptr _core = nullptr; }; } // namespace gui \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Scene/MeshLoader.cpp b/Sim_Engine/EngineLib/src/Assets/MeshLoader.cpp similarity index 97% rename from Sim_Engine/EngineLib/src/Scene/MeshLoader.cpp rename to Sim_Engine/EngineLib/src/Assets/MeshLoader.cpp index 0725fc0..2561b4f 100644 --- a/Sim_Engine/EngineLib/src/Scene/MeshLoader.cpp +++ b/Sim_Engine/EngineLib/src/Assets/MeshLoader.cpp @@ -1,9 +1,7 @@ #include "pch.h" // File: MeshLoader.cpp // GitHub: SaltyJoss -#include "Scene/MeshLoader.h" -#include "Scene/VertexHolder.h" - +#include "Assets/MeshLoader.h" #include #include #include @@ -12,7 +10,7 @@ #include "EngineLib/LogMacros.h" -namespace gui { +namespace assets { // Load a mesh from the specified file path and return a vector of shared pointers to Mesh objects std::vector> MeshLoader::load(const std::string& filepath) { _imported.clear(); @@ -76,7 +74,7 @@ namespace gui { // Get the vertices of the mesh and add them to the result mesh, applying the local transform to the vertex positions and normals for (unsigned int i = 0; i < mesh->mNumVertices; ++i) { - scene::VertexHolder vh; + assets::VertexHolder vh; vh._pos = glm::vec3(mesh->mVertices[i].x, mesh->mVertices[i].y, mesh->mVertices[i].z); vh._normal = mesh->mNormals ? glm::vec3(mesh->mNormals[i].x, mesh->mNormals[i].y, mesh->mNormals[i].z) diff --git a/Sim_Engine/EngineLib/src/Assets/importObj.cpp b/Sim_Engine/EngineLib/src/Assets/importObj.cpp index ca5c05c..c92af12 100644 --- a/Sim_Engine/EngineLib/src/Assets/importObj.cpp +++ b/Sim_Engine/EngineLib/src/Assets/importObj.cpp @@ -2,7 +2,7 @@ // File: importObj.cpp // GitHub: SaltyJoss #include "Assets/importObj.h" -#include "Scene/VertexHolder.h" +#include "Assets/VertexHolder.h" #include "Platform/str_utils.h" #include "EngineLib/LogMacros.h" diff --git a/Sim_Engine/EngineLib/src/EngineCore.cpp b/Sim_Engine/EngineLib/src/EngineCore.cpp new file mode 100644 index 0000000..5143681 --- /dev/null +++ b/Sim_Engine/EngineLib/src/EngineCore.cpp @@ -0,0 +1,17 @@ +#include "pch.h" +// File: EngineCore.cpp +// GitHub: SaltyJoss +#include "EngineCore.h" +#include "Scene/SimulationCore.h" // contains core::SimulationCore and core::ISimulationCore + +extern "C" { + // Factory function to create a SimulationCore instance + ENGINE_API core::ISimulationCore* CreateSimulationCore_v1() { + try { return new core::SimulationCore(); } + catch (...) { return nullptr; /*Avoids throwing exceptions across C ABI boundary by return null on failure*/ } + } + // Destroys a SimulationCore instance + ENGINE_API void DestroySimulationCore(core::ISimulationCore* p) { + delete p; // makes sure that deletion happens in the dll that created it. + } +} // extern "C" \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp b/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp index 2384cb9..29c3831 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/CommandContextMotion.cpp @@ -2,7 +2,7 @@ // File: CommandContextMotion.cpp // GitHub: SaltyJoss #include "Interpreter/CommandContextMotion.h" -#include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" #include "Scene/ObjectID.h" #include "Robots/RobotSystem.h" @@ -14,9 +14,11 @@ using namespace utils; namespace commands { // Constructor - CommandContextMotion::CommandContextMotion(gui::SimManager* sim, scene::ObjectID objID) - : _sim(sim), _phys(sim ? sim->physicsSystem() : nullptr), _robot(sim ? sim->robotSystem() : nullptr), - _objID(objID), _defaultObjID(objID), _angularUnits(AngularUnits::DegPerSec) { + CommandContextMotion::CommandContextMotion(core::ISimulationCore* core) + : _core(core), _phys(core ? core->physicsSystem() : nullptr), + _robot(core ? core->robotSystem() : nullptr), _angularUnits(AngularUnits::DegPerSec) { + _defaultObjID = scene::ObjectID::INVALID_OBJECT_ID; + _objID = _core->getObjectByID(_defaultObjID) ? _defaultObjID : scene::ObjectID::INVALID_OBJECT_ID; } // --- OBJECT RESOLUTION METHODS --- @@ -24,9 +26,9 @@ namespace commands { scene::ObjectID CommandContextMotion::ObjectID() const { return _objID; } scene::Object* CommandContextMotion::resolveObject(scene::ObjectID id) const { - if (!_sim) return nullptr; + if (!_core) return nullptr; if (id == scene::ObjectID::INVALID_OBJECT_ID) return nullptr; - return _sim->getObjectByID(id); + return _core->getObjectByID(id); } scene::Object* CommandContextMotion::resolveCurrentObject() const { return resolveObject(_objID); } diff --git a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp index 25a4bb2..81ef7b4 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajClearCmd.cpp @@ -4,7 +4,7 @@ #include "Interpreter/Commands/TrajClearCmd.h" #include "Robots/TrajectoryManager.h" #include "Robots/RobotSystem.h" -#include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" #include "EngineLib/LogMacros.h" #include "Interpreter/Utils.h" @@ -27,10 +27,11 @@ namespace commands { program_data::CmdResult TrajClearCmd::update(CommandContextMotion& cntx, double /*dt*/) { if (_done) return { CmdState::Executed, {}, "" }; - gui::SimManager* sim = cntx.Sim(); - if (!sim) return { CmdState::Failed, {}, "trajClear: no sim in context." }; + core::ISimulationCore* core = cntx.Core(); + if (!core) return { CmdState::Failed, {}, "trajClear: no sim in context." }; - auto trajMgr = sim->traj(); + auto trajMgr = core->trajectoryManager(); + if (!trajMgr) return { CmdState::Failed, {}, "trajClear: no trajectory manager in sim." }; trajMgr->clearAll(); // clear all trajectories // Zero qd/qdd refs for all joints so nothing lingers diff --git a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp index d76ec46..ac91c64 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/Commands/TrajSetCmd.cpp @@ -2,7 +2,7 @@ // File: TrajSetCmd.cpp // GitHub: SaltyJoss #include "Interpreter/Commands/TrajSetCmd.h" -#include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" #include "Robots/RobotSystem.h" #include "Robots/TrajectoryManager.h" @@ -55,8 +55,8 @@ namespace commands { // Updates trajSet command program_data::CmdResult TrajSetCmd::update(CommandContextMotion& cntx, double dt) { - auto* sim = cntx.Sim(); - if (!sim) { + auto* core = cntx.Core(); + if (!core) { markFailed("trajSet: no SimulationManager in context."); return { CmdState::Failed, {}, "trajSet failed" }; } @@ -75,7 +75,7 @@ namespace commands { return { CmdState::Failed, {}, "trajSet failed" }; } - const double t0 = sim->simTime(); + const double t0 = core->simTime(); const std::string typeU = upperCopy(trimCopy(_type)); // Get hardware max omega @@ -99,7 +99,7 @@ namespace commands { const double amax = degToRad(_params[2]); auto traj = std::make_unique(t0, q0, q1, vmax, amax); - auto trajMgr = sim->traj(); + auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); double wMax_est = std::abs(vmax); @@ -149,7 +149,7 @@ namespace commands { double phi = (_params.size() == 5) ? degToRad(_params[4]) : 0.0; // radians auto traj = std::make_unique(t0, t0 + dur, centre, amp, fHz, phi); - auto trajMgr = sim->traj(); + auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); double wMax_est = TWO_PI_d * fHz * amp; @@ -219,7 +219,7 @@ namespace commands { } auto traj = std::make_unique(t0, t0 + dur, centre, std::move(comps)); - auto trajMgr = sim->traj(); + auto trajMgr = core->trajectoryManager(); trajMgr->set(_link, std::move(traj)); SIM_SUCCESS("trajSet: MSINE link='%s' dur=%.6fs centre=%.6f nComps=%zu", _link.c_str(), centre, dur, nComps); diff --git a/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp b/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp index 5f61dcf..cd3e157 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp @@ -2,7 +2,7 @@ // File: StoredProgram.cpp // GitHub: SaltyJoss #include "Interpreter/StoredProgram.h" -#include "Scene/SimulationManager.h" +#include "Platform/ISimulationCore.h" #include "Physics/PhysicsSystem.h" #include "Robots/RobotSystem.h" #include "Scene/ObjectID.h" @@ -11,8 +11,13 @@ #include "EngineLib/LogMacros.h" namespace interpreter { - StoredProgram::StoredProgram(gui::SimManager* sim) : _currentLineNumber(0), PC(0), _sim(sim), - _cntx(sim, [&] { scene::Object* o = (sim ? sim->getObject() : nullptr); return o ? o->id : scene::ObjectID::INVALID_OBJECT_ID; }()) { + StoredProgram::StoredProgram(core::ISimulationCore* core) + : _currentLineNumber(0), PC(0), _core(core), _cntx(core) { + // If necessary, set a default object by querying core->getObject() + if (_core) { + scene::Object* obj = _core->getObject(); + _cntx.motion().setDefaultObjectID(obj ? obj->id : scene::ObjectID::INVALID_OBJECT_ID); + } } StoredProgram::~StoredProgram() { clear(); } @@ -69,7 +74,7 @@ namespace interpreter { // Start simulation void StoredProgram::startSim() { if (_state != ProgramState::Running) { start(); } - if (_sim && !_sim->isSimRunning()) { _sim->startSimulation(); } + if (_core && !_core->isSimRunning()) { _core->startSimulation(); } } // Stop program execution @@ -82,8 +87,8 @@ namespace interpreter { // Stop simulation void StoredProgram::stopSim() { - if (!_sim) { return; } - if (_sim->isSimRunning()) { _sim->stopSimulation(); } + if (!_core) { return; } + if (_core->isSimRunning()) { _core->stopSimulation(); } scene::Object* obj = _cntx.motion().resolveDefaultObject(); // <-- uses stored default ID if (obj) { @@ -92,7 +97,7 @@ namespace interpreter { _cntx.motion().stopTranslation(obj, all); } - if (_sim->hasRobot()) { _cntx.motion().Robot()->stopAll(); } + if (_core->hasRobot()) { _cntx.motion().Robot()->stopAll(); } } // Pause program execution @@ -106,19 +111,19 @@ namespace interpreter { _cntx.motion().stopTranslation(obj, all); } - if (_sim->hasRobot()) { _cntx.motion().Robot()->stopAll(); } + if (_core->hasRobot()) { _cntx.motion().Robot()->stopAll(); } } // Wait for simulation to run for dt seconds void StoredProgram::waitSim(double dt) { - if (_sim && !_sim->isSimRunning()) { _sim->startSimulation(); } + if (_core && !_core->isSimRunning()) { _core->startSimulation(); } double elapsed = 0.0; - const double stepDt = _sim ? _sim->fixedDt() : static_cast(1.0 / 180.0); + const double stepDt = _core ? _core->fixedDt() : static_cast(1.0 / 180.0); while (elapsed < dt) { - if (_sim) { _sim->updatePhysics(stepDt); } + if (_core) { _core->updatePhysics(stepDt); } elapsed += stepDt; } - if (_sim && _sim->isSimRunning()) { _sim->stopSimulation(); } + if (_core && _core->isSimRunning()) { _core->stopSimulation(); } } // Get current program status @@ -140,7 +145,7 @@ namespace interpreter { if (!commandsLeft()) { _state = ProgramState::Completed; return; } // Ensure default object is valid in context - scene::Object* o = _defaultObj ? _defaultObj : (_sim ? _sim->getObject() : nullptr); + scene::Object* o = _defaultObj ? _defaultObj : (_core ? _core->getObject() : nullptr); _cntx.motion().setDefaultObjectID(o ? o->id : scene::ObjectID::INVALID_OBJECT_ID); // Get current command @@ -176,14 +181,14 @@ namespace interpreter { // Set Integrator Method void StoredProgram::setIntegratorMethod(IntegratorMethod method) { _integratorMethod = method; - if (_sim) { - if (_sim->hasRobot()) { - robots::RobotSystem* robot = _sim->robotSystem(); + if (_core) { + if (_core->hasRobot()) { + robots::RobotSystem* robot = _core->robotSystem(); if (!robot) { D_FAIL("No robot system found in simulation manager."); return; } robot->setIntegrationMethod(static_cast(method)); } - physics::PhysicsSystem* phys = _sim->physicsSystem(); + physics::PhysicsSystem* phys = _core->physicsSystem(); if (!phys) { D_FAIL("No physics system found in simulation manager."); return; } phys->setIntegrationMethod(static_cast(method)); } @@ -198,38 +203,38 @@ namespace interpreter { } // Set Fixed Dt - void StoredProgram::setFixedDt(double dt) { _dt = dt; if (_sim) { _sim->setFixedDt(dt); } } + void StoredProgram::setFixedDt(double dt) { _dt = dt; if (_core) { _core->setFixedDt(dt); } } // Get Fixed Dt double StoredProgram::getFixedDt() const { - if (_sim) { return _sim->fixedDt(); } + if (_core) { return _core->fixedDt(); } return _dt; } // Set Gravity void StoredProgram::setGravity(double gravity) { _gravity = gravity; - if (_sim) { - if (_sim->hasRobot()) { - robots::RobotSystem* robot = _sim->robotSystem(); + if (_core) { + if (_core->hasRobot()) { + robots::RobotSystem* robot = _core->robotSystem(); if (!robot) { D_FAIL("No robot system found in simulation manager."); return; } robot->setGravity(gravity); } - - physics::PhysicsSystem* phys = _sim->physicsSystem(); + + physics::PhysicsSystem* phys = _core->physicsSystem(); if (!phys) { D_FAIL("No physics system found in simulation manager."); return; } phys->setGravity(mathlib::Vec3(0.0f, 0.0f, static_cast(gravity))); } } // Get Gravity double StoredProgram::getGravity() const { - if (_sim) { - if (_sim->hasRobot()) { - robots::RobotSystem* robot = _sim->robotSystem(); + if (_core) { + if (_core->hasRobot()) { + robots::RobotSystem* robot = _core->robotSystem(); if (!robot) { D_FAIL("No robot system found in simulation manager."); return _gravity; } return robot->getGravity(); } - physics::PhysicsSystem* phys = _sim->physicsSystem(); + physics::PhysicsSystem* phys = _core->physicsSystem(); if (!phys) { D_FAIL("No physics system found in simulation manager."); return _gravity; } return phys->getGravity().y(); } @@ -239,9 +244,10 @@ namespace interpreter { // Set Colour void StoredProgram::setColour(mathlib::Vec3 rgb) { _rgb = rgb; - if (_sim) { - _sim->setLightColour(toGlm(rgb)); - D_INFO("Set shader albedo -> %.2f,%.2f,%.2f", rgb[0],rgb[1],rgb[2]); + if (_core) { + D_WARN("This method has not been integrated with the rendering system yet, so it has no effect."); + // _core->setShaderAlbedo(toGlm(rgb)); + // D_INFO("Set shader albedo -> %.2f,%.2f,%.2f", rgb[0],rgb[1],rgb[2]); } } diff --git a/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp b/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp index 1b14e88..6254016 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/UIContext.cpp @@ -3,7 +3,7 @@ // GitHub: SaltyJoss #include "Interpreter/UIContext.h" #include "EngineLib/LogMacros.h" -#include "Scene/SimulationManager.h" +#include "Scene/SimulationCore.h" #include "Physics/PhysicsSystem.h" #include "Robots/RobotSystem.h" #include "Scene/ObjectID.h" @@ -18,9 +18,11 @@ using namespace utils; namespace commands { // Constructor - UIContext::UIContext(gui::SimManager* sim, scene::ObjectID objID) - : _sim(sim), _phys(sim ? sim->physicsSystem() : nullptr), _robot(sim ? sim->robotSystem() : nullptr), - _objID(objID), _defaultObjID(objID), _angularUnits(AngularUnits::DegPerSec) { + UIContext::UIContext(core::ISimulationCore* core) + : _core(core), _phys(core ? core->physicsSystem() : nullptr), + _robot(core ? core->robotSystem() : nullptr), _angularUnits(AngularUnits::DegPerSec) { + _defaultObjID = scene::ObjectID::INVALID_OBJECT_ID; + _objID = _core->getObjectByID(_defaultObjID) ? _defaultObjID : scene::ObjectID::INVALID_OBJECT_ID; } // --- OBJECT RESOLUTION METHODS --- @@ -28,9 +30,9 @@ namespace commands { scene::ObjectID UIContext::ObjectID() const { return _objID; } scene::Object* UIContext::resolveObject(scene::ObjectID id) const { - if (!_sim) return nullptr; + if (!_core) return nullptr; if (id == scene::ObjectID::INVALID_OBJECT_ID) return nullptr; - return _sim->getObjectByID(id); + return _core->getObjectByID(id); } scene::Object* UIContext::resolveCurrentObject() const { return resolveObject(_objID); } @@ -57,9 +59,9 @@ namespace commands { // Set the fixed delta time for the simulation OpResult UIContext::setFixedDt(double dt) { - if (!_sim) { return OpResult::Failure("Simulation manager is null."); } + if (!_core) { return OpResult::Failure("Simulation manager is null."); } if (dt <= 0.0) { return OpResult::Failure("Fixed dt must be positive."); } - _sim->setFixedDt(dt); + _core->setFixedDt(dt); return OpResult::Success(true); } @@ -88,10 +90,10 @@ namespace commands { // Loads a new object from the specified file path and updates the context with the new object's ID OpResult UIContext::loadObject(const std::string& objectPath) { - if (!_sim) { return OpResult::Failure("Simulation manager is null."); } + if (!_core) { return OpResult::Failure("Simulation manager is null."); } if (objectPath.empty()) { return OpResult::Failure("Object path is empty."); } - auto spawned = _sim->loadMeshReturn(objectPath); + auto spawned = _core->loadMeshReturn(objectPath); if (spawned.empty() || !spawned[0]) { return OpResult::Failure("No objects loaded from specified path."); } _objID = spawned[0]->id; @@ -101,11 +103,11 @@ namespace commands { // Removes the current object based on its index OpResult UIContext::clearObject() { - if (!_sim) { return OpResult::Failure("Simulation manager is null."); } + if (!_core) { return OpResult::Failure("Simulation manager is null."); } if (_objID == scene::ObjectID::INVALID_OBJECT_ID) { return OpResult::Failure("No object selected."); } // Finds the index of the current object - auto& objects = _sim->getObjects(); + auto& objects = _core->getObjects(); auto it = std::find_if(objects.begin(), objects.end(), [this](const std::unique_ptr& o) { return o && o->id == _objID; }); if (it == objects.end()) { return OpResult::Failure("Selected object ID not found."); } @@ -113,7 +115,7 @@ namespace commands { const int index = (int)std::distance(objects.begin(), it); const scene::ObjectID deletedId = _objID; - _sim->deleteObject(index); + _core->deleteObject(index); // Clear context IDs safely _objID = scene::ObjectID::INVALID_OBJECT_ID; @@ -128,20 +130,20 @@ namespace commands { // Loads a robot by name and updates the context with the new robot system OpResult UIContext::loadRobot(const std::string& robotName) { - if (!_sim) { return OpResult::Failure("Simulation manager is null."); } + if (!_core) { return OpResult::Failure("Simulation manager is null."); } if (robotName.empty()) return OpResult::Failure("Robot name is empty."); - _sim->loadRobot(robotName); - _robot = _sim->robotSystem(); + _core->loadRobot(robotName); + _robot = _core->robotSystem(); if (!_robot) return OpResult::Failure("Robot system is null after load."); return OpResult::Success(true); } OpResult UIContext::clearRobot() { - if (!_sim) { return OpResult::Failure("Simulation manager is null."); } + if (!_core) { return OpResult::Failure("Simulation manager is null."); } _robot = nullptr; - _sim->clearRobot(); + _core->clearRobot(); return OpResult::Success(true); } @@ -176,11 +178,11 @@ namespace commands { // Starts the simulation if the simulation manager is available OpResult UIContext::startSim() { - if (!_sim) { + if (!_core) { LOG_WARN("Simulation manager is null, cannot start simulation."); return OpResult::Failure("Simulation manager is null."); } - _sim->startSimulation(); + _core->startSimulation(); return OpResult::Success(); } } // namespace commands diff --git a/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp b/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp new file mode 100644 index 0000000..1d82273 --- /dev/null +++ b/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp @@ -0,0 +1,138 @@ +#include "pch.h" +// File: StudyRunner.cpp +// GitHub: SaltyJoss +#include "Platform/StudyRunner.h" +#include "Interpreter/Parser.h" +#include +#include +#include + +/* Fair warning: + * ------------- + * I am going to be making ALOT of notes here as I am covering new ground for with C++ concurrency and async programming. + * This way I can understand it better and actually reference it later without having to re-learn and re-read a bunch of documentation and examples. + * I will try reduce it incase anyone is using this code, or reading through it. + * That being said, I am a student first, and my primary purpose is to learn about these subjects. + * I cannot be bothered programming all of this to just not understand it - wasted time, and wasted opportunity honestly. + */ + +// Constructor +StudyRunner::StudyRunner(MakeCoreFn makeCore, size_t maxConcurrency) + : _makeCore(std::move(makeCore)) { + if (!_makeCore) { throw std::invalid_argument("StudyRunner constructor got null makeCore function"); } + + // If maxConcurrency is 0, use hardware concurrency (number of CPU cores) as the limit for concurrent runs + if (maxConcurrency == 0) { + unsigned int hc = std::thread::hardware_concurrency(); + _maxConcurrency = (hc == 0 ? 2u : hc); // Fallback if hardware_concurrency returns 0 (not well-defined or undetectable) + // NOTE: 2u means unsigned int literal with value 2 (I genuinely forgot the syntax for unsigned literals and had to look it up) + } + else { _maxConcurrency = maxConcurrency; } +} + +// Helper function to find the index of the first future that is ready in a vector of futures +// NOTE: future is a synchronisation primitive that represents a val which will be available at some point in the future (logged for my own re-readability) +static int findReadyIndex(std::vector>& futures) { + // Check each future for readiness without blocking, returning the index of the first ready future + for (size_t i = 0; i < futures.size(); ++i) { + // Check if this future is ready by waiting for 100ms (non-blocking) + if (futures[i].wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) { return (int)i; } + } + return -1; // No future is ready yet +} + +// Run a batch of studies in parallel using the script string as the "run" for each sim, returning the results when all are complete +std::vector StudyRunner::runStudies(const std::vector& configs, std::string& scriptText) { + std::vector> futures; // Vector to hold the futures for each async run + futures.reserve(configs.size()); + + for (size_t i = 0; i < configs.size(); ++i) { + const config& cfg = configs[i]; + + // Before starting a new async run, check if we have reached the max concurrency limit. + // If we have, we need to wait for at least one of the existing runs to finish before starting a new one. + while (futures.size() >= _maxConcurrency) { + int idx = findReadyIndex(futures); // Check for a ready future without blocking the main thread + if (idx >= 0) { + // collect results from ready future, and remove it from the vector + try { + StudyResult r = futures[idx].get(); // get() will block if the future is not ready + (void)r; // silence unused variable warning + } + // catch any exceptions thrown during the async run and log them, but continue processing other runs (very important to my initial implementation goal!!) + catch (const std::exception& e) { + std::cerr << "Error in study run: " << e.what() << std::endl; + } + futures.erase(futures.begin() + idx); // remove the future from the vector after collecting its result + } + else { std::this_thread::sleep_for(std::chrono::milliseconds(5)); } // Sleep briefly to avoid busy-waiting if no futures are ready + } + // Start a new async run for this config, capturing the current config and program by value to make sure they are safely used in the async context + futures.push_back(std::async(std::launch::async, [this, cfg, scriptText]() -> StudyResult { + StudyResult result{}; + result.tag = cfg.tag; + result.dt = cfg.dt; + + auto simCore = _makeCore(); + // If we couldn't create a SimulationCore, return a failed result immediately + if (!simCore) { + result.success = false; + result.intName = "N/A"; + result.simTime = 0.0; + result.samples = 0; + return result; + } + // Configure the SimulationCore for this run before building the program + simCore->setFixedDt(cfg.dt); + simCore->setIntegrationMethod(cfg.method); + + // Create a program and parser for this run, bound to the SimulationCore we just created + auto program = std::make_unique(simCore.get()); + interpreter::Parser parser(program.get()); + // Parse the script text to build the program for this run, and start it + parser.parse(scriptText); + program->start(); + + // Run synchronously (blocking inside thread) + bool ok = simCore->runScriptToCompletion(program.get(), cfg.method); // Run the provided program/script to completion with method, blocking until it finishes. + // NOTE: This is where the actual study run happens, it is block because it is inside the async thread, so it will not block the main thread (OR other runs), and allows DSFE to have multiple runs in parallel!!! <- EXACTLY what I want + + // Get telemetry data for results + result.success = ok; + result.intName = simCore->integrationMethodName(); // Get the name of the current integration method for reporting + result.simTime = simCore->simTime(); // Get the total simulation time that elapsed during this run + // Telemetry sample count is implementation-defined -> guarded for availability + try { + result.samples = simCore->telemetrySampleCount(); + } + // If telemetry is not available or throws an exception, it is caught and samples to 0 to indicate no data was collected + catch (...) { + result.samples = 0; + } + // Get the number of telemetry samples collected during this run (proxy for how much data we obtained) + return result; // Return the result of this study run + })); + } + + // Collect the results from all the futures + std::vector results; // Vector to hold the final results of all runs + for (auto& f : futures) { + // Wait for each future to be ready and collect its result + try { + StudyResult r = f.get(); // get() will block until the future is ready, collecting the result of each run + results.push_back(std::move(r)); // Move the result into the results vector to avoid unnecessary copying + } + // catch any exceptions thrown during the async runs and log them + catch (const std::exception& e) { + std::cerr << "Error collecting study result: " << e.what() << std::endl; + // Create a failed result to represent the exception case + StudyResult bad; + bad.success = false; + bad.tag = "exception"; + // Push a failed/bad result + results.push_back(bad); + } + } + return results; // Return the collected results of all study runs +} + \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Rendering/OpenGLBufferManager.cpp b/Sim_Engine/EngineLib/src/Rendering/OpenGLBufferManager.cpp index 37644a5..c7ef66e 100644 --- a/Sim_Engine/EngineLib/src/Rendering/OpenGLBufferManager.cpp +++ b/Sim_Engine/EngineLib/src/Rendering/OpenGLBufferManager.cpp @@ -12,7 +12,7 @@ namespace render { // Creates the VAO, VBO, and EBO for this buffer using the provided vertex and index data - void OpenGLVertexIndexBuffer::createBuffers(const std::vector& vertices, const std::vector& indices) { + void OpenGLVertexIndexBuffer::createBuffers(const std::vector& vertices, const std::vector& indices) { LOG_INFO("Called createBuffers() with %zu vertices and %zu indices", vertices.size(), indices.size()); glGenVertexArrays(1, &_VAO); @@ -24,19 +24,19 @@ namespace render { glBindVertexArray(_VAO); glBindBuffer(GL_ARRAY_BUFFER, _VBO); - glBufferData(GL_ARRAY_BUFFER, vertices.size() * sizeof(scene::VertexHolder), vertices.data(), GL_STATIC_DRAW); + glBufferData(GL_ARRAY_BUFFER, vertices.size() * sizeof(assets::VertexHolder), vertices.data(), GL_STATIC_DRAW); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, _EBO); glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices.size() * sizeof(GLuint), indices.data(), GL_STATIC_DRAW); glEnableVertexAttribArray(0); - glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(scene::VertexHolder), (void*)offsetof(scene::VertexHolder, _pos)); + glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(assets::VertexHolder), (void*)offsetof(assets::VertexHolder, _pos)); glEnableVertexAttribArray(1); - glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof(scene::VertexHolder), (void*)offsetof(scene::VertexHolder, _normal)); + glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof(assets::VertexHolder), (void*)offsetof(assets::VertexHolder, _normal)); glEnableVertexAttribArray(2); - glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, sizeof(scene::VertexHolder), (void*)offsetof(scene::VertexHolder, _texCoord)); + glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, sizeof(assets::VertexHolder), (void*)offsetof(assets::VertexHolder, _texCoord)); glBindVertexArray(0); diff --git a/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp b/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp index 2306485..fc4c5cc 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotLoader.cpp @@ -6,7 +6,7 @@ #include #include #include "Scene/Object.h" -#include "Scene/MeshLoader.h" +#include "Assets/MeshLoader.h" #include "EngineLib/LogMacros.h" #include diff --git a/Sim_Engine/EngineLib/src/Scene/Mesh.cpp b/Sim_Engine/EngineLib/src/Scene/Mesh.cpp index 03dbf4a..fc98ea3 100644 --- a/Sim_Engine/EngineLib/src/Scene/Mesh.cpp +++ b/Sim_Engine/EngineLib/src/Scene/Mesh.cpp @@ -1,7 +1,7 @@ #include "pch.h" // File: Mesh.cpp // GitHub: SaltyJoss -#include "Scene/MeshLoader.h" +#include "Assets/MeshLoader.h" #include #include #include @@ -17,16 +17,14 @@ namespace scene { createBuffers(); } - // Mesh Loading + // Load mesh data from file using MeshLoader bool Mesh::load(const std::string& path) { - gui::MeshLoader loader; + assets::MeshLoader loader; loader.load(path); - if (loader.load(path).empty()) { LOG_ERROR("Mesh load failed for file: %s", path.c_str()); return false; } - return true; } diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index 0191565..eb7d183 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -7,6 +7,8 @@ #include "Robots/RobotModel.h" #include "Robots/TrajectoryManager.h" +#include "Assets/MeshLoader.h" + #include "Interpreter/StoredProgram.h" #include "Interpreter/Parser.h" @@ -34,6 +36,16 @@ namespace core { intgr->setAdaptiveTolerances(1e-3, 1e-6); intgr->setMaxStep(_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); + } + // 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->getIntegrator()->IntegratorName(_robot->getIntegrator()->getIntegrationMethod()); + } // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { @@ -367,6 +379,13 @@ namespace core { D_SUCCESS("Synchronous run completed: %s (%.1fs, %zu samples)", methodName.c_str(), _simTime, _telemetry.ring.size()); return (_telemetry.ring.size() >= 2); } + + // Setter for fixed timestep duration + void SimulationCore::setFixedDt(double dt) { _dt = dt; } + // Getter for fixed timestep duration + double SimulationCore::fixedDt() const { return _dt; } + // Getter for current simulation time + double SimulationCore::simTime() const { return _simTime; } // Method to step the simulation with a fixed timestep void SimulationCore::tick(double frame_dt) { stepFixed(frame_dt); } @@ -380,13 +399,72 @@ namespace core { physics::PhysicsSystem* SimulationCore::physicsSystem() { return _physics; } const physics::PhysicsSystem* SimulationCore::physicsSystem() const { return _physics; } + // Accessor for the robot system (non-const and const versions) + robots::RobotSystem* SimulationCore::robotSystem() { return _robot; } + const robots::RobotSystem* SimulationCore::robotSystem() const { return _robot; } + // Setter and checker for Robot System void SimulationCore::setRobotSystem(robots::RobotSystem* robot) { _robot = robot; } bool SimulationCore::hasRobot() const { return _robot && _robot->hasRobot(); } - // Accessor for the robot system (non-const and const versions) - robots::RobotSystem* SimulationCore::robotSystem() { return _robot; } - const robots::RobotSystem* SimulationCore::robotSystem() const { return _robot; } + // Loads a robot into the robot system by name + void SimulationCore::loadRobot(const std::string& name) { + if (!_robot) { D_FAIL("Cannot load robot: RobotSystem not set"); return; } + _robot->loadRobot(name); + } + // Clears the currently loaded robot from the robot system + void SimulationCore::clearRobot() { + if (!_robot) { D_FAIL("Cannot clear robot: RobotSystem not set"); return; } + _robot->clearRobot(); + } + + // Getter for the scene objects reference (used for script object lookup) + std::vector>& SimulationCore::getObjects() { + if (!_objects) { throw std::runtime_error("Scene objects pointer not set in SimulationCore"); } + return *_objects; + } + // Deletes an object from the scene by index, with bounds checking + void SimulationCore::deleteObject(int index) { + if (!_objects) { D_FAIL("Cannot delete object: Scene objects pointer not set"); return; } + if (index < 0 || index >= static_cast(_objects->size())) { + D_FAIL("Cannot delete object: Index %d out of bounds (size=%zu)", index, _objects->size()); + return; + } + _objects->erase(_objects->begin() + index); + } + // Loads a mesh from the given path, adds it to the scene objects, and returns raw pointers to the new objects for script access + std::vector SimulationCore::loadMeshReturn(const std::string& path) { + assets::MeshLoader loader; + auto meshes = loader.load(path); + std::vector result; + for (auto& m : meshes) { + auto obj = std::make_unique(m); + auto raw = obj.get(); + raw->internal = true; + _objects->push_back(std::move(obj)); + result.push_back(raw); + } + return result; + } + + // Setter for the scene objects pointer (used for script object lookup) + void SimulationCore::setObjects(std::vector>* objects) { _objects = objects; } + // Getter for object + scene::Object* SimulationCore::getObject() { + if (!_objects) { return nullptr; } + for (auto& obj : *_objects) { + if (obj) { return obj.get(); } + } + return nullptr; + } + // Getter for object by ID + scene::Object* SimulationCore::getObjectByID(scene::ObjectID id) { + if (!_objects) { return nullptr; } + for (auto& obj : *_objects) { + if (obj && obj->id == id) { return obj.get(); } + } + return nullptr; + } // Setter for the trajectory manager void SimulationCore::setTrajectoryManager(control::TrajectoryManager* traj) { _traj = traj; } @@ -395,9 +473,6 @@ namespace core { control::TrajectoryManager* SimulationCore::trajectoryManager() { return _traj; } const control::TrajectoryManager* SimulationCore::trajectoryManager() const { return _traj; } - // Setter for the scene objects pointer (used for script object lookup) - void SimulationCore::setObjects(std::vector>* objects) { _objects = objects; } - // Setters for the metric buffers void SimulationCore::setJointLogBuffer(robots::JointLogBuffer* buf) { _jointLogBuffer = *buf; } void SimulationCore::setTrajRefBuffer(robots::TrajRefBuffer* buf) { _trajRefBuffer = *buf; } @@ -406,6 +481,9 @@ namespace core { diagnostics::TelemetryRecorder& SimulationCore::telemetry() { return _telemetry; } const diagnostics::TelemetryRecorder& SimulationCore::telemetry() const { return _telemetry; } + // Get the current number of telemetry samples recorded + size_t SimulationCore::telemetrySampleCount() const { return _telemetry.ring.size(); } + // Setter and getter for the active script program void SimulationCore::setActiveProgram(interpreter::IStoredProgram* p) { _activeProgram = p; } interpreter::IStoredProgram* SimulationCore::activeProgram() const { return _activeProgram; } diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp index 1d4929a..3a12415 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp @@ -15,7 +15,7 @@ #include "Scene/Input.h" #include "Scene/Camera.h" #include "Scene/Mesh.h" -#include "Scene/MeshLoader.h" +#include "Assets/MeshLoader.h" #include "Scene/Light.h" #include "Scene/AxisOrientator.h" @@ -779,7 +779,7 @@ namespace gui { // Load a mesh from file and create one Object per submesh. The last loaded mesh becomes the active selection. void SimManager::loadMesh(const std::string& filepath) { - gui::MeshLoader loader; + assets::MeshLoader loader; auto meshes = loader.load(filepath); if (meshes.empty()) { @@ -815,17 +815,13 @@ namespace gui { // Returns the loaded objects so they can be used as targets for robot joints in the same frame (e.g. end-effector) std::vector SimManager::loadMeshReturn(const std::string& filepath) { - gui::MeshLoader loader; + assets::MeshLoader loader; auto meshes = loader.load(filepath); - std::vector result; - for (auto& m : meshes) { auto obj = std::make_unique(m); auto raw = obj.get(); - raw->internal = true; - _impl->_objects.push_back(std::move(obj)); result.push_back(raw); } @@ -1175,7 +1171,11 @@ namespace gui { interpreter::IStoredProgram* SimManager::activeProgram() { return _core->activeProgram(); } const interpreter::IStoredProgram* SimManager::activeProgram() const { return _core->activeProgram(); } - // Access the simulation core for advanced users who want to run custom programs, etc. + // Access the simulation core interface (non-const and const versions) + core::ISimulationCore* SimManager::simCoreInterface() { return _core.get(); } + const core::ISimulationCore* SimManager::simCoreInterface() const { return _core.get(); } + + // Access the concrete simulation core (non-const and const versions) core::SimulationCore* SimManager::simCore() { return _core.get(); } const core::SimulationCore* SimManager::simCore() const { return _core.get(); } @@ -1215,20 +1215,15 @@ namespace gui { // Replace the integrator method in the script text std::string modifiedScript = replaceIntegratorInScript(scriptText, methodName); - // Create program and parser - auto program = std::make_unique(this); - program->setDefaultObject(getObject()); + // Create program and parser (bound to headless core) + auto program = std::make_unique(_core.get()); + if (scene::Object* o = getObject()) program->setDefaultObject(o); auto parser = std::make_unique(program.get()); - // Clear any existing program state - program->clear(); - // Parse the modified script + // Parse the modified script and start the program parser->parse(modifiedScript); - // Start the program program->start(); - - // Run to completion - return _core->runScriptToCompletion(program.get(), method); + return _core->runScriptToCompletion(program.get(), method); // this will block until the script finishes } // -------------------------------------------------- diff --git a/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp b/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp index 7cf711e..5991a2c 100644 --- a/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp +++ b/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp @@ -133,30 +133,33 @@ namespace gui { _sim->setScriptRunning(false); } else { + // If a script is already running, stop it and clean up before starting a new one _sim->setActiveProgram(nullptr); delete _wrapper; _wrapper = nullptr; delete _parser; _parser = nullptr; delete _program; _program = nullptr; - _program = new interpreter::StoredProgram(_sim); + // Create new program, parser, and wrapper instances + _program = new interpreter::StoredProgram(_sim->simCoreInterface()); _program->setDefaultObject(_sim->getObject()); _parser = new interpreter::Parser(_program); _wrapper = new interpreter::RunWrapper(_parser, _program); + // Set the active program in the simulation manager before running _sim->setActiveProgram(_program); - _sim->setScriptRunning(true); - _sim->setLastScriptText(_scriptText); + _sim->setScriptRunning(true); + _sim->setLastScriptText(_scriptText); - // Remove trailing null character if present + // Remove trailing null character if present std::string code = _scriptText; if (!code.empty() && code.back() == '\0') code.pop_back(); _wrapper->runProgram(_scriptText); } } - + // Pop button style colors if we pushed them if (wasRunning) { ImGui::PopStyleColor(3); } - + // Check script status and handle termination conditions if (_sim->isScriptRunning()) { auto* prog = _sim->activeProgram(); if (!prog) { terminateScript("Command script stopped -> active program is null.", true); return; } diff --git a/Sim_Engine/External/MathLib/include/core/Types.h b/Sim_Engine/External/MathLib/include/core/Types.h index 29b794f..55883e0 100644 --- a/Sim_Engine/External/MathLib/include/core/Types.h +++ b/Sim_Engine/External/MathLib/include/core/Types.h @@ -1,7 +1,7 @@ #pragma once #include "MathLibAPI.h" -#include +#include "Eigen/Dense" namespace mathlib { // Basic type definitions From 19439af005c9c4295f0a44ca5065aca88b5cce9e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 22 Feb 2026 02:13:01 +0000 Subject: [PATCH 12/20] WIP --- Sim_Engine/Engine.vcxproj | 7 +- Sim_Engine/Engine.vcxproj.filters | 8 + Sim_Engine/Engine/include/BatchEntry.h | 2 + Sim_Engine/Engine/src/BatchMain.cpp | 6 +- Sim_Engine/Engine/src/Main.cpp | 5 + .../EngineLib/include/Platform/StudyRunner.h | 16 +- .../EngineLib/include/Robots/RobotDynamics.h | 4 +- .../include/ui/CommandScriptEditor.h | 3 - .../EngineLib/src/Platform/StudyRunner.cpp | 14 + .../EngineLib/src/Robots/RobotDynamics.cpp | 6 +- .../EngineLib/src/Robots/RobotSystem.cpp | 2 +- Sim_Engine/External/MathLib/Debug/MathLib.dll | Bin 724992 -> 724992 bytes .../External/MathLib/Release/MathLib.dll | Bin 61952 -> 61952 bytes .../External/MathLib/include/core/Types.h | 2 +- output | 884 ++++++++++++++++++ 15 files changed, 935 insertions(+), 24 deletions(-) create mode 100644 Sim_Engine/Engine/include/BatchEntry.h create mode 100644 output diff --git a/Sim_Engine/Engine.vcxproj b/Sim_Engine/Engine.vcxproj index 0eb4c4a..6221490 100644 --- a/Sim_Engine/Engine.vcxproj +++ b/Sim_Engine/Engine.vcxproj @@ -57,7 +57,7 @@ true _DEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;%(AdditionalIncludeDirectories) + $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) stdcpp20 $(IntDir)vc$(PlatformToolsetVersion).pdb $(IntDir)%(FileName).obj @@ -87,7 +87,7 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;%(AdditionalIncludeDirectories) + $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) stdcpp20 false $(IntDir)vc$(PlatformToolsetVersion).pdb @@ -139,6 +139,9 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" + + + diff --git a/Sim_Engine/Engine.vcxproj.filters b/Sim_Engine/Engine.vcxproj.filters index a25c818..cc54faf 100644 --- a/Sim_Engine/Engine.vcxproj.filters +++ b/Sim_Engine/Engine.vcxproj.filters @@ -18,6 +18,9 @@ {383daa6b-14af-40d3-b6df-31732d4d7a6a} + + {146d8cfd-be4d-41a3-bc59-546deb084dc0} + @@ -87,4 +90,9 @@ Shaders + + + include + + \ No newline at end of file diff --git a/Sim_Engine/Engine/include/BatchEntry.h b/Sim_Engine/Engine/include/BatchEntry.h new file mode 100644 index 0000000..8d21035 --- /dev/null +++ b/Sim_Engine/Engine/include/BatchEntry.h @@ -0,0 +1,2 @@ +#pragma once +int runBatchMode(); \ No newline at end of file diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp index abf1249..0566d7d 100644 --- a/Sim_Engine/Engine/src/BatchMain.cpp +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -1,12 +1,10 @@ // BatchMain.cpp #include "pch.h" #include +#include "include/BatchEntry.h" #include "Platform/StudyRunner.h" #include "Numerics/IntegrationMethods.h" -extern "C" core::ISimulationCore* CreateSimulationCore_v1(); -extern "C" void DestroySimulationCore(core::ISimulationCore*); - // Helper: create CorePtr (unique_ptr with std::function deleter) static CorePtr makeCoreFactory() { core::ISimulationCore* raw = CreateSimulationCore_v1(); @@ -18,6 +16,8 @@ static CorePtr makeCoreFactory() { } int runBatchMode() { + fprintf(stdout, "BATCH MODE ENTERED\n"); + fflush(stdout); try { // Define the configurations for the studies to run (combinations of integrator methods, timesteps, and run lengths) std::vector configs; diff --git a/Sim_Engine/Engine/src/Main.cpp b/Sim_Engine/Engine/src/Main.cpp index 2738276..2e87f5b 100644 --- a/Sim_Engine/Engine/src/Main.cpp +++ b/Sim_Engine/Engine/src/Main.cpp @@ -3,8 +3,13 @@ // GitHub: SaltyJoss #include #include +#include "include/BatchEntry.h" int main(int argc, char** argv) { + printf("argc = %d\n", argc); + for (int i = 0; i < argc; ++i) + printf("argv[%d] = %s\n", i, argv[i]); + if (argc > 1 && std::string(argv[1]) == "--batch") { return runBatchMode(); } diff --git a/Sim_Engine/EngineLib/include/Platform/StudyRunner.h b/Sim_Engine/EngineLib/include/Platform/StudyRunner.h index f1c1680..2fbf68c 100644 --- a/Sim_Engine/EngineLib/include/Platform/StudyRunner.h +++ b/Sim_Engine/EngineLib/include/Platform/StudyRunner.h @@ -15,17 +15,17 @@ using CorePtr = std::unique_ptr>; // Struct to hold the results of a study/research run -struct StudyResult { - bool success; - std::string tag; // user tag for this run (I am so fed up of renaming in file explorer) - std::string intName; // integration method name - double dt; - double simTime; - size_t samples; +struct ENGINE_API StudyResult { + bool success = false; // whether the run completed successfully + std::string tag = "N/A"; // user-provided tag for the run + std::string intName = "N/A"; // name of the integrator used + double dt = 0.0; + double simTime = 0.0; + size_t samples = 0; }; // StudyRunner class to manage running batches of studies in parallel -class StudyRunner { +class ENGINE_API StudyRunner { public: // Config for a run struct config { diff --git a/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h index 0c2affc..53126bf 100644 --- a/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h +++ b/Sim_Engine/EngineLib/include/Robots/RobotDynamics.h @@ -22,7 +22,7 @@ namespace robots { class ENGINE_API RobotDynamics { public: // Constructor - RobotDynamics(RobotModel& robot, eTorqueMode mode); + RobotDynamics(RobotModel& robot); // Computes the inertia tensor of a robot link mathlib::Mat3 computeLinkInertiaTensor(const RobotLink& link) const; @@ -86,7 +86,7 @@ namespace robots { private: // References and pointers RobotModel& _robot; - std::unique_ptr _kinematics; + std::unique_ptr _kinematics = nullptr; double _dt = 1.0 / 180.0; // default timestep for dynamics updates diff --git a/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h b/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h index 2e11a29..e897878 100644 --- a/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h +++ b/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h @@ -27,9 +27,6 @@ namespace gui { // Main render function void render(); - // Run/Stop button handler - void runButtonHandler(); - private: // Simulation manager reference gui::SimManager* _sim; diff --git a/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp b/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp index 1d82273..e4a43be 100644 --- a/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp +++ b/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp @@ -67,6 +67,13 @@ std::vector StudyRunner::runStudies(const std::vector& conf } else { std::this_thread::sleep_for(std::chrono::milliseconds(5)); } // Sleep briefly to avoid busy-waiting if no futures are ready } + + fprintf(stdout, + "START thread %zu | tag=%s\n", + std::hash{}(std::this_thread::get_id()), + cfg.tag.c_str()); + fflush(stdout); + // Start a new async run for this config, capturing the current config and program by value to make sure they are safely used in the async context futures.push_back(std::async(std::launch::async, [this, cfg, scriptText]() -> StudyResult { StudyResult result{}; @@ -109,6 +116,13 @@ std::vector StudyRunner::runStudies(const std::vector& conf catch (...) { result.samples = 0; } + + fprintf(stdout, + "END thread %zu | tag=%s\n", + std::hash{}(std::this_thread::get_id()), + cfg.tag.c_str()); + fflush(stdout); + // Get the number of telemetry samples collected during this run (proxy for how much data we obtained) return result; // Return the result of this study run })); diff --git a/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp index dc04a9c..c002054 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotDynamics.cpp @@ -12,7 +12,7 @@ namespace robots { // Constructor - RobotDynamics::RobotDynamics(RobotModel& robot, eTorqueMode mode) + RobotDynamics::RobotDynamics(RobotModel& robot) : _robot(robot), _kinematics(std::make_unique(robot)) { } @@ -128,8 +128,6 @@ namespace robots { // Jacobian columns for joints i and j 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 - // Mass matrix contribution from this link for joints i and j - // Mass matrix contribution from this link for joints i and j M(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; } @@ -445,7 +443,7 @@ namespace robots { // Fill the reduced mass matrix row for active joint i for (size_t c = 0; c < m; ++c) { - int j = active[c]; + size_t j = active[c]; M(r, c) = M_full(i, j); } } diff --git a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp index 5a7523b..d246f25 100644 --- a/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp +++ b/Sim_Engine/EngineLib/src/Robots/RobotSystem.cpp @@ -29,7 +29,7 @@ namespace robots { RobotSystem::RobotSystem(std::vector>& objects, spawnFn meshLoader) : _objects(objects), _loadMeshReturn(std::move(meshLoader)), _torqueMode(eTorqueMode::CONTROLLED), _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), - _kinematics(std::make_unique(_robot)), _dynamics(std::make_unique(_robot, _torqueMode)) { + _kinematics(std::make_unique(_robot)), _dynamics(std::make_unique(_robot)) { if (!_integrator) { LOG_WARN("RobotSystem got null IntegrationService*"); } } // Destructor diff --git a/Sim_Engine/External/MathLib/Debug/MathLib.dll b/Sim_Engine/External/MathLib/Debug/MathLib.dll index 66aa9158ad73e205aaead1b64ab4fc2c1427d704..0e7b21bba16f27d472ab1f6e43bdd4a18c37a0ef 100644 GIT binary patch delta 56 zcmZozpwqBGXTlHWvq`fWf4BZ-Z2isD`kT4+H%sep*4E!_t-sl~{^l^f!^qNZc$Wi+ MIky|$&f4BZ-Z2isD`kT4+H%sep*4E!_t-sl~{^l^f!^qrjc$Wi+ MIky|$$^fNT*U-T{=` qfsmVQzeo~nQ1qg&Y=V(yyLS8gE_I!6{K8^m#-`_tn-f>g_W%H#)g@H` delta 114 zcmZp8!rbtLd4m8WGk?sC&BBZy7gz%snG8U{1jIQ&?6V(;SAs+s7+`WdK(+`F?*Pi} qK*&wDUnB`OD0GGL<@^N(2p^bN0Hz%%~?*RaKZ6ZYg diff --git a/Sim_Engine/External/MathLib/include/core/Types.h b/Sim_Engine/External/MathLib/include/core/Types.h index 55883e0..29b794f 100644 --- a/Sim_Engine/External/MathLib/include/core/Types.h +++ b/Sim_Engine/External/MathLib/include/core/Types.h @@ -1,7 +1,7 @@ #pragma once #include "MathLibAPI.h" -#include "Eigen/Dense" +#include namespace mathlib { // Basic type definitions diff --git a/output b/output new file mode 100644 index 0000000..53dd1fa --- /dev/null +++ b/output @@ -0,0 +1,884 @@ +* c303050 (HEAD -> add/implement_energy_metrics) feat: added Headless core API, batch runner, and asset refactor +* 3230a4c WIP: added simulation run modes and refactor log buffer handling +* 2b0b7cc WIP: adding double-buffered joint metric logging to RobotSystem +* 60c7afc update: going to add a basic double-buffer design to help testing runs +* 2f83673 (origin/add/implement_energy_metrics) refactor: refactored robot joint state naming and add energy metrics +* b5bb0ca (origin/Dev, Dev) Refactor/code seperation (#73) +| * 1802df6 (origin/refactor/code_seperation) Merge branch 'Dev' into refactor/code_seperation +| |\ +| |/ +|/| +* | 7ff6919 Refactor of RobotSystem.cpp (#72) +* | 0cb1f90 Merge branch 'Dev' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Dev +|\ \ +* | | 29f18ab fixes +* | | c2d9e8a fixes +* | | 0b4371d fixes +| | | * 5f80878 (refactor/code_seperation) fixes +| | | * f93bee9 refactor: Introduce SceneRenderer abstraction +| | |/ +| | * 718ba5b fixes +| | * 7816a26 refactor: moved all core simulator methods, logic, variables, and pointers into SimulationCore.cpp, making SimManage focused on actually MANAGING the simulator. +| | * 3793fac Merge branch 'refactor/code_seperation' of https://github.com/SaltyJoss/RoboticArm_MathModelling into refactor/code_seperation +| | |\ +| | | * 84576d5 Merge branch 'refactor/code_seperation' of https://github.com/SaltyJoss/RoboticArm_MathModelling into refactor/code_seperation +| | | |\ +| | * | \ 15f0f69 Merge branch 'refactor/code_seperation' of https://github.com/SaltyJoss/RoboticArm_MathModelling into refactor/code_seperation +| | |\ \ \ +| | | |/ / +| | |/| / +| | | |/ +| | | * 7e21f60 fixes: fixed previous files to correctly function with new abstraction +| | * | 8f8190b fixes: fixed previous files to correctly function with new abstraction +| | |/ +| | * 9ba9dfd feat: move robot kinematic and dynamic logic to seperate headers and compiler files respectively +| |/ +| * 36d193f Merge pull request #70 from SaltyJoss/SaltyJoss-patch-1 +| |\ +| | * 170e339 Revise Todo List format and item status +| * | b1316c7 Merge pull request #69 from SaltyJoss/Main +| |\| +| * | 6d2b444 Merge pull request #68 from SaltyJoss/add/full_rigid-body_dynamics +|/| | +| | | * 3d93f94 (origin/Main, origin/HEAD) Revise Todo List format and item status (#71) +| | |/ +| | * 8374665 (tag: v0.7.0r-alpha) Merge pull request #67 from SaltyJoss/add/full_rigid-body_dynamics +| | |\ +| | |/ +| |/| +| * | aaab286 feat: improve robot inertia, control, and UI dt controls +| * | 80d8e2b (tag: v0.6.3r-alpha) feat: added setOmega command for direct joint velocity injection +| * | 942c8be feat: added explicit handling for eTorqueMode in RobotSystem.cpp, improving validity for integration study +| * | 9ae344e fixes: physically fine, just not the best for the numerical integration test. +| * | 9d801a2 fixes: fixed signularity in mass matrix +| * | b1dce4a NOTE: realised I need to sort my mapping of joints so world joints are GENUINELY IGNORED. +| * | c320208 fixes +| * | 187127b fixes +| * | cb66966 feat: add torque mode toggle into control panel +| * | 28fb03f update +| * | 94f3a0a refactor: moved from diagonal inertia and coriolis (using finite difference) rigid-body model to a more numerically transparent +|/ / +| * c8647c0 Revise project details and roadmap in README (#66) +| * 959658b (tag: v0.6.0r-alpha) fixes: telemetry results page fixes, improving first size, first position, and general pipeline (#65) +| * 371d2b3 Merge pull request #64 from SaltyJoss/Dev +| |\ +| * \ 336074a (tag: v0.5.3-alpha) Merge pull request #63 from SaltyJoss/Dev +| |\ \ +| * \ \ a9b79e7 (tag: v0.5.2-alpha) Merge pull request #62 from SaltyJoss/Dev +| |\ \ \ +| | | | | * be31e8f (fixes/improve_diagonal_use) feat: removed integral gain, torque saturation, and omega barrier +| |_|_|_|/ +|/| | | | +* | | | | e1b82c9 (origin/fixes/improve_diagonal_use) fixes +* | | | | 71f1dab fixes: telemetry results page fixes, improving first size, first position, and general pipeline +| |_|_|/ +|/| | | +* | | | 0a4c0f5 refactor: improved sim settings and UI dt controls, added gravity param +| |_|/ +|/| | +* | | 8da1fa8 fixes: buffer-based logging for robot sim and trajectory refs +| |/ +|/| +* | b7ff992 readme fixes +|\| +| * a0db7fd (tag: v0.5.1-alpha) merge(dev): integrate rendering updates, robotics models, and integrator fixes into main (#61) +| * cde6fb1 Add H1 robotic system and VISPA model references (#58) +* | 0849727 fixes: shader path changes corrected to suit the current program state +* | 1004932 merge fixes: simplify filters, support multiple attached objects +|\| +| * 24c4f71 (tag: v0.5.0-alpha, Main) Graphical and UI improvements / Comparitive analysis plots added (#57) +* | 24a3c9c feat: added support for free-floating (floating base) robots +* | e8fe657 Merge pull request #60 from SaltyJoss/ui/tweak-plots-and-general-ui +|\ \ +| * | 859d097 fixes: DSFE is no longer capped at 16:9 aspect ratio, but upon first loading it will automatically size to 16:9 +| * | f1018bd refactor: added reusable ImGui widgets and a generally improve layout for GUI +| * | 66f3db3 docs +|/ / +* | 4aa6083 Merge pull request #59 from SaltyJoss/fix/rk45-method-corrections +|\ \ +| * | e15bc11 fixes: rk45 integration method implementation corrected to produce realstic and accurate results, improving numerical transparency. +|/ / +* | 5353d03 Merge origin/Main into Dev: resolve all conflicts keeping Dev +|\| +| * 2176511 Add robotic models section to README (#56) +| * c4e6244 General house keeping improvements (#55) +| * a6b3201 (tag: v0.4.0-alpha) Fixes/fix robotic arm model transforms + implementation of new robotic models (#54) +* | 2e88249 feat: added integrator comparison and CSV export to GUI +* | ade0c3b feat: addition of H1 and VISPA robot models, along with partial implementation of individual run plots +* | 6deaeaa feat: added checkbox to allow realtime update for plots +* | 999eec4 fixes: added colour to existing robotic models +* | 5e17306 fixes: enforce 16:9 window, improve camera and viewport UX +* | 2103964 fixes: fixed quad view follow target resetting to nothing on a new model following the currently loaded model +* | 7b2eacb fixes: improve material appearence on differerent meshes, along with improved lighting +* | 4f3047f refactor: improve PBR ambient control and quad view rendering, and add ssao rendering +* | 56f71a0 refactor: move UI code to ui/ namespace, cleanup & docs +* | 14446d6 fixes: removal of redundancy +| | * e1fe60d (origin/feature/h1-addition) feat: basic additions to allows for simple mvement +| | * adfaaed feat: addition of H1 humanoid robot +| |/ +|/| +* | 0e490a1 Fix Markdown formatting in README.md +* | 4993123 Document robotic models used in the project +* | d26d53b Fixes/fix robotic arm model transforms (#53) +* | 12c7f2f Merge remote-tracking branch 'origin/Main' into Dev +|\| +| * 34149c5 fixes: release build pipeline and assimp toolset mismatch (#51) +| * acc9624 (tag: v0.3.0-alpha) Merge branch 'Main' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Main +| |\ +| | * acc6554 Dev (#49) +| * | e3692ba merge fixes +| * | a1d9995 merge fixes +| * | 5c7a152 merge fixes +| * | 79de91f merge fixes +| * | ed60ac4 fixes +| * | b61465b fixes +| * | a0485a0 feat: added a very basic selector for gravity values +| * | 5c0c0c7 fixes: Inverse dynamics style feedforward corrected to be inline with the physically correct model. +| * | ad9ba1d fixes: fixed gravity inconsistenties with robot root frame, and recomputation of FK +| * | 8627599 fixes: fixed computational overestimation of wrist gravity in ComputeGravityTorque() +| * | ce86dca feat: implemented basic computation of a joints gravity torque, +| * | ad45a8a Refactor/robot metrics (#46) +| |/ +* | 42e95c8 Merge branch 'Dev' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Dev +|\ \ +| * | 26147e4 Remove latest update section from README +| * | 09aa1f9 Remove libraries used section from README +| * | c0e9e7a Revise disclaimer in README for clarity +| * | f80547c Update README with installation and tools information +* | | 1e9a047 fixes to robot loading +* | | c179e87 feat: refactor robotsystem and loading to allow other robotic models. +|/ / +* | 5f3d86b Merge remote-tracking branch 'origin/Main' into Dev +|\| +| * c51d94c (tag: v0.2.0-alpha) Coriolis addition, Code cleanup and refactoring (#45) +* | 76b3b3d Merge branch 'Main' into Dev +|\| +| * 317591c Update Main with dev changes (#43) +* | 4f7ad1a merge fixes +* | bdc6042 merge fixes +* | 4bb88a0 merge fixes +* | 447a609 merge fixes +* | 7364477 fixes +* | 38b0607 fixes +* | e4f5640 feat: added a very basic selector for gravity values +* | e2dc073 fixes: Inverse dynamics style feedforward corrected to be inline with the physically correct model. +* | fe40d9f fixes: fixed gravity inconsistenties with robot root frame, and recomputation of FK +* | cfa2450 fixes: fixed computational overestimation of wrist gravity in ComputeGravityTorque() +* | 56497ec feat: implemented basic computation of a joints gravity torque, +* | 343f616 Refactor/robot metrics (#46) +* | c57fdd0 Feature/joint friction remodel (#44) +* | 04b7326 Merge branch 'Main' into Dev +|\| +| * 0433771 alpha release v0.1.0 (#40) +| * f546692 Update author information and release date (#41) +| * 4a14b7b (tag: v0.1.0-alpha) Update citation link in README.md +| * de4efa6 Merge pull request #39 from SaltyJoss/Dev +| |\ +* | \ 1656caa Merge branch 'Dev' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Dev +|\ \ \ +| * | | 1c8595f Remove badge section from README.md +| * | | 0ca78ed update dev inline with Main branch (#42) +* | | | bde36df fixes +|/ / / +* | | 08c8a14 fixes: correct added postbuild copmmand for HDF5 dlls, along with 3rd party licenses in new release +* | | 8e7d9f5 fixes +* | | 0408409 fixes: plots now optionally follow, and can be scaled according to user choice +* | | 8d76244 Merge branch 'Dev' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Dev +|\ \ \ +| | |/ +| |/| +| * | 60788f9 Merge pull request #38 from SaltyJoss/Main +| |\| +| | * 1320a9f Enhance installation script with exit code checks +| * | 6f4a150 Update citation and author links in README.md +| * | 02612e2 Merge pull request #37 from SaltyJoss/Main +| |\| +| | * d2c96f4 feat: Citation Addition +| | * c165864 feat: addition to README +| | * eee442f Merge pull request #36 from SaltyJoss/Dev +| | |\ +| | |/ +| |/| +| | * 3727f40 Update build.yml +| | * 1299017 Change Visual Studio build tools to 2026 +| | * 4512f0b Update build.yml to use Visual Studio 2017 components +| | * dec5540 Update build.yml +| | * 6231a8d Add step to install Visual Studio 2026 build tools +| | * bbacb97 Specify PlatformToolset v145 in build.yml +| | * b320349 Add build step for MathLib solution +| | * 2950243 Update README.md +| | * 4122bad Update build path for msbuild in workflow +| | * eb04fd6 Revise README for new robotic arm integration details +| | * bcd49fb Merge branch 'Main' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Main +| | |\ +| | | * fa85ece Merge pull request #35 from SaltyJoss/Dev +| | | |\ +| | | * | 30e7578 Clean up outdated robotic arm script entries in README +| | | * | 43c13bf Revise README with enhanced robotic arm project notes +| | | * | 33113db Update README with robotic arm script details +| | | * | ff8eaa2 Merge pull request #34 from SaltyJoss/Dev +| | | |\ \ +| | | * \ \ f715d60 Merge pull request #33 from SaltyJoss/Dev +| | | |\ \ \ +| | | * \ \ \ fae930f Merge pull request #32 from SaltyJoss/Dev +| | | |\ \ \ \ +| | | * \ \ \ \ 743dd60 Merge pull request #31 from SaltyJoss/Dev +| | | |\ \ \ \ \ +| | | * \ \ \ \ \ cccfd0d Merge pull request #30 from SaltyJoss/Dev +| | | |\ \ \ \ \ \ +| | | * | | | | | | 75a653f Delete Sim_Engine/Runs directory +| | | * | | | | | | 9ed43a5 Merge pull request #29 from SaltyJoss/Dev +| | | |\ \ \ \ \ \ \ +| | | * \ \ \ \ \ \ \ a34128e Merge pull request #28 from SaltyJoss/Dev +| | | |\ \ \ \ \ \ \ \ +| | * | | | | | | | | | 9f14945 refactor +| | |/ / / / / / / / / +| | * | | | | | | | | bbb62b8 docs +| | * | | | | | | | | 04220c0 Update README with new robotic arm script example +| | * | | | | | | | | 92624af Merge pull request #27 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ e800994 Merge pull request #26 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ +| | * | | | | | | | | | | 09d1a7b Update README with new robotic arm scripts and links +| | * | | | | | | | | | | f586263 Merge pull request #25 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ +| | * | | | | | | | | | | | 5a84bac Update README with new robotic arm scripts +| | * | | | | | | | | | | | 05680e8 Merge pull request #24 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ +| | * | | | | | | | | | | | | be6482c Update README with robotic arm script information +| | * | | | | | | | | | | | | b9ef50e Merge pull request #23 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ +| | * | | | | | | | | | | | | | aae72ec Revise README with updated Robotic Arm script examples +| | * | | | | | | | | | | | | | e461605 Merge pull request #22 from SaltyJoss/Dev - Mathematics and Physics fixes +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ 81f8d9c Merge pull request #21 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 6190936 Merge pull request #20 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * | | | | | | | | | | | | | | | | 3b58ede Update README with examples of robotic arm scripts +| | * | | | | | | | | | | | | | | | | b3c54a9 Rename project to Dynamic Systems Framework +| | * | | | | | | | | | | | | | | | | 5fcb09f Merge pull request #11 from SaltyJoss/Dev -> POC it works! +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 2e5202b Merge pull request #10 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 447f7d8 Merge pull request #9 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ e84ff11 Merge pull request #8 from SaltyJoss/Dev - DSL update +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * | | | | | | | | | | | | | | | | | | | | 874da82 Update README with new examples and formatting +| | * | | | | | | | | | | | | | | | | | | | | 7d52a52 Add examples of new scripting language to README +| | * | | | | | | | | | | | | | | | | | | | | 697cd85 Merge pull request #7 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 668efb7 Merge pull request #6 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * | | | | | | | | | | | | | | | | | | | | | | b2a2615 Update README for formatting consistency +| | * | | | | | | | | | | | | | | | | | | | | | | 0d48f6c Update project status image and caption +| | * | | | | | | | | | | | | | | | | | | | | | | 0115ecd Add current state image for project +| | * | | | | | | | | | | | | | | | | | | | | | | 79e9583 Update current state image in README +| | * | | | | | | | | | | | | | | | | | | | | | | 2aa077b Update README with current project state +| | * | | | | | | | | | | | | | | | | | | | | | | 0a428b1 Merge pull request #5 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 0eaf151 Merge pull request #4 from SaltyJoss/Dev ~ WIP adding SCL language and interpreter to application +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 897187d Merge pull request to Main from Dev, PoC +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ cf7652f Visual overhaul, graphics settings, GUI additions, basic code fixes +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +| | * \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ c98b249 Merge pull request #1 from SaltyJoss/Dev +| | |\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +* | | | | | | | | | | | | | | | | | | | | | | | | | | | | | 80e2955 refactor: fixed and refactored path, plots, and small QoL bugs in preperation for release (first version) +|/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / +* | | | | | | | | | | | | | | | | | | | | | | | | | | | / 9976b90 fixes +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | | | | | | | | edea558 fixes +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | | | | | | | f503ce9 fixes +* | | | | | | | | | | | | | | | | | | | | | | | | | | 5e7e78f fixes: general fixes with bad implementation of pointers +* | | | | | | | | | | | | | | | | | | | | | | | | | | 231ff71 feat: added Integral Gain to existing PD controller in robot system (PID now) +* | | | | | | | | | | | | | | | | | | | | | | | | | | 8493b25 feat: added Integral Gain to existing PD controller in robot system (PID now) +* | | | | | | | | | | | | | | | | | | | | | | | | | | 68540e2 fixes: rerun error, bad pointer allocation / destruction +* | | | | | | | | | | | | | | | | | | | | | | | | | | 731bc6e fixes: fixed memory leak in GUIContext, ImPlot was initialised in the incorrect method. +* | | | | | | | | | | | | | | | | | | | | | | | | | | d85621c docs +* | | | | | | | | | | | | | | | | | | | | | | | | | | 31f3a49 removal of docs folder +* | | | | | | | | | | | | | | | | | | | | | | | | | | ccdd524 fixes +* | | | | | | | | | | | | | | | | | | | | | | | | | | aa1a539 feat: added integral gain into robotic arm controller, along with better clamping of omega and theta for more accurate simulation data +* | | | | | | | | | | | | | | | | | | | | | | | | | | c7c445c fixes +* | | | | | | | | | | | | | | | | | | | | | | | | | | 82cf1cb fixes +* | | | | | | | | | | | | | | | | | | | | | | | | | | 5e47b2a fixes; dynamics updates +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | | | | | | f18d932 fixes +* | | | | | | | | | | | | | | | | | | | | | | | | | 8ce5431 fixes +* | | | | | | | | | | | | | | | | | | | | | | | | | 717bed7 fixes +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | | | | | 7a43e2f fixes +* | | | | | | | | | | | | | | | | | | | | | | | | acee774 docs +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | | | | c39d93f feat: add ImPlot plotting for robotic arm telemetry plots +* | | | | | | | | | | | | | | | | | | | | | | | e6b8940 fixes +* | | | | | | | | | | | | | | | | | | | | | | | 42af208 fixes +* | | | | | | | | | | | | | | | | | | | | | | | f970a27 fixes: telemetry output fixes +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | | | 3ee2d45 fixes +* | | | | | | | | | | | | | | | | | | | | | | 5378430 feat: camera following corrrection +* | | | | | | | | | | | | | | | | | | | | | | 1d7e481 fixes: additiion of mipmap into rendering +* | | | | | | | | | | | | | | | | | | | | | | 917906d fixes +* | | | | | | | | | | | | | | | | | | | | | | f356ba0 feat: add basic joint telemetry for simulation runs into control panel +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | | 793100d fixes +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | | 26f5c95 fixes +* | | | | | | | | | | | | | | | | | | | | dedb6e7 feat: improve GUI, single and quad views +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | | 07a7593 fixes +* | | | | | | | | | | | | | | | | | | | 9fdd376 feat: add RK45 adaptive integration path, visually and numerically a selectable integration method for simulations +* | | | | | | | | | | | | | | | | | | | a3e1578 fixes: fixed reference step -> apply trajectory outputs directly directly as ground truth +* | | | | | | | | | | | | | | | | | | | 1f483fb fixes: parallel command continuation error fix +* | | | | | | | | | | | | | | | | | | | d86c7ca fixes +* | | | | | | | | | | | | | | | | | | | 5f7364d docs +* | | | | | | | | | | | | | | | | | | | 05194a6 fixes: QoL fixes +* | | | | | | | | | | | | | | | | | | | fec8f36 fixes: updates trajectory usage in derivative, correcting omega,theta,alpha reference states +* | | | | | | | | | | | | | | | | | | | bace059 fixes +* | | | | | | | | | | | | | | | | | | | 67af883 docs +* | | | | | | | | | | | | | | | | | | | 679f6dd feat: added HDF5 as external library +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | | f8ec1b8 docs +* | | | | | | | | | | | | | | | | | | fc1ab9d refactor: refactored joint control: dynamic gains and soft velocity limit +* | | | | | | | | | | | | | | | | | | 9926eaa feat: added simulation and reference data capture for HDF5 files (CODE FOR THIS IS ADAPTED FROM MULTIPLE RESOURCES, NOT REALLY ORIGINAL IN ANY SENSE) +* | | | | | | | | | | | | | | | | | | b979df2 feat: added simulation and reference data capture for HDF5 files (CODE FOR THIS IS ADAPTED FROM MULTIPLE RESOURCES, NOT REALLY ORIGINAL IN ANY SENSE) +* | | | | | | | | | | | | | | | | | | 3db8897 feat: added HDF5 dependancy +* | | | | | | | | | | | | | | | | | | 64f47dc fixes +* | | | | | | | | | | | | | | | | | | dc10292 feat: added ref logic and dataoutput +* | | | | | | | | | | | | | | | | | | 7e6ef1b docs +* | | | | | | | | | | | | | | | | | | 90bb472 fixes: ref model added, not implemented in sim step +* | | | | | | | | | | | | | | | | | | 642482d docs +* | | | | | | | | | | | | | | | | | | e3ae288 fixes +* | | | | | | | | | | | | | | | | | | 23cf41d runs +* | | | | | | | | | | | | | | | | | | 084c285 feat: basic CSV logging using new macro +* | | | | | | | | | | | | | | | | | | f0e2465 feat: basic csv writer for data capture of simulation model +* | | | | | | | | | | | | | | | | | | f80d0b2 feat: added correct Effective Inertia for physically correct dynamic simulation (no gravity yet, im planning that right now) +* | | | | | | | | | | | | | | | | | | 00d48f9 fixes: trajectory functionality fixes +* | | | | | | | | | | | | | | | | | | d1adde3 docs +* | | | | | | | | | | | | | | | | | | d6c21dd fixes: fixed runtime errors with commands +* | | | | | | | | | | | | | | | | | | 865f4f6 fixes +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | | 727efb9 docs +* | | | | | | | | | | | | | | | | | f7966ea WIP: fixes in command parsing of trajectory commands +* | | | | | | | | | | | | | | | | | ec7c258 feat(WIP): add joint trajectory framework with reference tracking +* | | | | | | | | | | | | | | | | | 8bfa37f feat: added 3 basic trajectory models into mathlib +* | | | | | | | | | | | | | | | | | 2bf7948 fixes +* | | | | | | | | | | | | | | | | | dc2ea7e feat: basic tuning to current timeout and controller use in joint commands +* | | | | | | | | | | | | | | | | | ad6d564 fixes: refactor and fix rotateJointBy to correct utilise the PD controller, depending on pos error and vecloity to control the action +* | | | | | | | | | | | | | | | | | 9b57610 docs +* | | | | | | | | | | | | | | | | | ab1fbcd fixes +* | | | | | | | | | | | | | | | | | bab7320 feat: added basic simulation log to seperate simulation run logs from general program logs +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | | 1999660 docs +* | | | | | | | | | | | | | | | | df77678 fixes: separated parse-time data from runtime commands +* | | | | | | | | | | | | | | | | 17ade27 feat(WIP): added basic parallel command execution blocks +| |_|_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | | +* | | | | | | | | | | | | | | | 7c32748 docs +* | | | | | | | | | | | | | | | 9b04fa3 fixes +* | | | | | | | | | | | | | | | e2c0a19 wip: NEW URDF and CORRECT FK with integration methods work! simulator reacts correctly using PD, and controller just needs refining & tuning (not sure which way I should do that) +* | | | | | | | | | | | | | | | 330947e wip: correcting DSL to correctly interpret and output new logic onto robotic arm, creating consistent and coherent simulation environmetn +* | | | | | | | | | | | | | | | 7d4a7c6 feat: update RobotSystem to utilise new joint and lnk params +* | | | | | | | | | | | | | | | f722940 docs +| |_|_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | | +* | | | | | | | | | | | | | | c08b501 fixes: improved robotic arm manual use, and state statistics for all current objects +* | | | | | | | | | | | | | | 54627d3 refactor: refactor robotLoader and RobotModel to correctly parse and log new parameters from JSON. +* | | | | | | | | | | | | | | d2fa98d refactor: update Z1 JSON to represent URDF better, add collision, inertial, visual! +* | | | | | | | | | | | | | | 0414f68 fixes +* | | | | | | | | | | | | | | 69b84b7 fixes: update FK in MathLib to contain URDF-style FK methods +* | | | | | | | | | | | | | | 1e0eba3 fixes: removed DH params currently (Current robot has NO trace of them on any documentation), moving to full URDF style, refactored with Z1 exact documentation +* | | | | | | | | | | | | | | 5548eb6 wip: still having issues with DH params mixing into visual world params. +* | | | | | | | | | | | | | | 730720a wip: RoboticSystem brief fixes for running, IT RUNS +* | | | | | | | | | | | | | | 01fbff7 fixes +* | | | | | | | | | | | | | | f577338 fixes: renaming fixes, will most likely have a few of thsese, indecisive and tired! +* | | | | | | | | | | | | | | a420a3e fixes +* | | | | | | | | | | | | | | beae2e5 wip: loader refactor to align with new logic (NEEDS TESTING) +* | | | | | | | | | | | | | | 92dc141 wip: refactor robotModel to match URDF and new JSON +* | | | | | | | | | | | | | | 2695758 wip: refactor Z1 json to match Z1 URDF +* | | | | | | | | | | | | | | 324365f wip: Issue with correct parameters of robotic arm, need to refactor JSON, and loading to correct prexisting issues +* | | | | | | | | | | | | | | 190065f fixes +* | | | | | | | | | | | | | | c4720ca refactor: changed quat in Z1.json to mesh alignment, not rotation. +* | | | | | | | | | | | | | | b10126d WIP: Integrate robot joint ODE state with IntegrationService +* | | | | | | | | | | | | | | 08218f2 docs +* | | | | | | | | | | | | | | c5601cb fixes +* | | | | | | | | | | | | | | 2dbef21 feat: new commands for simulation logging, and function +* | | | | | | | | | | | | | | 24fb8fc fixes +* | | | | | | | | | | | | | | 59fc781 feat: added basic dt and omega set commands, along with base for consistent sim running, data collection, and visualisation +* | | | | | | | | | | | | | | 8c5fdcb wip +* | | | | | | | | | | | | | | da187dc fixes: PBR correction, no longer harsh lighting +* | | | | | | | | | | | | | | 6654bad feat: correct graphical scaling +* | | | | | | | | | | | | | | f4b356e fixes: worldgrid "fixes" +* | | | | | | | | | | | | | | 323bda4 fixes: basic shader fixes +* | | | | | | | | | | | | | | c1d7519 docs +* | | | | | | | | | | | | | | a15591b fixes +* | | | | | | | | | | | | | | c9a0aa2 fixes +| |_|_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | | +* | | | | | | | | | | | | | b2908a9 refactor robotics architecture, decouple SimulationManager, and reorganise DSL assets +* | | | | | | | | | | | | | a8f87ca docs +| |_|_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | | +* | | | | | | | | | | | | b293165 fixes: improved location of angle fix (align), added new methods to prepare for reset ability +* | | | | | | | | | | | | 9a389ee fixes +* | | | | | | | | | | | | 86464a3 fixes +* | | | | | | | | | | | | d443907 fixes: visual scaling sorted +* | | | | | | | | | | | | acce287 fixes: world grid scale, 0.1 (10cm) to relate scale correct so all objects / robots can be correct scaled +* | | | | | | | | | | | | a3c0869 feat: new script +* | | | | | | | | | | | | 321bccd fixes: scaling in robotic arm FK, and use of quat in robotic arm loader -> robot model +* | | | | | | | | | | | | 4e1d603 fixes: naming correction for max speed +* | | | | | | | | | | | | a61bd25 fixes: tweak Z1 json to contain correct joint limits based Unitree's own Z1 specification +* | | | | | | | | | | | | ac3daf1 docs +| |_|_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | | +* | | | | | | | | | | | 637f931 feat: add joint-based rotation commands and robotics DSL demo +| |_|_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | | +* | | | | | | | | | | c3a1286 fixes: set integrator, colour command +* | | | | | | | | | | 93a21ef wip: robotic script +* | | | | | | | | | | 6a3ee10 docs +| |_|_|_|_|_|_|_|_|/ +|/| | | | | | | | | +* | | | | | | | | | af3ee92 feat: ADDED QUATERNIONS, WORKING! +* | | | | | | | | | cf6f864 docs: Initial commit to change from Euler Angles to quaternions - necessary +* | | | | | | | | | 28c9b0f fixes +| |_|_|_|_|_|_|_|/ +|/| | | | | | | | +* | | | | | | | | 8215f5b fixes: added correct termination of a script run, loading functionality, and QoL changed. +* | | | | | | | | 7714510 fixes: fixed inheritance issues, nearly done with UI based stuff - onto the fun part! +* | | | | | | | | 05c220b WIP: QoL fixes for Panels, additional fixes for Script Environment Open/Save/SaveAs functionality +* | | | | | | | | c08e908 feat: new scripts to test +* | | | | | | | | 6439b7b fixes: CommandFactory correction to creator function, and logic inside create() method +* | | | | | | | | 6c53e5b fixes: correction to DSL language instructions, updated to new syntax (still subject to change) +* | | | | | | | | 2ae4f3b fixes: corrected logic in rotate command to align with new syntax +* | | | | | | | | d822d72 refactor: corrections made to parser methods for improve tokenisation and classification of a script +* | | | | | | | | 80d749a fixes: significant fixes to existing logic for new syntax: +* | | | | | | | | 376fca2 WIP: part way through fixing and refactoring langauge syntax +* | | | | | | | | 4f598c9 feat: refactor parser to suit new syntax +* | | | | | | | | aac2b96 wip: add logic for new commands +* | | | | | | | | 69a8053 feat: add new commands for scripting language +* | | | | | | | | 0789aa3 feat: add UIContext for basic object/robot mesh visual logic for scripting language +* | | | | | | | | 648e264 fixes: basic FK working +| |_|_|_|_|_|_|/ +|/| | | | | | | +* | | | | | | | 5842dff docs: added small example videos for readme +* | | | | | | | c406004 fixes +| |_|_|_|_|_|/ +|/| | | | | | +* | | | | | | 41c66fe WIP: add frame-based execution, proper stop semantics, and correct angular units +* | | | | | | 1a6cebb feat: first PoC, rotating cube along Z-axis (doesnt stop) +* | | | | | | c9228a9 wip: language intergrator semi functional with engine +* | | | | | | f3e5d82 fixes: small refactor of command and storedprogram classes +| |_|_|_|_|/ +|/| | | | | +* | | | | | def29d3 fixes: re-added accidental end catcher in setRoboticLinkRotation +| |_|_|_|/ +|/| | | | +* | | | | 926e86a fixes +* | | | | 4bd3585 feat: add basic instructions and formatting to syntax tab +* | | | | 1d88c27 fixes: remove old fopen method use, replaced by fopen_s method in CommandScriptEditor +* | | | | e9d94c0 fixes: fix memory allocation, types, and other small code bugs for CommandScriptEditor +* | | | | b0fd010 wip: Interpreter logic corrections, and method definitions for testing +* | | | | f1c3bea wip: add Command Script Editor into existing simulation +* | | | | 666bd84 wip: addition of a single command - "ROTATE" - with basic logic. +* | | | | 4153ae2 wip: add logic in commandcontext for all commands, testing different methods to see which works best with me +* | | | | 3718f0d feat: add basic functionality to all classes +* | | | | a5edb2b fixes +* | | | | 2b7b890 fixes +* | | | | 57ddc5f refactor: research and refactor exsiting headers for simple simulator language +* | | | | e40145d fixes: add parser foundation +* | | | | d97626c WIP: add basic language functionality +* | | | | 6d2894f wip: add basic language with interpreter for simulation code loading and saving. WIP +| |_|_|/ +|/| | | +* | | | d4d40c2 feat: PoC for comparisons using reference model +* | | | 9368878 fixes: add selectable lines in debugger, removed broken logic +* | | | 975e23e wip: integration of a reference model that outputs existing results for comparison +* | | | 0c5955b fixes: RK45 adaptive step logic and error handling corrections +* | | | 957ea34 fixes: fixed lack of numerical definition for double in rk45step method +* | | | 7a41df9 wip: add reference intergration method for error analysis, acting as a setpoint to compare other methods too +* | | | fe3eabc refactor: MathLib corrected to work inside current solution, MSE added, RMSE changed to sqrt of MSE, no repeated logic +| |_|/ +|/| | +* | | 0629de8 fixes +* | | 7b5ec6a refactor: moved menu bar from control panel into the dockign window (main window) +* | | e3abe58 feat: added main taskbar +* | | e7ca8a9 fixes +* | | f18b45c WIP: graphical simulator settings +* | | 7121511 feat: add fxaa, aiming to add msaa difference +* | | e875936 fixes: fix cut issue with msaa addition +* | | ae0246e fixes +* | | 9a7c6cc WIP: add MSAA to simulator, continue fixing graphical errors and bad code logic +* | | 228a44f feat: Add basic Post-Processing and WorldGrid processing, HUGE IMPROVEMENT GRAPHICALLY +* | | 074f8b0 fixes: remove checkered floor, work on improving current pbr for new grid logic, and object lighting +* | | aa9dff4 WIP: shader fixes for simulation space +* | | 74659f8 feat: refactor and add new lighting to simulation WIP +* | | 4dcb0c1 feat: add graphic settings foundation +* | | 40dc466 refactor: Fix a few parts of WorldGrid, needs tweaking +* | | f8b9a69 re-enable worldgrid (refer back if next commit breaks the program) +* | | 72509b6 fixes +* | | e8467bd WIP: CMake foundations +| |/ +|/| +* | a181c8c fixes: reverted updateKineamtics back to pre-existing logic for correct loading. +* | ab776a7 WIP: Introduction or correct Forward Kineamtics for the robotics arm fuinctionality. Plan to build upon this +|/ +* f06aa9e fixes +* a22f3e3 feat(ControlPanel): added basic diagnostics functionality. +* 7134c9b fixes: diagnotistic runs display correct time instead of constantly running +* 9019775 general fixes +* 3ae2584 WIP fixes +* fb762e0 fixes +* 78bb3fd feat(MathLib): add control methods for eval and error analysis +* 777cd36 fixes +* 7808f7a fixes +* d050853 feat(MathLib): add basic dynamic methods for mathLib +* 14d04cb feat(MathLib): add basic kineamtic methods to mathLib kinematics. +* 8649cc3 fixes +* f9a2a71 fixes(General): update code comments and naming styles +* e3dafad fixes +* 5edcb8e feat(GUI): add orientation "gizmo" to SceneView rendering pipeline +* 19b59a8 fixes +* 944693a feat(Physics): improved the `PhysicsSystem` with integration diagnostics +* c2da00e fixes: update EngineLib to add new integration methods +* 8cfecc5 feat(MathLib): add new integration and analysis methods to improve understanding and availablility for testing. +* 18c131d fixes +* bf65589 feat(Physics): integrated the numerical methods, add damping coefficient, improved derivative usage in updateRotation() +* b0cd9da fixes: Improved ControlPanel UI +* b0b874c refactor(PhysicsSystem): refactor `eIntegrationMethod` enum to be publicly accessible, enabling dynamic integration method selection in the GUI. Replaced `_method` with a public `method` variable in `PhysicsSystem` and updated default timestep to 120 FPS. Enhanced `integrationMethod` to support RK2/RK4 with derivative functions. +* 6ffcfc0 feat(General): add joint limits, selection tracking, and logging updates +* f9bf93b fixes(ControlPanel): move scene tables (Objects etc.) into a single table +* a7608f3 - General code cleanup +* 41f926a feat(ControlPanel): improve the UI for the ControlPanel +* 2e470c3 feat(SceneView): add `setRobotLinkRotation` in `SceneView` to update robot link angles. +* 89b4276 refactor(PhysicSystem): refactor the physics system to support dynamic integration method selection +* eb61f25 refactor(General): enhance logging, simulation, and documentation +* a8827b2 feat(Control Panel): Added robot link table +* 9be0ff1 fixes +* 5df52dc fixes: +* a5cbda3 fixes +* 18cdf14 fixes +* dd23a25 Refactor: Introduce reloadable shader system and expand rendering pipeline (debugged with GPT) +* 052a435 fixes(Robot): Fix robot orientation: apply Z-up → Y-up conversion at base transform +* e3809d4 feat(ROBOT): Add RobotLoader pipeline with JSON robot model reader and mesh loading +* 8bab1fc fixes(State): Fixed objects scale rotation and position +* 27ca639 feat(Mesh): Refactor MeshLoader to support multi-mesh import with local transforms +* 588f75d feat(Mesh): Refactor MeshLoader to support multi-mesh import with local transforms +* 8d1ca40 feat(Mesh): Refactor MeshLoader to support multi-mesh import with local transforms +* 46a3ce1 refactory(Meshes): Overhauled the mesh handling system to separate geometry loading, scene transforms, and rendering responsibilities. +* 0308228 fixes: QOL fixes, UI fixes. +* f5babd4 refactor(Obj): Created collection of objects instead of only allowing one object, loading objects pushes to collection. +* 5e482da feat(Phys): Updated PhysicsSystems to correctly calculate rotation within -360 and 360 degree range, using MathLib constants and EulerStep +* 149aa34 feat(Physics): Added basic camera tracking of the rigid-body object +* 5d2d3ca feat(Physics): Added user input to objects linear and angular velocities, the objects mass, and the a reset button for all object state. +* 5dbaa22 feat(Physics): Added new variables, matricies and vectors to PhysicsState +* 52662a1 feat(PhysicsSystem): MESHES NOW ROTATE! BASIC refactoring of physicsSystem, in preparation for pipeline plan to inact robotic arm aspect. +* 1cbd647 fixes +* 1d8ff11 removals +* 571c277 refactor(SceneView): Reorganised SceneView for redability and logically correct pipeline +* 517e0c4 fixes: Prepartion for code refactor, making object physics easier to manage +* 64ff0f4 removals +* 8132e9a Attempted FIxes +* 6a4c884 fixes(Shader): Some fixes to shader issues +* 24e5732 added Eigen to externals +* 88a51eb feat(MathLib): Added MathLib dll into SimEngine, basic method created to test +* 44e670b fixes(Shader): Fixed lighting map, mesh normals inverted to correct colour +* 2aa1401 Merge branch 'Main' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Main +|\ +| * 96bf217 Update project title and add subtitle +* | 037ac72 removals +* | ffddca2 refactor(MathLib): Moved MathCore to MathLib DLL, tweaked existing file names, and logic to function within the mathlib scope. +* | 80d2103 feat(Positioning): Added up and down functionality into the camera movement +|/ +* 2f9f852 fixes +* 44b652b removals: old file structure +* eafc1d8 Add 'Sim_Engine/' from commit '261a901b789ddc05c2bfc2a1480ef5ba371ca704' +|\ +| | * 75a9a94 (SimEngine/Main, SimEngine/HEAD) feat: added basic axis orientator for debugging +| |/ +| * 261a901 feat(Shaders): Improved IBL and PBR shader logic, shader files and c++ / header files now all work together +| * 82a2420 feat(Shaders): Improved IBL and PBR shader logic, shader files and c++ / header files now all work together +| * 1750a00 fixes: fixed IBL not allowing meshes to be visible +| * 73b31d3 fixes +| * 380957c fixes +| * 75e1ce3 fixes +| * 7d45a12 feat(IBL): Image Base Lighting implemented +| * 4fae869 fixes +| * 8f96666 fixes +| * 5f30c5c feat(IBL): Added equirect cubemap shader logic (fragment) +| * 7536104 feat(IBL): Added Irradiance glsl shader logic +| * cab85a1 feat(IBL): Added Equirectangular Cubemap glsl shaders +| * ece48c1 fixes +| * d2ec064 feat(IBL): added shader logic for BRDF-LUT +| * 4e55238 feat(IBL): Added IBL header and cpp file, along with BASIC logic for IBL integration +| * b0356ec fixes +| * 3756d94 feat(CSM): Moved from PCF-only shader to CMS & PCF shader +| * e7ddf71 general commit: Attempting to add cascading shadows +| * 20a8417 fixes: movement stuttering +| * 9042cab fixes & tweaks +| * d693e2b feat: HUGE FEAT, FUNCTIONAL WALKING AND JUMPING OF PLAYER CAMERA +| * 351246f feat: Added gravity logic, basic implementation (DISFUNCTIONAL) +| * 2c979fb fixes: Release support fixes, functional. +| |\ +| | * 78202a3 Merge pull request #6 from SaltyJoss/Salty/Dev +| | |\ +| | | * 7aaf076 Merge pull request #3 from SaltyJoss/Salty/Playground +| | | |\ +| | * | \ 54df47a EngineLib DLL integration, input/movement updates, and CI runtime fixes +| | |\ \ \ +| | | * | | 622baaf fixes to .vcxproj +| | | * | | 07bff66 fixes +| | | * | | de9dbee Sync CI workflow from main +| | | * | | f122530 gitignore update +| | | * | | f2ed95a Resolved merge conflict, kept local dll porject structure +| | | |\ \ \ +| | | | | |/ +| | | | |/| +| | | | * | 2e59858 Merge branch 'Salty/Dev' into Salty/Playground +| | | | |\| +| | | | | * dd5cfdb fixes +| | | | | * 2cae0ec tests: Added foundation for testing +| | | * | | 6f9df09 feat(movement): added WASD movement, along realtime mouse input +| | | * | | e28dd10 refactor(DLL): build to DLL/EXE structure - fix GLAD integration and Win32 architecture issues +| | | * | | e0883ac fixes(DLL): Sorted build config along with header and cpp fixes +| | | * | | 5ff1072 fixes(DLL): sorted all errors from move to DLL. +| | | * | | 36ac104 fixes +| | | * | | 1586e1f removals +| | | * | | 5e99bb1 refactor(DLL): moved all cpp and h files to a dll library (Engine +| | | |/ / +| | | * | f1f4dbf Resolve merge: keep test files +| | * | | a7143cb Simplify msbuild command in build workflow +| | * | | a8b7c4e Add clean and rebuild steps to build workflow +| | * | | 6ff7d44 Update README with GitHub badges +| | * | | c6880ba Create build script to use Engine.sln +| | * | | 1100a21 Add images section with intro image +| * | | | 7a475cb Release IS WORKING! +| |/ / / +| * | | 6a5693e Merge branch 'Main' of https://github.com/SaltyJoss/Project-Pulsar into Main +| |\ \ \ +| | * \ \ c6d95e0 Merge pull request from SaltyJoss/Salty/Dev for FPS Counter addition and UI tweak +| | |\ \ \ +| | | |/ / +| | |/| / +| | | |/ +| | | * c099bb8 added fps counter, and semi-translusent docking stations +| | |/ +| * / dd8591c tweak +| |/ +| * 9840607 fixes +| * 9c2c729 fixes +| * 6ca156d fixes +| * 0235ae9 feat(skybox): Added fully fucnctional skybox to engine +| * 025152c fixes: Update gitignore to exclude External/*/bin and their dlls +| * da39928 fixes +| * c2c96fa fixes: Shader filepaths sorted. IT RUNS +| * babf35b fixes: correctly added paths for include, library and other linked dependencies +| * c788bc6 refactor: Solution created, folders intiialised +| * 70a3233 refactor(repo): cleanup +| * 2462a74 refactor(repo): migrate simulator + reorganize into Engine/Game structre +| * bf1c0dd Add 'GameSim/' from commit '5e562dbc6304bc29cdda8e0a9e4039231588b495' +| |\ +| | * 5e562db feat(Shders): Add directional shadow shaders +| | * 33d4458 fixes +| | * 40376a5 feat: FIRST SUCCESFULL WORLD GRID RUN +| | * fbe1d89 feat: World Grid now ouputs as an a basic interpolation +| | * a0e6701 fixes +| | * 2e8e833 feat: Initialised new shared pointer of shaderUtil, WorldGridShader +| | * 145f702 fixes +| | * 5ce85e9 feat(Object): Added object.h to deal with object specific movements +| | * 29ac662 fixes +| | * f651156 feat(Looger): Added mutex lock in Debug::logCentral for thread-safe logging to file and console +| | * 315b9f9 fixes: Removed unused files in elements +| | * 792d7b1 fixes: Position of Camera AND Object no longer moves right when mouse is centered +| | * bf41b21 fixes: Moved objects into Objects Folder, with subfolders for types +| | * 04a2e14 fixes +| | * 5a8dcd7 fixes: Simulator no longer moves when mouse is not within its frame +| | * 4f3f7dd feat(Camera Focus Changer): Add buttons to change from Camera focus to Obejct focus when changing position relative to the world +| | * b157a85 feat(Checkered Plane): Added basic checkered plane to act as a "0" layer for objects. +| | * acf426f fixes: MEMORY LEAK +| | * b9d861e feat(Logs): Added logging to files. +| | * 7b2ee5e fixes +| | * a028a34 feat(Logs): Added Logger.h thats a global header for debugging logs, using 3 distinct type {WARN, ERROR, INFO} +| | * 58280fc fixes: OBJECT CREATION NOW WORKS! +| | * 501169d fixes +| | * d92ebb8 fixes +| | * 54c0a82 tweaks: adjust GUI panel positions, docking behavior, and initial layouts +| | * 5fecab0 fixes: OpenGLContext init, correct window pointer handling, prevent NULL this/window deref +| | * e607254 fixes: Stub unused logic files - commented-out placeholders for future implementation +| | * 434396b Added ASSIMP library +| | * e77c82d fixes +| | * ffbb440 feat(Scene View): Add scene view logic, will change to adapt my old logic, this is BAREBONES +| | * df7307b feat(ShaderUtil): Moved basic shader logic over to ShaderUtil +| | * 8088767 fixes +| | * d2939d5 fixes +| | * 8d68084 feat(Scene View): SceneView initialisation +| | * 5054d4e feat(Elements): Addition of elements into Element folder +| | * 1ef9007 fixes +| | * d98c259 feat(GUIContext): Add logic into GUIContext.cpp +| | * 2d73706 Add ImGui Docking libraries +| | * 224947e feat(OpenGLContext): Add logic to OpenGLContext.cpp +| | * 73271a6 Added GLEW library into includes +| | * d948bc0 feat(Buffer Management): Added method logic for VertexIndexBuffers and FrameBuffers +| | * 64a086d fixes +| | * 4bb34f5 feat(Mesh): Initialised a mesh header and cpp file for all stages of the rendering a mesh object into the sim +| | * 6ef8176 feat(Window Manager): Updated WindowManager.h to function inline with current idea of project structure +| | * 63b6235 feat(Render Base): introduced centeralised base for render operations in Render/ +| | * e86c0d5 refactor(File structure): Finally refactored files, not pushing to main until code works +| | * bf2c287 feat(Simulator): Reintroduce Basic object initialisation (but in 3D) +| | * a25bd85 fixes +| | * ccb6006 fixes +| | * a1d41b0 libs(GLM): Added OpenGL's Mathematics library, DOES NOT REPLACE MY OWN LOGIC TO BE LATER INCOPERATED +| | * 9c7bdce feat(Simulation Manager): Initialised basic structure of Simulation Manager Files. +| | * ac8473d fixes +| | * 2f1b0fb feat(Styles): Added Two styles into Style header, and cpp file +| | * f40d005 fixes(GUI): Improved overall placements of each section of the GUI +| | * 5263940 refactore(GUI): Refactored the present GUI so it is sized correctly +| | * e262268 fixes(GUI): Commented out calls to TitleBarPanel.h, may revist in the future if I have time! +| | * 20b50dc fixes +| | * b3ba659 fixes: Corrected "Exception thrown: read access violation. **this** was nullptr." due to mismatched objects +| | * 07d1c84 feat(GUI): Added TaskBarPanel, removed GLFW OS border. Bugs currently needing fixing. +| | * 8ac7f65 fixes +| | * c74b767 refactor(Application): Application and GUIManager initialisation refactored to improve stability, state persistence, and lifetime management. +| | * 114ffc7 feat(DebugLog): Add basic GUI for Debug Logging and Error Handling +| | * b9352a1 fixes +| | * e5dda9e fixes +| | * eed6503 feat(SimulationPanels): Added basic simulator panels (DUMMY PANELS FOR NOW), improving GUI layout +| | * 348b913 feat(SimulationPanels): Initialisation of simulation panels in header and cpp files +| | * 82d20f4 additions: general +| | * ca3e712 feat(Simulator): Addition of ControlPanel header and cpp file +| | * aecd9ed fixes +| | * c6cbf58 feat(Simulation_Core): Added Application class and header file to deal the central application +| | * 482f08e feat(GUI Manager): Add GUI Manager class and header, updated main.cpp +| | * 8678a8f feat(ImGui): Basic initilisation and implementation of ImGui into GLFW window +| | * 43983c8 feat(ImGui library): Added relevant ImGui files into lib folder +| | * 69c3859 fixes: Restrcuture of MathCore for better readablility and seperation between Solutions! +| | * e0c05e9 chore(shaderClass): Add basic error handingling for compilation and linking of shaders +| | * 2932571 feat(OpenGL Sim): Add basic uniform shader +| | * a738176 feat(OpenGL Sim): Added basic Shader for current triangle object using interpolation +| | * bb3e6cd chore(OpenGL Sim): Update main file to use new classes, and their functions +| | * becd363 feat(OpenGL Sim): Created VAO, VBO, and EBO classes +| | * 000616b feat(OpenGL): Added Basic Shader class and header +| | * 99f4f0f feat(OpenGL Sim): Basic test for drawing a triangle onto gl program +| | * 225087f fixes +| | * c4c9507 feat(OpenGL Sim): Added basic test window, intiialisation check for the new libs included. +| | * 3cfbc70 feat(OpenGL): Setup basic simulation folder structure. Inlucdes OpenGL libraries. +| * be97154 Update README.md +| * 3ad6f60 Initial commit +| * ba5f7ce (refs/stash) On Main: temp-before-subtree +|/| +| * d5486d4 index on Main: d922350 refactor(Solution): Refactored entire solution to flow a bit better, more organised (QoL and futureproofing) +|/ +* d922350 refactor(Solution): Refactored entire solution to flow a bit better, more organised (QoL and futureproofing) +* 1387e79 fixes: Corrected dependencies in old Libraries folder, relocated to External, and split by file not lib and include +* 655fb2d feat(Shders): Add directional shadow shaders +* 74fb1d5 fixes +* 03fd5e1 feat: FIRST SUCCESFULL WORLD GRID RUN +* d31d2ff feat: World Grid now ouputs as an a basic interpolation +* a621362 fixes +* f219cef feat: Initialised new shared pointer of shaderUtil, WorldGridShader +* 35a3e86 fixes +* eabdf87 feat(Object): Added object.h to deal with object specific movements +* 3f42023 fixes +* e1ce07a feat(Looger): Added mutex lock in Debug::logCentral for thread-safe logging to file and console +* ee48a46 fixes: Removed unused files in elements +* b927341 fixes: Position of Camera AND Object no longer moves right when mouse is centered +* 8ca2539 fixes: Moved objects into Objects Folder, with subfolders for types +* 043427d fixes +* 9cbb87f fixes: Simulator no longer moves when mouse is not within its frame +* 8f259af feat(Camera Focus Changer): Add buttons to change from Camera focus to Obejct focus when changing position relative to the world +* bb5dd37 feat(Checkered Plane): Added basic checkered plane to act as a "0" layer for objects. +* 7199b8f fixes: MEMORY LEAK +* fa0686f feat(Logs): Added logging to files. +* e042200 fixes +* 92ff672 feat(Logs): Added Logger.h thats a global header for debugging logs, using 3 distinct type {WARN, ERROR, INFO} +* 6e6b418 fixes: OBJECT CREATION NOW WORKS! +* 27fc6ec fixes +* 49fb90b fixes +* 4964669 tweaks: adjust GUI panel positions, docking behavior, and initial layouts +* 124ff01 fixes: OpenGLContext init, correct window pointer handling, prevent NULL this/window deref +* f4e7242 fixes: Stub unused logic files - commented-out placeholders for future implementation +* 0fbcf86 Added ASSIMP library +* 77d9920 fixes +* fd6adb7 feat(Scene View): Add scene view logic, will change to adapt my old logic, this is BAREBONES +* 9bad0af feat(ShaderUtil): Moved basic shader logic over to ShaderUtil +* 07f07bc fixes +* 01c1396 fixes +* 6275fef feat(Scene View): SceneView initialisation +* 042bee2 feat(Elements): Addition of elements into Element folder +* eabd03e fixes +* b4027e6 feat(GUIContext): Add logic into GUIContext.cpp +* bb0f740 Add ImGui Docking libraries +* 8fc7dd3 feat(OpenGLContext): Add logic to OpenGLContext.cpp +* 5564670 Added GLEW library into includes +* 29aa976 feat(Buffer Management): Added method logic for VertexIndexBuffers and FrameBuffers +* c1bd4e4 fixes +* 827de47 feat(Mesh): Initialised a mesh header and cpp file for all stages of the rendering a mesh object into the sim +* 6d8230d feat(Window Manager): Updated WindowManager.h to function inline with current idea of project structure +* b90a704 feat(Render Base): introduced centeralised base for render operations in Render/ +* 9d4e172 refactor(File structure): Finally refactored files, not pushing to main until code works +* 5fc04ec feat(Simulator): Reintroduce Basic object initialisation (but in 3D) +* 182bbb6 fixes +* 371be02 fixes +* b7fa57c libs(GLM): Added OpenGL's Mathematics library, DOES NOT REPLACE MY OWN LOGIC TO BE LATER INCOPERATED +* c243baf feat(Simulation Manager): Initialised basic structure of Simulation Manager Files. +* d8f792a fixes +* 4829149 feat(Styles): Added Two styles into Style header, and cpp file +* 04b284f fixes(GUI): Improved overall placements of each section of the GUI +* ef24297 refactore(GUI): Refactored the present GUI so it is sized correctly +* a7aca65 fixes(GUI): Commented out calls to TitleBarPanel.h, may revist in the future if I have time! +* 8a02ab0 fixes +* 451804d fixes: Corrected "Exception thrown: read access violation. **this** was nullptr." due to mismatched objects +* 95461c6 feat(GUI): Added TaskBarPanel, removed GLFW OS border. Bugs currently needing fixing. +* f125a8b fixes +* 8a8408e refactor(Application): Application and GUIManager initialisation refactored to improve stability, state persistence, and lifetime management. +* 87f5698 feat(DebugLog): Add basic GUI for Debug Logging and Error Handling +* df45a40 fixes +* 51259e6 fixes +* edd07d7 feat(SimulationPanels): Added basic simulator panels (DUMMY PANELS FOR NOW), improving GUI layout +* 3aa9a0f feat(SimulationPanels): Initialisation of simulation panels in header and cpp files +* 26f30b3 additions: general +* c964391 feat(Simulator): Addition of ControlPanel header and cpp file +* f46d2c8 fixes +* 1a4b1bd feat(Simulation_Core): Added Application class and header file to deal the central application +* 6cfb78e feat(GUI Manager): Add GUI Manager class and header, updated main.cpp +* 9c11d32 feat(ImGui): Basic initilisation and implementation of ImGui into GLFW window +* 21c86b6 feat(ImGui library): Added relevant ImGui files into lib folder +* f67132d fixes: Restrcuture of MathCore for better readablility and seperation between Solutions! +* 770e797 chore(shaderClass): Add basic error handingling for compilation and linking of shaders +* d3d40e3 feat(OpenGL Sim): Add basic uniform shader +* 7a1c558 feat(OpenGL Sim): Added basic Shader for current triangle object using interpolation +* c7add94 chore(OpenGL Sim): Update main file to use new classes, and their functions +* b24f263 feat(OpenGL Sim): Created VAO, VBO, and EBO classes +* eb840bd feat(OpenGL): Added Basic Shader class and header +* 731bb38 feat(OpenGL Sim): Basic test for drawing a triangle onto gl program +* ef5e205 fixes +* 0c82f0c feat(OpenGL Sim): Added basic test window, intiialisation check for the new libs included. +* 283d8e5 Merge branch 'Main' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Main +|\ +| * 529c683 Remove duplicate entry +| * ca4afbe Update .gitignore to include .out directory +| * f65c84c Fix duplicate entry for '*.VC.VC.opendb' in .gitignore +* | b097d5a feat(OpenGL): Setup basic simulation folder structure. Inlucdes OpenGL libraries. +* | 1b4bb18 feat: Add Eigen-based numerical integration header with Euler, RK2, and RK4 +|/ +* 100de0d Merge branch 'Main' of https://github.com/SaltyJoss/RoboticArm_MathModelling into Main +|\ +| * c4e8bee Update README.md +* | 8ed245a Initial project setup: folder structure, CMakeLists.txt, vcpkg Eigen3 integration, placeholder src/main.cpp +|/ +* ee90cf0 Initial commit From 0a3601f7a439caeb20ca4c20c65bddd08140900a Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 22 Feb 2026 13:58:11 +0000 Subject: [PATCH 13/20] WIP: parallel batch runs & async integrator comparison in GUI NOTES: - Major refactor for thread-safe parallel simulation and integrator comparison - Batch mode now uses hardware concurrency, times runs, and sorts results. - StudyRunner and GUI now launch async simulation tasks, poll results, and handle exceptions robustly. - Integrator comparison runs each method in parallel, results are sorted and displayed when ready. - Added thread-safe result handling in SimManager and ControlPanel. - SSAO kernel generation uses C++ RNG for reproducibility. - Script integrator replacement uses regex for reliability. - Improved OpenGL resource cleanup and modularity. - UI is more responsive; background runs do not block main thread. TODO: - Make parallelisation Universal --- Sim_Engine/Engine/src/BatchMain.cpp | 35 ++- .../EngineLib/include/Platform/StudyRunner.h | 2 +- .../EngineLib/include/Scene/SimulationCore.h | 2 +- .../include/Scene/SimulationManager.h | 8 + .../include/ui/CommandScriptEditor.h | 21 ++ .../EngineLib/include/ui/ControlPanel.h | 44 ++- Sim_Engine/EngineLib/pch.h | 2 + .../EngineLib/src/Platform/StudyRunner.cpp | 99 +++---- .../EngineLib/src/Scene/SimulationCore.cpp | 15 +- .../EngineLib/src/Scene/SimulationManager.cpp | 130 +++++---- .../EngineLib/src/ui/CommandScriptEditor.cpp | 104 +++++++ Sim_Engine/EngineLib/src/ui/ControlPanel.cpp | 260 ++++++++++++------ 12 files changed, 518 insertions(+), 204 deletions(-) diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp index 0566d7d..7569090 100644 --- a/Sim_Engine/Engine/src/BatchMain.cpp +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -5,6 +5,10 @@ #include "Platform/StudyRunner.h" #include "Numerics/IntegrationMethods.h" +#include // ensure at top of file +#include // for fprintf + + // Helper: create CorePtr (unique_ptr with std::function deleter) static CorePtr makeCoreFactory() { core::ISimulationCore* raw = CreateSimulationCore_v1(); @@ -31,7 +35,7 @@ int runBatchMode() { }; std::vector dts = { 0.001, 0.002, 0.005 }; std::vector lengths_mins = { 0.5, 1.0, 5.0 }; - + // Create a config for each combination of method, dt, and length for (auto m : methods) { for (double dt : dts) { for (double len : lengths_mins) { @@ -45,18 +49,37 @@ int runBatchMode() { } } - // Build StudyRunner with factory - StudyRunner runner([]() { return makeCoreFactory(); }, /*maxConcurrency=*/ 0); + // Determine the number of worker threads to use based on hardware concurrency + size_t cores = std::thread::hardware_concurrency(); + size_t workers = 0; // default: use all cores + + // If not in batch mode only, reserve one core for the main thread +#ifndef _BATCH_MODE_ONLY + workers = (cores > 1) ? cores - 1 : 1; +#endif + // Build runner with factory and worker count + StudyRunner runner(makeCoreFactory, workers); // The script text for the run(s) std::string scriptText = R"( - // your DSL script here, example: set(integrator, rk4) - // ... more script ... + start() )"; - // Run studies (note: runStudies signature expects std::string& in your header) + // Run the studies and time the total batch duration + auto T0 = std::chrono::high_resolution_clock::now(); auto results = runner.runStudies(configs, scriptText); + auto T1 = std::chrono::high_resolution_clock::now(); + + // Log total batch time + double total = std::chrono::duration(T1 - T0).count(); + fprintf(stderr, "TOTAL BATCH TIME = %.3f sec\n", total); + + // Sort results by tag for easier reading + std::sort(results.begin(), results.end(), + [](const StudyResult& a, const StudyResult& b) { + return a.tag < b.tag; + }); // Print summary for (const auto& r : results) { diff --git a/Sim_Engine/EngineLib/include/Platform/StudyRunner.h b/Sim_Engine/EngineLib/include/Platform/StudyRunner.h index 2fbf68c..cd05214 100644 --- a/Sim_Engine/EngineLib/include/Platform/StudyRunner.h +++ b/Sim_Engine/EngineLib/include/Platform/StudyRunner.h @@ -43,7 +43,7 @@ class ENGINE_API StudyRunner { ~StudyRunner() = default; // Run a batch of studies in parallel using the script string as the "run" for each sim, returning the results when all are complete - std::vector runStudies(const std::vector& configs, std::string& scriptText); + std::vector runStudies(const std::vector& configs, const std::string& scriptText); private: MakeCoreFn _makeCore; // Factory function to create SimulationCore instances for each run diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h index d65afbd..daff98d 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h @@ -140,4 +140,4 @@ namespace core { robots::TrajRefBuffer _trajRefBuffer; // Buffer for logging trajectory reference data each step bool _telemetryBegun = false; }; -} \ No newline at end of file +} // namespace core \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h index 0235774..fa65464 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h @@ -5,7 +5,9 @@ #include #include #include +#include #include +#include "Platform/StudyRunner.h" #include "Rendering/ModelGroup.h" #include "Scene/ObjectID.h" @@ -56,6 +58,12 @@ namespace gui { // SimManager Class (Plan on renaming later) class ENGINE_API SimManager { public: + // Called by background worker threads (via CommandScriptEditor) to hand finished results to the manager + void pushCompletedRun(StudyResult result); + + // Get and clear completed runs for display (GUI calls this) + std::vector takeCompletedRuns(); + // Constructor & Destructor SimManager(); ~SimManager(); diff --git a/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h b/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h index e897878..8c04e01 100644 --- a/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h +++ b/Sim_Engine/EngineLib/include/ui/CommandScriptEditor.h @@ -2,6 +2,12 @@ // File: CommandScriptEditor.h // GitHub: SaltyJoss #include "EngineCore.h" + +#include +#include +#include +#include "Platform/StudyRunner.h" + #include #include #include @@ -15,8 +21,14 @@ #include "Platform/Logger.h" +struct ENGINE_API StudyResult; // forward declaration to avoid circular dependency namespace gui { + struct ActiveRun { + std::future fut; + std::string tag; + }; + class ENGINE_API CommandScriptEditor { public: CommandScriptEditor(gui::SimManager* sims); @@ -28,6 +40,10 @@ namespace gui { void render(); private: + // Active runs management + std::mutex _activeRunsMutex; // Mutex for synchronizing access to active runs + std::vector _activeRuns; // Vector to hold active runs and their futures + // Simulation manager reference gui::SimManager* _sim; @@ -49,6 +65,8 @@ namespace gui { std::string _pendingSavePath = "Engine/assets/scripts"; bool _requestSaveAsPopup = false; + void runButtonHandler(); + void renderEnvironment(); void renderCmdInstructions(); @@ -57,6 +75,9 @@ namespace gui { bool tryLoadFromDialog(); bool trySaveScriptToFile(const std::string& filepath); + void pollRuns(); + void launchBackgroundRun(const std::string& code, const std::string& tag); + void renderSaveAsPopup(); void beginEditorPanel(const char* id); diff --git a/Sim_Engine/EngineLib/include/ui/ControlPanel.h b/Sim_Engine/EngineLib/include/ui/ControlPanel.h index fb16380..17ae0e4 100644 --- a/Sim_Engine/EngineLib/include/ui/ControlPanel.h +++ b/Sim_Engine/EngineLib/include/ui/ControlPanel.h @@ -2,20 +2,26 @@ // File: ControlPanel.h // GitHub: SaltyJoss #include "EngineCore.h" +#include +#include +#include #include -#include "Physics/PhysicsSystem.h" +#include -#include "Scene/Object.h" -#include "Analysis/Telemetry.h" -#include "Scene/SimulationManager.h" -#include "Scene/Light.h" -#include "Platform/Logger.h" #include "Platform/SimulationState.h" -#include +#include "Platform/Logger.h" -#include -#include "Platform/imguiWidgets.h" -#include +// Forward Declarations +namespace scene { + class ENGINE_API Mesh; + class ENGINE_API Object; + class ENGINE_API Light; + class ENGINE_API Camera; + class ENGINE_API SimManager; +} +namespace physics { class ENGINE_API PhysicsSystem; } +namespace robots { class ENGINE_API RobotSystem; } +namespace diagnostics { class ENGINE_API TelemetryRecorder; } namespace gui { // Gravity UI Modes @@ -63,6 +69,16 @@ namespace gui { inline GravityLevel gravityLevel = GravityLevel::Root; inline GravityUIMode gravityMode = GravityUIMode::Preset; + // 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; + }; + // ControlPanel Class class ENGINE_API ControlPanel { public: @@ -74,6 +90,11 @@ namespace gui { void setMeshLoadCallback(const std::function& callback) { meshLoadCallback = callback; } private: + // Comparison results management + std::vector> _comparisonFutures; + std::atomic _comparisonPending{ 0 }; + std::mutex _comparisonMutex; + // Internal Pointers std::shared_ptr _mesh; @@ -114,7 +135,8 @@ namespace gui { // Results Methods void drawResultsWindow(); void exportTelemetryCSV(const char* filepath); - void runComparisonAllIntegrators(); + void runComparisonAllIntegratorsAsync(); + void pollComparisonFutures(); void drawComparisonPlots(); // Helper Methods diff --git a/Sim_Engine/EngineLib/pch.h b/Sim_Engine/EngineLib/pch.h index 9da973e..1b10a9a 100644 --- a/Sim_Engine/EngineLib/pch.h +++ b/Sim_Engine/EngineLib/pch.h @@ -20,9 +20,11 @@ #include #include #include +#include #include #include #include +#include #include #include diff --git a/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp b/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp index e4a43be..9aba33a 100644 --- a/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp +++ b/Sim_Engine/EngineLib/src/Platform/StudyRunner.cpp @@ -35,14 +35,14 @@ StudyRunner::StudyRunner(MakeCoreFn makeCore, size_t maxConcurrency) static int findReadyIndex(std::vector>& futures) { // Check each future for readiness without blocking, returning the index of the first ready future for (size_t i = 0; i < futures.size(); ++i) { - // Check if this future is ready by waiting for 100ms (non-blocking) - if (futures[i].wait_for(std::chrono::milliseconds(100)) == std::future_status::ready) { return (int)i; } + // Check if this future is ready by waiting for 0ms (non-blocking) + if (futures[i].wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) { return (int)i; } } return -1; // No future is ready yet } // Run a batch of studies in parallel using the script string as the "run" for each sim, returning the results when all are complete -std::vector StudyRunner::runStudies(const std::vector& configs, std::string& scriptText) { +std::vector StudyRunner::runStudies(const std::vector& configs, const std::string& scriptText) { std::vector> futures; // Vector to hold the futures for each async run futures.reserve(configs.size()); @@ -60,72 +60,55 @@ std::vector StudyRunner::runStudies(const std::vector& conf (void)r; // silence unused variable warning } // catch any exceptions thrown during the async run and log them, but continue processing other runs (very important to my initial implementation goal!!) - catch (const std::exception& e) { - std::cerr << "Error in study run: " << e.what() << std::endl; - } + catch (const std::exception& e) { std::cerr << "Error in study run: " << e.what() << std::endl; } futures.erase(futures.begin() + idx); // remove the future from the vector after collecting its result } else { std::this_thread::sleep_for(std::chrono::milliseconds(5)); } // Sleep briefly to avoid busy-waiting if no futures are ready } - fprintf(stdout, - "START thread %zu | tag=%s\n", - std::hash{}(std::this_thread::get_id()), - cfg.tag.c_str()); - fflush(stdout); - // Start a new async run for this config, capturing the current config and program by value to make sure they are safely used in the async context - futures.push_back(std::async(std::launch::async, [this, cfg, scriptText]() -> StudyResult { - StudyResult result{}; - result.tag = cfg.tag; - result.dt = cfg.dt; - - auto simCore = _makeCore(); - // If we couldn't create a SimulationCore, return a failed result immediately - if (!simCore) { - result.success = false; - result.intName = "N/A"; - result.simTime = 0.0; - result.samples = 0; - return result; - } - // Configure the SimulationCore for this run before building the program - simCore->setFixedDt(cfg.dt); - simCore->setIntegrationMethod(cfg.method); + futures.push_back(std::async(std::launch::async, + [this, cfg, script = scriptText]() -> StudyResult { + StudyResult result{}; + result.tag = cfg.tag; + result.dt = cfg.dt; - // Create a program and parser for this run, bound to the SimulationCore we just created - auto program = std::make_unique(simCore.get()); - interpreter::Parser parser(program.get()); - // Parse the script text to build the program for this run, and start it - parser.parse(scriptText); - program->start(); + auto simCore = _makeCore(); + // If we couldn't create a SimulationCore, return a failed result immediately + if (!simCore) { + result.success = false; + result.intName = "N/A"; + result.simTime = 0.0; + result.samples = 0; + return result; + } + // Configure the SimulationCore for this run before building the program + simCore->setFixedDt(cfg.dt); + simCore->setIntegrationMethod(cfg.method); - // Run synchronously (blocking inside thread) - bool ok = simCore->runScriptToCompletion(program.get(), cfg.method); // Run the provided program/script to completion with method, blocking until it finishes. - // NOTE: This is where the actual study run happens, it is block because it is inside the async thread, so it will not block the main thread (OR other runs), and allows DSFE to have multiple runs in parallel!!! <- EXACTLY what I want + // Create a program and parser for this run, bound to the SimulationCore we just created + auto program = std::make_unique(simCore.get()); + interpreter::Parser parser(program.get()); + // Parse the script text to build the program for this run, and start it + parser.parse(script); + program->start(); - // Get telemetry data for results - result.success = ok; - result.intName = simCore->integrationMethodName(); // Get the name of the current integration method for reporting - result.simTime = simCore->simTime(); // Get the total simulation time that elapsed during this run - // Telemetry sample count is implementation-defined -> guarded for availability - try { - result.samples = simCore->telemetrySampleCount(); - } - // If telemetry is not available or throws an exception, it is caught and samples to 0 to indicate no data was collected - catch (...) { - result.samples = 0; - } + // Run synchronously (blocking inside thread) + bool ok = simCore->runScriptToCompletion(program.get(), cfg.method); // Run the provided program/script to completion with method, blocking until it finishes. + // NOTE: This is where the actual study run happens, it is block because it is inside the async thread, so it will not block the main thread (OR other runs), and allows DSFE to have multiple runs in parallel!!! <- EXACTLY what I want - fprintf(stdout, - "END thread %zu | tag=%s\n", - std::hash{}(std::this_thread::get_id()), - cfg.tag.c_str()); - fflush(stdout); + // Get telemetry data for results + result.success = ok; + result.intName = simCore->integrationMethodName(); // Get the name of the current integration method for reporting + result.simTime = simCore->simTime(); // Get the total simulation time that elapsed during this run + // Telemetry sample count is implementation-defined -> guarded for availability + try { result.samples = simCore->telemetrySampleCount(); } + // If telemetry is not available or throws an exception, it is caught and samples to 0 to indicate no data was collected + catch (...) { result.samples = 0; } - // Get the number of telemetry samples collected during this run (proxy for how much data we obtained) - return result; // Return the result of this study run - })); + // Get the number of telemetry samples collected during this run (proxy for how much data we obtained) + return result; // Return the result of this study run + })); } // Collect the results from all the futures diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index eb7d183..ff43e72 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -487,4 +487,17 @@ namespace core { // Setter and getter for the active script program void SimulationCore::setActiveProgram(interpreter::IStoredProgram* p) { _activeProgram = p; } interpreter::IStoredProgram* SimulationCore::activeProgram() const { return _activeProgram; } -} \ No newline at end of file +} + +// DLL Exported factory Functions +extern "C" { + // Create function + __declspec(dllexport) core::ISimulationCore* CreateSimulationCore_v1() { + try { return new core::SimulationCore(); } + catch (...) { return nullptr; } + } + // Delete function + __declspec(dllexport) void DestroySimulationCore(core::ISimulationCore* p) { + delete p; // free inside DLL + } +} // extern "C" \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp index 3a12415..6398e5b 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp @@ -12,6 +12,8 @@ #include #include +#include + #include "Scene/Input.h" #include "Scene/Camera.h" #include "Scene/Mesh.h" @@ -42,6 +44,10 @@ namespace gui { // --- PIMPL Implementation --- struct SimManager::Impl { + // Completed Simulation Runs (thread-safe) + std::mutex _completedRunsMutex; + std::vector _completedRuns; + // View ID Alias using VID = gui::ViewID; @@ -267,36 +273,47 @@ namespace gui { initSSAONoise(); } + // Random number generation for SSAO kernel and noise + std::mt19937 _rng{ std::random_device{}() }; + std::uniform_real_distribution _uni{ -1.0f, 1.0f }; + + // Helper to get random float in [a,b] + inline float rngFloat(float a, float b) { + std::uniform_real_distribution d(a, b); + return d(_rng); + } + + // Initialises the SSAO kernel with random samples in a hemisphere oriented along the positive Z axis, scaled to favor samples closer to the origin void initSSAOKernel(int size) { _ssaoKernel.clear(); _ssaoKernel.reserve(size); for (int i = 0; i < size; ++i) { glm::vec3 sample( - ((float)rand() / RAND_MAX) * 2.0f - 1.0f, - ((float)rand() / RAND_MAX) * 2.0f - 1.0f, - ((float)rand() / RAND_MAX) // hemisphere: z in [0,1] + rngFloat(-1.0f, 1.0f), + rngFloat(-1.0f, 1.0f), + rngFloat(0.0f, 1.0f) // hemisphere: z in [0,1] ); sample = glm::normalize(sample); - sample *= ((float)rand() / RAND_MAX); - - // Accelerating interpolation: cluster samples closer to origin + sample *= rngFloat(0.0f, 1.0f); float scale = (float)i / (float)size; - scale = 0.1f + scale * scale * 0.9f; // lerp(0.1, 1.0, scale*scale) + scale = 0.1f + scale * scale * 0.9f; sample *= scale; - _ssaoKernel.push_back(sample); } } + // Initializes the SSAO noise texture with random rotation vectors in the XY plane void initSSAONoise() { std::vector noise(16); + // Generate 16 random rotation vectors in the XY plane (Z=0) for (int i = 0; i < 16; ++i) { noise[i] = glm::vec3( - ((float)rand() / RAND_MAX) * 2.0f - 1.0f, - ((float)rand() / RAND_MAX) * 2.0f - 1.0f, + rngFloat(-1.0f, 1.0f), + rngFloat(-1.0f, 1.0f), 0.0f ); } + // Create OpenGL texture glGenTextures(1, &_ssaoNoiseTex); glBindTexture(GL_TEXTURE_2D, _ssaoNoiseTex); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA16F, 4, 4, 0, GL_RGB, GL_FLOAT, noise.data()); @@ -306,20 +323,20 @@ namespace gui { glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); } + // Ensures SSAO FBOs and textures are created and match the given size, recreating them if necessary void ensureSSAOBuffers(int w, int h) { if (_ssaoW == w && _ssaoH == h) return; _ssaoW = w; _ssaoH = h; - - // Cleanup old + // Delete old buffers if they exist if (_ssaoFBO) { glDeleteFramebuffers(1, &_ssaoFBO); glDeleteTextures(1, &_ssaoTex); } if (_ssaoBlurFBO) { glDeleteFramebuffers(1, &_ssaoBlurFBO); glDeleteTextures(1, &_ssaoBlurTex); } - + // Lambda to create a single-channel floating point FBO and texture auto makeSingleChannelFBO = [](GLuint& fbo, GLuint& tex, int w, int h) { glGenFramebuffers(1, &fbo); glGenTextures(1, &tex); glBindFramebuffer(GL_FRAMEBUFFER, fbo); glBindTexture(GL_TEXTURE_2D, tex); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, w, h, 0, GL_RED, GL_FLOAT, nullptr); + glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, w, h, 0, GL_RED, GL_FLOAT, nullptr); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); @@ -327,22 +344,22 @@ namespace gui { glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, tex, 0); glBindFramebuffer(GL_FRAMEBUFFER, 0); }; - + // Create SSAO and blur FBOs/textures makeSingleChannelFBO(_ssaoFBO, _ssaoTex, w, h); makeSingleChannelFBO(_ssaoBlurFBO, _ssaoBlurTex, w, h); } + // Renders the SSAO pass and subsequent blur pass, writing results to SSAO FBOs. Should be called after rendering the scene to the view's framebuffer (to provide depth texture input). void renderSSAO(SimManager& owner, Viewport& v) { if (!owner._settingsCurrent.ssao) return; - + // Determine SSAO buffer size based on division factor int ssaoDiv = std::max(1, owner._settingsCurrent.ssaoResDiv); int ssaoW = std::max(1, v.w / ssaoDiv); int ssaoH = std::max(1, v.h / ssaoDiv); ensureSSAOBuffers(ssaoW, ssaoH); - + // Clamp kernel size to available samples int kernelSize = std::min((int)_ssaoKernel.size(), owner._settingsCurrent.ssaoSamples); - - // --- SSAO pass --- + // SSAO Pass glBindFramebuffer(GL_FRAMEBUFFER, _ssaoFBO); glViewport(0, 0, ssaoW, ssaoH); glClear(GL_COLOR_BUFFER_BIT); @@ -355,11 +372,12 @@ namespace gui { glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, v.fb->getDepthTexture()); _ssaoShader->setInt1(0, "gDepth"); - + // Noise texture glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_2D, _ssaoNoiseTex); _ssaoShader->setInt1(1, "gNoise"); + // Camera matrices and parameters _ssaoShader->setMat4(v.cam->getProjection(), "projection"); _ssaoShader->setMat4(glm::inverse(v.cam->getProjection()), "invProjection"); _ssaoShader->setVec2(glm::vec2((float)ssaoW / 4.0f, (float)ssaoH / 4.0f), "noiseScale"); @@ -368,29 +386,28 @@ namespace gui { _ssaoShader->setFlt1(0.035f, "bias"); _ssaoShader->setFlt1(owner._settingsCurrent.ssaoStrength, "strength"); - for (int i = 0; i < kernelSize; ++i) { - _ssaoShader->setVec3(_ssaoKernel[i], "samples[" + std::to_string(i) + "]"); - } + // SSAO kernel samples + for (int i = 0; i < kernelSize; ++i) { _ssaoShader->setVec3(_ssaoKernel[i], "samples[" + std::to_string(i) + "]"); } glBindVertexArray(_fullscreenVAO); glDrawArrays(GL_TRIANGLES, 0, 3); - // --- Blur pass --- + // Blur pass glBindFramebuffer(GL_FRAMEBUFFER, _ssaoBlurFBO); glViewport(0, 0, ssaoW, ssaoH); glClear(GL_COLOR_BUFFER_BIT); + // No need for depth test or blending for a simple fullscreen blur _ssaoBlurShader->use(); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, _ssaoTex); _ssaoBlurShader->setInt1(0, "ssaoInput"); - glDrawArrays(GL_TRIANGLES, 0, 3); glBindVertexArray(0); - glBindFramebuffer(GL_FRAMEBUFFER, 0); } + // Renders the given viewport to its framebuffer, handling dynamic resizing of the framebuffer and camera aspect ratio based on the provided display size void renderView(SimManager& owner, Viewport& v, int displayW, int displayH) { displayW = std::max(1, displayW); displayH = std::max(1, displayH); @@ -568,6 +585,7 @@ namespace gui { _core->setTrajectoryManager(&_impl->_traj); } + // Initialises OpenGL resources, including framebuffers, shaders, and IBL. Also picks an internal resolution preset based on the display size to balance quality and performance. void SimManager::initGL() { if (_glReady) return; _glReady = true; @@ -587,13 +605,47 @@ namespace gui { applyRenderProfile(s, bestPreset); } + // Cleans up OpenGL resources SimManager::~SimManager() { - if (_impl->_mesh) _impl->_mesh->clean(); + // Clean up OpenGL resources + if (_impl) { + glDeleteFramebuffers(NUM_CASCADES, _impl->_cascadeFBO); + glDeleteTextures(NUM_CASCADES, _impl->_cascadeDepth); + if (_impl->_ssaoNoiseTex) glDeleteTextures(1, &_impl->_ssaoNoiseTex); + if (_impl->_ssaoTex) glDeleteTextures(1, &_impl->_ssaoTex); + if (_impl->_ssaoBlurTex) glDeleteTextures(1, &_impl->_ssaoBlurTex); + if (_impl->_fullscreenVAO) glDeleteVertexArrays(1, &_impl->_fullscreenVAO); + if (_impl->_worldGridVAO) glDeleteVertexArrays(1, &_impl->_worldGridVAO); + } + // Clean up scene objects and meshes if needed + if (_impl && _impl->_mesh) { _impl->_mesh->clean(); } } // Helper to get the next ObjectID static inline scene::ObjectID next(scene::ObjectID id) { return static_cast(static_cast(id) + 1); } + // -------------------------------------------------- + // THREAD-SAFE SIMULATION RESULTS + // -------------------------------------------------- + + // Add a completed simulation run to the list in a thread-safe manner + void SimManager::pushCompletedRun(StudyResult result) { + if (!_impl) { return; } + std::lock_guard lk(_impl->_completedRunsMutex); + _impl->_completedRuns.push_back(std::move(result)); + } + + // Retrieve and clear completed runs in a thread-safe manner + std::vector SimManager::takeCompletedRuns() { + std::vector copy; + if (!_impl) { return copy; } + std::lock_guard lk(_impl->_completedRunsMutex); + copy = std::move(_impl->_completedRuns); + _impl->_completedRuns.clear(); + return copy; + } + + // -------------------------------------------------- // LIGHT & SKYBOX // -------------------------------------------------- @@ -1182,26 +1234,12 @@ namespace gui { // Helper to replace the integrator method in the script text // A hacky approach my idea, but it works for me and honestly im starting to write up the dissertation so IT WILL DO :) // PS:If anyone has any better solution msg me - static std::string replaceIntegratorInScript(const std::string& script, const std::string& methodName) { - std::string result = script; - // Find set(integrator, ...) and replace the method name - const std::string prefix = "set(integrator,"; - auto pos = result.find(prefix); - - // If not found, also check for "set(integrator, " with a space after the comma - if (pos == std::string::npos) { - const std::string prefix2 = "set(integrator, "; - pos = result.find(prefix2); - } - // If we found a set(integrator, ...), replace the method name inside the parentheses - if (pos != std::string::npos) { - auto end = result.find(')', pos); - if (end != std::string::npos) { - result.replace(pos, end - pos + 1, "set(integrator, " + methodName + ")"); - } - } - return result; + // This seems to be the better solution? + static std::string replaceIntegratorInScript(const std::string& script, const std::string& methodName) { + std::regex re(R"((?i)set\s*\(\s*integrator\s*,\s*([a-z0-9_]+)\s*\))"); // case-insensitive regex to match my DSL command -> set(integrator, method) + std::string replacement = "set(integrator, " + methodName + ")"; + return std::regex_replace(script, re, replacement); } // Run a script to completion synchronously with a specific integrator diff --git a/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp b/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp index 5991a2c..dfb7bf5 100644 --- a/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp +++ b/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp @@ -3,6 +3,7 @@ // GitHub: SaltyJoss #include #include "ui/CommandScriptEditor.h" +#include "Numerics/IntegrationMethods.h" #include "Interpreter/StoredProgram.h" #include "Platform/Paths.h" #include @@ -171,6 +172,45 @@ namespace gui { } } + void CommandScriptEditor::runButtonHandler() { + if (ImGui::Button("Run (background)")) { + // snapshot script text & tag + std::string code = _scriptText; + if (!code.empty() && code.back() == '\0') code.pop_back(); + std::string tag = filenameOnly(_currentScriptFile); + + // launch async task (by-value capture) + std::future f = std::async(std::launch::async, [code, tag]() -> StudyResult { + StudyResult r{}; + r.tag = tag; + // create a fresh sim core + core::ISimulationCore* raw = CreateSimulationCore_v1(); + if (!raw) { r.success = false; return r; } + CorePtr core(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); + + // create program+parser bound to new core + auto program = std::make_unique(core.get()); + interpreter::Parser parser(program.get()); + parser.parse(code); + program->start(); + + // run to completion (blocks inside worker thread) + bool ok = core->runScriptToCompletion(program.get(), integration::eIntegrationMethod::RK4 /*or read from script*/); + + // gather result (you can use core->telemetry() etc to fill more fields) + r.success = ok; + r.intName = "background"; r.simTime = core->simTime(); + return r; + }); + + // register active run + { + std::lock_guard lk(_activeRunsMutex); + _activeRuns.push_back(ActiveRun{ std::move(f), tag }); + } + } + } + void CommandScriptEditor::terminateScript(const char* reason, bool fault) { _sim->setActiveProgram(nullptr); _sim->stopSimulation(); @@ -329,6 +369,70 @@ namespace gui { return true; } + // Poll active background runs for completion and forward results to SimManager + void CommandScriptEditor::pollRuns() { + std::lock_guard lk(_activeRunsMutex); + // Iterate over active runs and check for completion + for (auto it = _activeRuns.begin(); it != _activeRuns.end(); ) { + auto& ar = *it; + if (ar.fut.valid() && ar.fut.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) { + StudyResult r; + try { r = ar.fut.get(); } + catch (const std::exception& e) { + // convert exception into a failed StudyResult + r.success = false; + r.tag = ar.tag; + } + // forward to SimManager (thread-safe push) + if (_sim) { _sim->pushCompletedRun(std::move(r)); } + it = _activeRuns.erase(it); + } + else { + ++it; + } + } + } + + // Launch a background run of the current script (for studies) - captures code and tag by value for thread safety + void CommandScriptEditor::launchBackgroundRun(const std::string& code, const std::string& tag) { + // capture code and tag by value -> worker owns its own SimulationCore and program + std::future f = std::async(std::launch::async, [code, tag]() -> StudyResult { + StudyResult r{}; + r.tag = tag; + + // Create fresh simulation core for worker thread + core::ISimulationCore* raw = CreateSimulationCore_v1(); + if (!raw) { r.success = false; return r; } + CorePtr core(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); + + // bind program+parser to the new core + auto program = std::make_unique(core.get()); + interpreter::Parser parser(program.get()); + parser.parse(code); + program->start(); + + // Choose integration method (could be read from script or set as default) + integration::eIntegrationMethod method = integration::eIntegrationMethod::RK4; // NOTE: we need to infer integrator from script or choose a default (RK4 is default everywhere!) + + // Block inside worker thread until completion + bool ok = core->runScriptToCompletion(program.get(), method); + + // Fill results + r.success = ok; + r.intName = core->integrationMethodName(); + r.simTime = core->simTime(); + try { r.samples = core->telemetrySampleCount(); } + catch (...) { r.samples = 0; } + return r; + }); + + // register active run + { + std::lock_guard lk(_activeRunsMutex); + _activeRuns.push_back(ActiveRun{ std::move(f), tag }); + } + } + void CommandScriptEditor::renderSaveAsPopup() { // Window Padding ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(16.0f, 12.0f)); diff --git a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp index 6315f68..c2432eb 100644 --- a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp +++ b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp @@ -1,16 +1,7 @@ #include "pch.h" // File: ControlPanel.cpp // GitHub: SaltyJoss -#include "Scene/Camera.h" -#include "Scene/Mesh.h" #include "ui/ControlPanel.h" -#include "Robots/RobotSystem.h" -#include "Platform/Paths.h" -#include -#include -#include "Platform/imguiWidgets.h" - -#include #ifdef __gl_h_ #undef __gl_h_ @@ -18,6 +9,25 @@ #include #include +#include "Scene/Mesh.h" +#include "Scene/Object.h" +#include "Scene/Light.h" +#include "Scene/Camera.h" + +#include "Scene/SimulationManager.h" +#include "Physics/PhysicsSystem.h" +#include "Robots/RobotSystem.h" +#include "Interpreter/Parser.h" + +#include "Analysis/Telemetry.h" + +#include "Platform/Paths.h" +#include +#include +#include "Platform/imguiWidgets.h" +#include +#include + #include "EngineLib/LogMacros.h" namespace gui { @@ -294,22 +304,22 @@ namespace gui { } void ControlPanel::render(SimManager* sceneView) { - // Initialize pointers to scene scene - _sim = sceneView; + _sim = sceneView; // update internal pointer to current SimManager + pollComparisonFutures(); // poll any pending comparison results and update state accordingly fov = _sim->getCamera()->getFOVRadians(); + + // Update pointers to current scene data _mesh = _sim->getMesh(); _obj = _sim->getObject(); _light = _sim->getLight(); _hasRobot = _sim->hasRobot(); - _phys = _sim->physicsSystem(); + // Begin Control Panel Window ImGui::SetNextWindowPos(ImVec2(10, 10), ImGuiCond_FirstUseEver); ImGui::SetNextWindowSize(ImVec2(300, 400), ImGuiCond_FirstUseEver); - ImGui::PushStyleColor(ImGuiCol_WindowBg, ImVec4(0.129f, 0.129f, 0.129f, 0.8f)); ImGui::Begin("Control Panel", nullptr, ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoTitleBar); - const bool wasRunning = _sim->isSimRunning(); // snapshot // Simulation Start/Stop Button @@ -319,10 +329,10 @@ namespace gui { D_RUNTIME_ONCE("Simulation %s", _sim->isSimRunning() ? "started" : "stopped"); } - beginControlPanel("ControlPanel"); // Begin Child Panel - - roboticArmSelector(); + beginControlPanel("ControlPanel"); // Begin Decorative Child Panel + // Tab bar for organizing control sections + roboticArmSelector(); // Draw robotic arm selector (if enabled) if (ImGui::BeginTabBar("ControlPanelTabs")) { if (ImGui::BeginTabItem("Rigid Body Properties")) { simulationProperties(); @@ -342,8 +352,7 @@ namespace gui { } ImGui::EndTabBar(); } - - endControlPanel(); // End Child Panel + endControlPanel(); // End Decorative Child Panel ImGui::End(); ImGui::PopStyleColor(); @@ -364,10 +373,9 @@ namespace gui { } _simWasRunningLastFrame = runningNow; } + drawResultsWindow(); // Draw separate results window (if open) - // Draw separate results window (if open) - drawResultsWindow(); - + // Handle file dialogs for mesh loading _meshLoad.Display(); if (_meshLoad.HasSelected()) { auto file_path = _meshLoad.GetSelected().string(); @@ -378,7 +386,7 @@ namespace gui { _meshLoad.ClearSelected(); } - + // Handle file dialog for HDR loading _hdrLoad.Display(); if (_hdrLoad.HasSelected()) { auto file_path = _hdrLoad.GetSelected().string(); @@ -390,7 +398,7 @@ namespace gui { } } - + // Temporary light controls for testing and debugging void ControlPanel::tempLightControls() { if (!_light) return; ImGui::SeparatorText("Light Settings:"); @@ -411,6 +419,7 @@ namespace gui { ImGui::Separator(); } + // Simulation properties controls, including integration method, torque mode, and delta time settings void ControlPanel::simulationProperties() { ImGui::SectionHeader("Simulation Settings"); ImGui::SectionDivider(); @@ -639,19 +648,22 @@ namespace gui { ImGui::TextDisabled("Robot has no joints."); return; } - + // Display joint properties header float minDamping = 0.0; float maxDamping = 1.0; // damping limits float minFriction = 0.0; float maxFriction = 10.0; // friction limits double minGravity = 0.0; double maxGravity = 100.0; // gravity limits + // Clamp current joint index to valid range to prevent out-of-bounds access static int currentJointIndex = 0; currentJointIndex = std::clamp(currentJointIndex, 0, (int)joints.size() - 1); + // Get references to currently selected joint and its corresponding link auto& j = joints[currentJointIndex]; int linkIndex = currentJointIndex; linkIndex = std::clamp(linkIndex, 0, (int)links.size() - 1); auto& L = links[linkIndex]; + // Copy joint properties to local variables for editing float c = (float)j.dynamics.damping; float f = (float)j.dynamics.friction; double g = (double)robot->getGravity(); @@ -660,52 +672,52 @@ namespace gui { ImGui::BeginDisabled(_sim->isSimRunning()); ImGui::Text("Joint Dynamics:"); - + // Damping controls ImGui::Text("Damping:"); ImGui::SetNextItemWidth(150.0f); if (ImGui::DragFloat("kg/s##damp", &c, 0.001f, minDamping, maxDamping)) { j.dynamics.damping = c; } ImGui::Spacing(); - + // Friction controls ImGui::Text("Friction:"); ImGui::SetNextItemWidth(150.0f); if (ImGui::DragFloat("##fric", &f, 0.001f, minFriction, maxFriction)) { j.dynamics.friction = f; } ImGui::Spacing(); + // Gravity controls with preset menu ImGui::Text("Gravity:"); - - static GravityPreset gravityPreset = PRESET_EARTH; + static GravityPreset gravityPreset = PRESET_MICRO_G; // default preset + // Draw gravity preset menu and update gravity value based on selection drawGravityChoiceMenu(gravityPreset, g); - + // If in custom gravity mode, allow direct editing of gravity value if (gravityMode == GravityUIMode::Custom) { ImGui::SetNextItemWidth(150.0f); if (ImGui::DragScalar("m/s²##g", ImGuiDataType_Double, &g, 0.005f, &minGravity, &maxGravity)) { robot->setGravity(g); } } - else { - robot->setGravity(g); - } + else { robot->setGravity(g); } ImGui::TextDisabled("Gravity: %.3f", g); ImGui::Spacing(); ImGui::Separator(); - ImGui::EndDisabled(); + // Telemetry inspector for currently selected joint ImGui::SectionHeader("Joint Telemetry:"); - + // Draw telemetry inspector for the selected joint, showing its state over time drawTrajectoryInspector(rec, (int)robot->joints().size(), _selection.index); ImGui::TextDisabled("Selected Joint: %s - Child Link: %s", j.name.c_str(), L.name.c_str()); ImGui::Spacing(); - + // Reset joint properties to initial conditions ImGui::Spacing(); ImGui::Text("Reset Robot:"); ImGui::Spacing(); + // Reset button to reset the entire robot to its initial state, including all joints and links + ImGui::SetNextItemWidth(150.0f); if (ImGui::Button("Reset")) { if (!_hasRobot) { LOG_WARN("No robot selected to reset."); return; } // should not happen - _sim->robotSystem()->resetRobot(); // Clear selection @@ -1269,8 +1281,8 @@ namespace gui { // --- Integrator Comparison --- // Run the loaded script with all integrators and capture telemetry for comparison plots - void ControlPanel::runComparisonAllIntegrators() { - const std::string& scriptText = _sim->lastScriptText(); + void ControlPanel::runComparisonAllIntegratorsAsync() { + const std::string scriptText = _sim->lastScriptText(); if (scriptText.empty()) { D_FAIL("No script loaded. Run a script first, then compare."); return; @@ -1290,54 +1302,144 @@ namespace gui { }; static const char* names[] = { "Euler", "Midpoint", "Heun", "Ralston", "RK4", "RK45" }; - _comparisonResults.clear(); - _comparisonReady = false; + // Clear previous state + { + std::lock_guard lk(_comparisonMutex); + _comparisonResults.clear(); + _comparisonReady = false; + _comparisonFutures.clear(); + _comparisonPending = 0; + } - D_INFO("Starting integrator comparison (6 methods)..."); + D_INFO("Starting integrator comparison (parallel, %zu methods)...", (size_t)(sizeof(methods) / sizeof(methods[0]))); const size_t methodCount = sizeof(methods) / sizeof(methods[0]); - // Loop through each integrator method, run the script, and capture telemetry for comparison + // Launch one async worker per integrator for (size_t i = 0; i < methodCount; ++i) { - D_INFO(" Running: %s ...", names[i]); + const auto method = methods[i]; + const std::string methodName = names[i]; + _comparisonPending.fetch_add(1, std::memory_order_relaxed); + + // Capture by value: scriptText and methodName + _comparisonFutures.emplace_back(std::async(std::launch::async, [scriptText, method, methodName]() -> ComparisonSnapshot { + ComparisonSnapshot snap; + snap.integratorName = methodName; + try { + // Create fresh simulation core local to this worker + core::ISimulationCore* raw = CreateSimulationCore_v1(); + if (!raw) { D_FAIL("Worker: failed to CreateSimulationCore_v1 for %s", methodName.c_str()); return snap; } + CorePtr core(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); + + // Program and parser bound to new core + auto program = std::make_unique(core.get()); + interpreter::Parser parser(program.get()); + parser.parse(scriptText); + program->start(); + + // Run to completion with this integrator + bool ok = core->runScriptToCompletion(program.get(), method); + if (!ok) { + D_WARN("Worker: integrator %s returned failure.", methodName.c_str()); + // still try to gather telemetry if any + } - if (!_sim->runScriptToCompletion(scriptText, methods[i])) { - D_FAIL(" %s failed — skipping.", names[i]); - continue; - } + // Snapshot telemetry from the worker core + const auto& ring = core->telemetry().ring; + if (ring.size() < 2) { + // nothing to record + return snap; + } - // Snapshot telemetry into comparison result - const auto& ring = _sim->telemetry().ring; - const size_t sampleCount = ring.size(); - - // Need at least 2 samples to plot error over time - if (sampleCount < 2) { continue; } - - // Create snapshot for this integrator - ComparisonSnapshot snap; - snap.integratorName = names[i]; - snap.jointCount = ring.at(sampleCount - 1).j.size(); - - snap.time.resize(sampleCount); - snap.errRms.resize(sampleCount); - snap.errMax.resize(sampleCount); - snap.jointErr.resize(snap.jointCount); - for (size_t j = 0; j < snap.jointCount; ++j) { snap.jointErr[j].resize(sampleCount); } - - for (size_t k = 0; k < sampleCount; ++k) { - const auto& s = ring.at(k); - snap.time[k] = (float)s.timeSec; - snap.errRms[k] = (float)s.err_rms; - snap.errMax[k] = (float)s.err_max; - const size_t m = std::min(snap.jointCount, s.j.size()); - for (size_t j = 0; j < m; ++j) { - snap.jointErr[j][k] = (float)(s.j[j].q_ref - s.j[j].q); + const size_t sampleCount = ring.size(); + snap.time.assign(sampleCount, 0.0f); + snap.errRms.assign(sampleCount, 0.0f); + snap.errMax.assign(sampleCount, 0.0f); + + // infer joint count from last sample + size_t jointCount = ring.back().j.size(); + snap.jointCount = (int)jointCount; + snap.jointErr.resize(jointCount); + for (size_t j = 0; j < jointCount; ++j) snap.jointErr[j].assign(sampleCount, 0.0f); + + for (size_t k = 0; k < sampleCount; ++k) { + const auto& s = ring[k]; + snap.time[k] = (float)s.timeSec; + snap.errRms[k] = (float)s.err_rms; + snap.errMax[k] = (float)s.err_max; + const size_t m = std::min(jointCount, s.j.size()); + for (size_t j = 0; j < m; ++j) { + snap.jointErr[j][k] = (float)(s.j[j].q_ref - s.j[j].q); + } + } + + // optional: record integrator name already set + return snap; + } + catch (const std::exception& ex) { + D_FAIL("Worker exception for %s: %s", methodName.c_str(), ex.what()); + return snap; } + catch (...) { + D_FAIL("Worker unknown exception for %s", methodName.c_str()); + return snap; + } + })); + } + // Results are polled by pollComparisonFutures() + } + + // Poll the async comparison workers for completion, gather results, and mark ready when all done + void ControlPanel::pollComparisonFutures() { + if (_comparisonFutures.empty()) return; + // Check all futures for completion, gather results, and remove completed ones from the list + for (auto it = _comparisonFutures.begin(); it != _comparisonFutures.end();) { + std::future& fut = *it; + if (fut.valid() && fut.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) { + ComparisonSnapshot snap; + try { snap = fut.get(); } + catch (const std::exception& e) { D_FAIL("pollComparisonFutures: future.get() threw: %s", e.what()); } + catch (...) { D_FAIL("pollComparisonFutures: unknown exception from future.get()"); } + // If snapshot has data, add to results for plotting + { + std::lock_guard lk(_comparisonMutex); + _comparisonResults.push_back(std::move(snap)); + } + // Remove this future from the list + it = _comparisonFutures.erase(it); + _comparisonPending.fetch_sub(1, std::memory_order_relaxed); + } + else ++it; + } + + // When all workers finished, mark ready so UI plotting code can run + if (_comparisonFutures.empty()) { + // sort results into canonical integrator order so UI & CSV are deterministic + if (!_comparisonResults.empty()) { + // canonical order for display + static const std::vector canonical = { "Euler", "Midpoint", "Heun", "Ralston", "RK4", "RK45" }; + std::unordered_map order; + for (int i = 0; i < (int)canonical.size(); ++i) order[canonical[i]] = i; + // sort by canonical order if both integrators are known, otherwise keep order of completion + std::stable_sort(_comparisonResults.begin(), _comparisonResults.end(), + [&order](const ComparisonSnapshot& a, const ComparisonSnapshot& b) { + auto ia = order.find(a.integratorName); + auto ib = order.find(b.integratorName); + if (ia == order.end() && ib == order.end()) return false; + if (ia == order.end()) return false; + if (ib == order.end()) return true; + return ia->second < ib->second; + }); + // mark ready for plotting + _comparisonReady = true; + D_INFO("Integrator comparison complete: %d methods captured.", (int)_comparisonResults.size()); + } + // if no results, still mark ready to avoid indefinite "loading" state in UI + else { + _comparisonReady = true; + D_WARN("Integrator comparison complete: no usable results."); } - _comparisonResults.push_back(std::move(snap)); } - _comparisonReady = !_comparisonResults.empty(); - D_SUCCESS("Integrator comparison complete: %d/%d methods captured.", (int)_comparisonResults.size(), 6); } // Draw comparison plots (overlays of all integrators) @@ -1665,9 +1767,7 @@ namespace gui { const bool hasScript = !_sim->lastScriptText().empty(); ImGui::BeginDisabled(!hasScript || _sim->isSimRunning()); - if (ImGui::Button("Compare All Integrators")) { - runComparisonAllIntegrators(); - } + if (ImGui::Button("Compare All Integrators")) { runComparisonAllIntegratorsAsync(); } ImGui::EndDisabled(); ImGui::SameLine(); ImGui::TextDisabled("Runs Euler/Midpoint/Heun/Ralston/RK4/RK45 sequentially"); From c893a88277670945b88642cc3931ccdf528fa945 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 22 Feb 2026 15:55:26 +0000 Subject: [PATCH 14/20] WIP: batch study and background run support in GUI NOTES: - Added StudyRunner to SimManager for batch/background studies. - Introduced thread-safe result handling and integration method querying in ISimulationCore. - Refactored CommandScriptEditor to use StudyRunner for background runs. - Tweaked telemetry and comparison results collection in ControlPanel (I want to spend some time fixing the entire class honestly, but after). - "Standardised" simulation core factory usage and cleaned up declarations (I think there is a lot of work to be done here though). TODO: - Get parallelisation working with runs on gui. --- Sim_Engine/Engine/src/BatchMain.cpp | 2 +- Sim_Engine/EngineLib/include/EngineCore.h | 2 +- .../include/Platform/ISimulationCore.h | 1 + .../EngineLib/include/Scene/SimulationCore.h | 1 + .../include/Scene/SimulationManager.h | 28 ++++--- .../EngineLib/include/ui/ControlPanel.h | 42 +++++------ Sim_Engine/EngineLib/src/EngineCore.cpp | 1 + .../EngineLib/src/Scene/SimulationCore.cpp | 20 ++--- .../EngineLib/src/Scene/SimulationManager.cpp | 42 ++++++++++- .../EngineLib/src/ui/CommandScriptEditor.cpp | 70 +++++++++--------- Sim_Engine/EngineLib/src/ui/ControlPanel.cpp | 39 +++++----- .../External/MathLib/Release/MathLib.dll | Bin 61952 -> 61952 bytes 12 files changed, 137 insertions(+), 111 deletions(-) diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp index 7569090..29f44c1 100644 --- a/Sim_Engine/Engine/src/BatchMain.cpp +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -8,7 +8,6 @@ #include // ensure at top of file #include // for fprintf - // Helper: create CorePtr (unique_ptr with std::function deleter) static CorePtr makeCoreFactory() { core::ISimulationCore* raw = CreateSimulationCore_v1(); @@ -62,6 +61,7 @@ int runBatchMode() { // The script text for the run(s) std::string scriptText = R"( + load(robot, VISPA) set(integrator, rk4) start() )"; diff --git a/Sim_Engine/EngineLib/include/EngineCore.h b/Sim_Engine/EngineLib/include/EngineCore.h index c0cc0e5..7c75f68 100644 --- a/Sim_Engine/EngineLib/include/EngineCore.h +++ b/Sim_Engine/EngineLib/include/EngineCore.h @@ -14,7 +14,7 @@ #endif // Forward declarations for core components -namespace core { struct ISimulationCore; } +namespace core { struct ENGINE_API ISimulationCore; } // Factory functions extern "C" { diff --git a/Sim_Engine/EngineLib/include/Platform/ISimulationCore.h b/Sim_Engine/EngineLib/include/Platform/ISimulationCore.h index cf16cc9..d3caede 100644 --- a/Sim_Engine/EngineLib/include/Platform/ISimulationCore.h +++ b/Sim_Engine/EngineLib/include/Platform/ISimulationCore.h @@ -32,6 +32,7 @@ namespace core { // Integrator virtual void setIntegrationMethod(integration::eIntegrationMethod method) = 0; virtual std::string integrationMethodName() const = 0; + virtual integration::eIntegrationMethod integrationMethod() const = 0; // Subsystems virtual physics::PhysicsSystem* physicsSystem() = 0; virtual robots::RobotSystem* robotSystem() = 0; diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h index daff98d..8a5b2c6 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h @@ -45,6 +45,7 @@ namespace core { void setupSimulationIntegrator(); void setIntegrationMethod(integration::eIntegrationMethod method) override; std::string integrationMethodName() const override; + integration::eIntegrationMethod integrationMethod() const override; // Subsystems access physics::PhysicsSystem* physicsSystem() override; diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h index fa65464..6acb4d9 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationManager.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationManager.h @@ -55,15 +55,15 @@ namespace gui { // Forward Declarations for Axis Orientator class ENGINE_API AxisOrientator; + // Control Modes & Camera + enum class ControlMode { + Camera, + Object + }; + // SimManager Class (Plan on renaming later) class ENGINE_API SimManager { public: - // Called by background worker threads (via CommandScriptEditor) to hand finished results to the manager - void pushCompletedRun(StudyResult result); - - // Get and clear completed runs for display (GUI calls this) - std::vector takeCompletedRuns(); - // Constructor & Destructor SimManager(); ~SimManager(); @@ -109,11 +109,6 @@ namespace gui { // Getter plane height (y=0 plane for physics and object placement) float getPlaneHeight() const { return planeHeight; } - // Control Modes & Camera - enum class ControlMode { - Camera, - Object - }; ControlMode ctrlMode = ControlMode::Camera; // Setter and getter for control mode @@ -253,6 +248,15 @@ namespace gui { core::SimulationCore* simCore(); const core::SimulationCore* simCore() const; + // Access to the underlying StudyRunner for running batch studies from the GUI + StudyRunner* studyRunner() { return _studyRunner.get(); } + + // Methods for handling completed studies from the background worker + void pushCompletedStudies(std::vector results); + void pushCompletedStudy(StudyResult result); + bool hasCompletedStudy() const; + std::vector consumeCompletedStudy(); + // Input Handling void processMovementKey(int key, float delta); void handleContinuousMovement(GLFWwindow* window, float dt); @@ -263,6 +267,8 @@ namespace gui { private: std::unique_ptr _core = nullptr; + std::unique_ptr _studyRunner = nullptr; // Background worker for running batch studies + bool _hasCompletedStudy = false; // Rendering Pipeline Methods void MeshRender(scene::Camera* cam); diff --git a/Sim_Engine/EngineLib/include/ui/ControlPanel.h b/Sim_Engine/EngineLib/include/ui/ControlPanel.h index 17ae0e4..389e393 100644 --- a/Sim_Engine/EngineLib/include/ui/ControlPanel.h +++ b/Sim_Engine/EngineLib/include/ui/ControlPanel.h @@ -8,6 +8,10 @@ #include #include +#include +#include +#include + #include "Platform/SimulationState.h" #include "Platform/Logger.h" @@ -17,13 +21,20 @@ namespace scene { class ENGINE_API Object; class ENGINE_API Light; class ENGINE_API Camera; - class ENGINE_API SimManager; +} +namespace render { + enum class ResolutionPreset; + enum class QualityPreset; } namespace physics { class ENGINE_API PhysicsSystem; } namespace robots { class ENGINE_API RobotSystem; } namespace diagnostics { class ENGINE_API TelemetryRecorder; } namespace gui { + // Forward Declaration for SimManager + class ENGINE_API SimManager; + enum class ControlMode; + // Gravity UI Modes enum class GravityUIMode { Preset, @@ -91,23 +102,24 @@ namespace gui { private: // Comparison results management + std::vector _comparisonResults; std::vector> _comparisonFutures; std::atomic _comparisonPending{ 0 }; std::mutex _comparisonMutex; + bool _comparisonReady = false; // Internal Pointers - std::shared_ptr _mesh; - SimManager* _sim = nullptr; - physics::PhysicsSystem* _phys; - scene::Light* _light; - scene::Object* _obj; + std::shared_ptr _mesh = nullptr; + physics::PhysicsSystem* _phys = nullptr; + scene::Light* _light = nullptr; + scene::Object* _obj = nullptr; ImGui::FileBrowser _meshLoad; ImGui::FileBrowser _hdrLoad; std::string _currentMeshFile; std::string _currentHDRFile; - SimManager::ControlMode* _controlMode; + ControlMode* _controlMode; std::function meshLoadCallback; std::function simCallback; @@ -152,8 +164,8 @@ namespace gui { bool scrollToBottom = false; // Render Presets - render::ResolutionPreset r = render::ResolutionPreset::R_1080p; - render::QualityPreset q = render::QualityPreset::Medium; + render::ResolutionPreset r; + render::QualityPreset q; bool _qualityChanged = false; bool _resChanged = false; @@ -172,18 +184,6 @@ namespace gui { bool _resultsFocusNeeded = false; bool _selectResultsTab = false; - // Integrator comparison state - struct ComparisonSnapshot { - std::string integratorName; - std::vector time; - std::vector errRms; - std::vector errMax; - std::vector> jointErr; - size_t jointCount = 0; - }; - std::vector _comparisonResults; - bool _comparisonReady = false; - // Currently selected items std::string _requestedRobot; // name of requested robot to load std::string _currentObjectName; // name of currently selected object diff --git a/Sim_Engine/EngineLib/src/EngineCore.cpp b/Sim_Engine/EngineLib/src/EngineCore.cpp index 5143681..1f494c0 100644 --- a/Sim_Engine/EngineLib/src/EngineCore.cpp +++ b/Sim_Engine/EngineLib/src/EngineCore.cpp @@ -4,6 +4,7 @@ #include "EngineCore.h" #include "Scene/SimulationCore.h" // contains core::SimulationCore and core::ISimulationCore +// Factory function definitions extern "C" { // Factory function to create a SimulationCore instance ENGINE_API core::ISimulationCore* CreateSimulationCore_v1() { diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index ff43e72..57ec94b 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -46,6 +46,11 @@ namespace core { if (!_robot) { return "no_robot"; } return _robot->getIntegrator()->IntegratorName(_robot->getIntegrator()->getIntegrationMethod()); } + // Get the current integration method + integration::eIntegrationMethod SimulationCore::integrationMethod() const { + if (!_robot) { return integration::eIntegrationMethod::RK4; } + return _robot->getIntegrator()->getIntegrationMethod(); + } // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { @@ -487,17 +492,4 @@ namespace core { // Setter and getter for the active script program void SimulationCore::setActiveProgram(interpreter::IStoredProgram* p) { _activeProgram = p; } interpreter::IStoredProgram* SimulationCore::activeProgram() const { return _activeProgram; } -} - -// DLL Exported factory Functions -extern "C" { - // Create function - __declspec(dllexport) core::ISimulationCore* CreateSimulationCore_v1() { - try { return new core::SimulationCore(); } - catch (...) { return nullptr; } - } - // Delete function - __declspec(dllexport) void DestroySimulationCore(core::ISimulationCore* p) { - delete p; // free inside DLL - } -} // extern "C" \ No newline at end of file +} \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp index 6398e5b..345f1ae 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationManager.cpp @@ -5,6 +5,9 @@ #include "Scene/SimulationManager.h" #include "Scene/SimulationCore.h" +extern "C" core::ISimulationCore* CreateSimulationCore_v1(); +extern "C" void DestroySimulationCore(core::ISimulationCore*); + #ifdef __gl_h_ #undef __gl_h_ #endif @@ -573,12 +576,23 @@ namespace gui { } }; + // Helper: create CorePtr (unique_ptr with std::function deleter) + static CorePtr makeCoreFactory() { + core::ISimulationCore* raw = CreateSimulationCore_v1(); + if (!raw) { + return CorePtr(nullptr, [](core::ISimulationCore*) {}); + } + // std::function deleter is constructed from the lambda implicitly + return CorePtr(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); + } + // -------------------------------------------------- // CONSTRUCTOR & DESTRUCTOR // -------------------------------------------------- SimManager::SimManager() : _internalSize(1920, 1080), _displaySize(1.0f, 1.0f), _backgroundColour(0.18f, 0.18f, 0.20f), - _backgroundAlpha(1.0f), _impl(std::make_unique(*this)), _core(std::make_unique()) { + _backgroundAlpha(1.0f), _impl(std::make_unique(*this)), _core(std::make_unique()), + _studyRunner(std::make_unique(makeCoreFactory, std::thread::hardware_concurrency() > 1 ? std::thread::hardware_concurrency() - 1 : 1)) { _core->setPhysicsSystem(_impl->_physics.get()); _core->setRobotSystem(_impl->_robotSystem.get()); _core->setObjects(&_impl->_objects); @@ -627,21 +641,32 @@ namespace gui { // -------------------------------------------------- // THREAD-SAFE SIMULATION RESULTS // -------------------------------------------------- - + // Add a completed simulation run to the list in a thread-safe manner - void SimManager::pushCompletedRun(StudyResult result) { + void SimManager::pushCompletedStudies(std::vector results) { + if (!_impl) { return; } + std::lock_guard lk(_impl->_completedRunsMutex); + _impl->_completedRuns.insert(_impl->_completedRuns.end(), results.begin(), results.end()); + _hasCompletedStudy = true; + } + // Add a completed simulation run to the list in a thread-safe manner + void SimManager::pushCompletedStudy(StudyResult result) { if (!_impl) { return; } std::lock_guard lk(_impl->_completedRunsMutex); _impl->_completedRuns.push_back(std::move(result)); + _hasCompletedStudy = true; } + bool SimManager::hasCompletedStudy() const { return _hasCompletedStudy; } + // Retrieve and clear completed runs in a thread-safe manner - std::vector SimManager::takeCompletedRuns() { + std::vector SimManager::consumeCompletedStudy() { std::vector copy; if (!_impl) { return copy; } std::lock_guard lk(_impl->_completedRunsMutex); copy = std::move(_impl->_completedRuns); _impl->_completedRuns.clear(); + _hasCompletedStudy = false; return copy; } @@ -932,6 +957,15 @@ namespace gui { // Main render function called by the application void SimManager::render() { + if (hasCompletedStudy()) { + auto results = consumeCompletedStudy(); + + for (const auto& r : results) { + LOG_INFO("Study completed: %s", r.tag.c_str()); + // TODO: update plots, telemetry graphs, UI panels here + } + } + ImGuiIO& io = ImGui::GetIO(); _core->tick(io.DeltaTime); _fpsCounter.update(); diff --git a/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp b/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp index dfb7bf5..dc9ccf8 100644 --- a/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp +++ b/Sim_Engine/EngineLib/src/ui/CommandScriptEditor.cpp @@ -158,6 +158,14 @@ namespace gui { _wrapper->runProgram(_scriptText); } } + ImGui::SameLine(); + // Background run button + if (ImGui::Button("Run (background)")) { + std::string code = _scriptText; + if (!code.empty() && code.back() == '\0') { code.pop_back(); } + std::string tag = filenameOnly(_currentScriptFile); + launchBackgroundRun(code, tag); + } // Pop button style colors if we pushed them if (wasRunning) { ImGui::PopStyleColor(3); } // Check script status and handle termination conditions @@ -172,43 +180,31 @@ namespace gui { } } + // Handler for the Run button -> launches the script in a background thread using the StudyRunner void CommandScriptEditor::runButtonHandler() { - if (ImGui::Button("Run (background)")) { - // snapshot script text & tag - std::string code = _scriptText; - if (!code.empty() && code.back() == '\0') code.pop_back(); - std::string tag = filenameOnly(_currentScriptFile); - - // launch async task (by-value capture) - std::future f = std::async(std::launch::async, [code, tag]() -> StudyResult { - StudyResult r{}; - r.tag = tag; - // create a fresh sim core - core::ISimulationCore* raw = CreateSimulationCore_v1(); - if (!raw) { r.success = false; return r; } - CorePtr core(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); - - // create program+parser bound to new core - auto program = std::make_unique(core.get()); - interpreter::Parser parser(program.get()); - parser.parse(code); - program->start(); - - // run to completion (blocks inside worker thread) - bool ok = core->runScriptToCompletion(program.get(), integration::eIntegrationMethod::RK4 /*or read from script*/); - - // gather result (you can use core->telemetry() etc to fill more fields) - r.success = ok; - r.intName = "background"; r.simTime = core->simTime(); - return r; - }); - - // register active run - { - std::lock_guard lk(_activeRunsMutex); - _activeRuns.push_back(ActiveRun{ std::move(f), tag }); - } - } + if (!ImGui::Button("Run (background)")) { return; } + + // Snapshot script text + std::string code = _scriptText; + if (!code.empty() && code.back() == '\0') { code.pop_back(); } + std::string tag = filenameOnly(_currentScriptFile); + + // Build single config (or multiple if desired) + std::vector configs; + StudyRunner::config c; + + // Read integration method and dt from the sim core (defaults will be used if not set in the sim core) + c.method = _sim->simCoreInterface()->integrationMethod(); + c.dt = _sim->simCoreInterface()->fixedDt(); + c.len_min = 1.0; + c.tag = tag; + configs.push_back(c); + + // Launch background thread + std::thread([this, configs, code]() { + auto results = _sim->studyRunner()->runStudies(configs, code); + _sim->pushCompletedStudies(std::move(results)); + }).detach(); } void CommandScriptEditor::terminateScript(const char* reason, bool fault) { @@ -384,7 +380,7 @@ namespace gui { r.tag = ar.tag; } // forward to SimManager (thread-safe push) - if (_sim) { _sim->pushCompletedRun(std::move(r)); } + if (_sim) { _sim->pushCompletedStudy(std::move(r)); } it = _activeRuns.erase(it); } else { diff --git a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp index c2432eb..da75b1d 100644 --- a/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp +++ b/Sim_Engine/EngineLib/src/ui/ControlPanel.cpp @@ -19,15 +19,11 @@ #include "Robots/RobotSystem.h" #include "Interpreter/Parser.h" -#include "Analysis/Telemetry.h" - -#include "Platform/Paths.h" -#include -#include #include "Platform/imguiWidgets.h" -#include #include +#include "Analysis/Telemetry.h" +#include "Platform/Paths.h" #include "EngineLib/LogMacros.h" namespace gui { @@ -208,8 +204,9 @@ namespace gui { // --- ControlPanel Implementation --- - ControlPanel::ControlPanel(SimManager* sceneView) : - _sim(sceneView), _controlMode(&sceneView->ctrlMode), _phys(nullptr), _obj(nullptr), _light(nullptr), + ControlPanel::ControlPanel(SimManager* sim) : + _sim(sim), _controlMode(&sim->ctrlMode), _phys(nullptr), _obj(nullptr), _light(nullptr), + r(render::ResolutionPreset::R_1080p), q(render::QualityPreset::Medium), _meshLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal), _hdrLoad(ImGuiFileBrowserFlags_CloseOnEsc | ImGuiFileBrowserFlags_NoModal) { @@ -1346,10 +1343,7 @@ namespace gui { // Snapshot telemetry from the worker core const auto& ring = core->telemetry().ring; - if (ring.size() < 2) { - // nothing to record - return snap; - } + if (ring.size() < 2) { return snap; } const size_t sampleCount = ring.size(); snap.time.assign(sampleCount, 0.0f); @@ -1357,13 +1351,13 @@ namespace gui { snap.errMax.assign(sampleCount, 0.0f); // infer joint count from last sample - size_t jointCount = ring.back().j.size(); + size_t jointCount = ring.at(ring.size() - 1).j.size(); snap.jointCount = (int)jointCount; snap.jointErr.resize(jointCount); for (size_t j = 0; j < jointCount; ++j) snap.jointErr[j].assign(sampleCount, 0.0f); for (size_t k = 0; k < sampleCount; ++k) { - const auto& s = ring[k]; + const auto& s = ring.at(k); snap.time[k] = (float)s.timeSec; snap.errRms[k] = (float)s.err_rms; snap.errMax[k] = (float)s.err_max; @@ -1394,17 +1388,18 @@ namespace gui { if (_comparisonFutures.empty()) return; // Check all futures for completion, gather results, and remove completed ones from the list for (auto it = _comparisonFutures.begin(); it != _comparisonFutures.end();) { - std::future& fut = *it; + auto& fut = *it; if (fut.valid() && fut.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) { - ComparisonSnapshot snap; - try { snap = fut.get(); } + try { + auto snap = fut.get(); + // If snapshot has data, add to results for plotting + { + std::lock_guard lk(_comparisonMutex); + _comparisonResults.emplace_back(std::move(snap)); + } + } catch (const std::exception& e) { D_FAIL("pollComparisonFutures: future.get() threw: %s", e.what()); } catch (...) { D_FAIL("pollComparisonFutures: unknown exception from future.get()"); } - // If snapshot has data, add to results for plotting - { - std::lock_guard lk(_comparisonMutex); - _comparisonResults.push_back(std::move(snap)); - } // Remove this future from the list it = _comparisonFutures.erase(it); _comparisonPending.fetch_sub(1, std::memory_order_relaxed); diff --git a/Sim_Engine/External/MathLib/Release/MathLib.dll b/Sim_Engine/External/MathLib/Release/MathLib.dll index 3e5905e996a318c7e67c21956eb975804c87fd69..b630b0c9cdbe8f2baef6ff51602b46bc40e07022 100644 GIT binary patch delta 113 zcmZp8!rbtLd4m8W^Ae@mn}r!aF0ckNG8uq?35aum*k?ZwuLOxOFu>$^fNT*U-T{=` pfsmVQzeo~nQ1qg&Yy$bU)44Y-nB%l^f64CXj&pA}C#+oH0RX{-BP9R; delta 113 zcmZp8!rbtLd4m8W^RJXyn}r!aF0ckNG8uq?35aum*k?ZwuLOxOFu>$^fNT*U-T{=` pfsmVQzeo~nQ1qg&Yyy#HyLS8gE_I!6{K8^m#-`_+6IL$p004RiB~t(Z From 706c4ab2a9d89d51e679e2d9aa338ce080351546 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 22 Feb 2026 17:28:23 +0000 Subject: [PATCH 15/20] refactor: refactored SimulationCore to use unique_ptr for core systems NOTES: - Refactored SimulationCore to manage RobotSystem, PhysicsSystem, TrajectoryManager, and object container with std::unique_ptr for improved memory safety and automatic cleanup. - Updated factory and setter/getter methods accordingly (just to allow for batch runs using these systems). - Added ENGINE_API to paths functions for DLL compatibility. - BatchMain.cpp now initialises paths and includes expanded batch script and debug logging. TODO: - Fixed HDF5 to be thread-safe, it currently is not. - Run tests, and finish report, I HAVE LOTS OF NOTES DISCLAIMER: - Some changes in this commit were assisted using AI, as I am finding this interesting but also difficult, NOTES HAVE BEEN MADE SO I CAN LEARN :) --- Sim_Engine/Engine/src/BatchMain.cpp | 55 ++++++++++++++++-- Sim_Engine/EngineLib/include/Platform/Paths.h | 16 ++--- .../EngineLib/include/Scene/SimulationCore.h | 13 +++-- Sim_Engine/EngineLib/src/EngineCore.cpp | 34 ++++++++++- .../EngineLib/src/Scene/SimulationCore.cpp | 30 +++++----- .../External/MathLib/Release/MathLib.dll | Bin 61952 -> 61952 bytes 6 files changed, 113 insertions(+), 35 deletions(-) diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp index 29f44c1..08c4527 100644 --- a/Sim_Engine/Engine/src/BatchMain.cpp +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -4,21 +4,35 @@ #include "include/BatchEntry.h" #include "Platform/StudyRunner.h" #include "Numerics/IntegrationMethods.h" +#include "Platform/Paths.h" #include // ensure at top of file #include // for fprintf // Helper: create CorePtr (unique_ptr with std::function deleter) static CorePtr makeCoreFactory() { + // Create a raw pointer to ISimulationCore using the factory function from EngineCore core::ISimulationCore* raw = CreateSimulationCore_v1(); - if (!raw) { - return CorePtr(nullptr, [](core::ISimulationCore*) {}); - } - // std::function deleter is constructed from the lambda implicitly + if (!raw) { return CorePtr(nullptr, [](core::ISimulationCore*) {}); } + + // Robot System check + std::cout << "[BATCH DEBUG] Core created: " << raw << "\n"; + if (raw->robotSystem() == nullptr) { std::cout << "[BATCH DEBUG] robotSystem() is NULL\n"; } + else { std::cout << "[BATCH DEBUG] robotSystem() OK\n"; } + + // Physics System check + if (raw->physicsSystem() == nullptr) { std::cout << "[BATCH DEBUG] physicsSystem() is NULL\n"; } + else { std::cout << "[BATCH DEBUG] physicsSystem() OK\n"; } + + // Construct CorePtr with custom deleter that calls DestroySimulationCore return CorePtr(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); } int runBatchMode() { + // initialises paths (required for loading robots, and any other file access in the core) + paths::init(); + + // Log batch mode entry fprintf(stdout, "BATCH MODE ENTERED\n"); fflush(stdout); try { @@ -56,6 +70,8 @@ int runBatchMode() { #ifndef _BATCH_MODE_ONLY workers = (cores > 1) ? cores - 1 : 1; #endif + assert(core->robotSystem() != nullptr); + // Build runner with factory and worker count StudyRunner runner(makeCoreFactory, workers); @@ -63,7 +79,38 @@ int runBatchMode() { std::string scriptText = R"( load(robot, VISPA) set(integrator, rk4) + + wait(2.5) + trajClear() + wait(0.25) + start() + + parallel(5.0) { + trajSet(link01, TRAP, -60.0, 45.0, 110.0) + trajSet(link02, TRAP, 50.0, 40.0, 100.0) + trajSet(link03, TRAP, -55.0, 45.0, 110.0) + trajSet(link04, TRAP, 40.0, 55.0, 130.0) + trajSet(link05, TRAP, -50.0, 60.0, 140.0) + trajSet(link06, TRAP, 60.0, 65.0, 150.0) + } + + wait(5.0) + + parallel(5.0) { + trajSet(link01, TRAP, 65.0, 55.0, 140.0) + trajSet(link02, TRAP, -55.0, 50.0, 130.0) + trajSet(link03, TRAP, 60.0, 55.0, 140.0) + trajSet(link04, TRAP, -75.0, 65.0, 160.0) + trajSet(link05, TRAP, 65.0, 70.0, 170.0) + trajSet(link06, TRAP, -60.0, 75.0, 180.0) + } + + wait(55.0) + trajClear() + wait(0.25) + + stop() )"; // Run the studies and time the total batch duration diff --git a/Sim_Engine/EngineLib/include/Platform/Paths.h b/Sim_Engine/EngineLib/include/Platform/Paths.h index d2926a0..c262648 100644 --- a/Sim_Engine/EngineLib/include/Platform/Paths.h +++ b/Sim_Engine/EngineLib/include/Platform/Paths.h @@ -1,15 +1,17 @@ #pragma once // File: Paths.h // GitHub: SaltyJoss +#include "EngineCore.h" #include #include namespace paths { - void init(); - - const std::filesystem::path& root(); - const std::filesystem::path& assets(); - const std::filesystem::path& configs(); - const std::filesystem::path& logs(); - const std::filesystem::path& runs(); + /// Initialises the paths system + ENGINE_API void init(); + // Accessors for the various paths used by the application + ENGINE_API const std::filesystem::path& root(); + ENGINE_API const std::filesystem::path& assets(); + ENGINE_API const std::filesystem::path& configs(); + ENGINE_API const std::filesystem::path& logs(); + ENGINE_API const std::filesystem::path& runs(); } // namespace paths \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h index 8a5b2c6..72eb3a0 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h @@ -28,6 +28,7 @@ namespace core { class ENGINE_API SimulationCore : public ISimulationCore { public: SimulationCore(); + ~SimulationCore(); // Simulation control void startSimulation() override; @@ -112,6 +113,12 @@ namespace core { interpreter::IStoredProgram* activeProgram() const; private: + // Core Systems + std::unique_ptr>> _objects; + std::unique_ptr _robot; + std::unique_ptr _physics; + std::unique_ptr _traj; + // Simulation Timing double _dt = 1.0 / 180.0; // [seconds], fixed timestep duration for physics updates double _telHz = 120.0; // [Hz], controls how often telemetry updates during simulation runs @@ -120,12 +127,6 @@ namespace core { bool _simRunning = false; // Whether the simulation loop is currently running bool _scriptRunning = false; // Whether a script is currently running - // Core Systems - robots::RobotSystem* _robot = nullptr; - physics::PhysicsSystem* _physics = nullptr; - control::TrajectoryManager* _traj = nullptr; - std::vector>* _objects = nullptr; - // Run mode eRunMode _runMode = eRunMode::Interactive; diff --git a/Sim_Engine/EngineLib/src/EngineCore.cpp b/Sim_Engine/EngineLib/src/EngineCore.cpp index 1f494c0..b26bf75 100644 --- a/Sim_Engine/EngineLib/src/EngineCore.cpp +++ b/Sim_Engine/EngineLib/src/EngineCore.cpp @@ -2,13 +2,43 @@ // File: EngineCore.cpp // GitHub: SaltyJoss #include "EngineCore.h" -#include "Scene/SimulationCore.h" // contains core::SimulationCore and core::ISimulationCore +#include "Scene/SimulationCore.h" + +#include "Physics/PhysicsSystem.h" +#include "Robots/RobotSystem.h" +#include "Robots/TrajectoryManager.h" +#include "Scene/Object.h" + +#include +#include // Factory function definitions extern "C" { // Factory function to create a SimulationCore instance ENGINE_API core::ISimulationCore* CreateSimulationCore_v1() { - try { return new core::SimulationCore(); } + try { + auto* core = new core::SimulationCore(); + + auto* physics = new physics::PhysicsSystem(); + + // Headless object container + auto* objects = new std::vector>(); + // Robot system must be constructible WITHOUT OpenGL + auto* robot = new robots::RobotSystem( + *objects, [](const std::string&) { + return std::vector{}; + } + ); + // Trajectory manager must also be constructible without OpenGL + auto* traj = new control::TrajectoryManager(); + + core->setPhysicsSystem(physics); + core->setRobotSystem(robot); + core->setObjects(objects); + core->setTrajectoryManager(traj); + + return core; + } catch (...) { return nullptr; /*Avoids throwing exceptions across C ABI boundary by return null on failure*/ } } // Destroys a SimulationCore instance diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index 57ec94b..7e4cff4 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -17,8 +17,8 @@ namespace core { // Constructor - SimulationCore::SimulationCore() { - } + SimulationCore::SimulationCore() {} + SimulationCore::~SimulationCore() = default; // Update physics for all objects in the scene using the physics system void SimulationCore::updatePhysics(double dt) { @@ -96,7 +96,7 @@ namespace core { _telemetryBegun = true; D_INFO_ONCE("Telemtry Capture Started (dt=%.6f s, simTime=%.3f s)", (1 / _telHz), _simTime); } - _telemetry.update(_simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); + _telemetry.update(_simTime, *_robot, _traj.get(), diagnostics::eTelemetryLevel::FULL); } } _accum -= _dt; // decrease accumulator by fixed timestep until we catch up to the current frame time @@ -298,8 +298,6 @@ namespace core { // ---> I have implemented this for short (<5 minute) test scripts where blocking is acceptable // ---> IT IS NOT intended for general use and WILL CAUSE THE UI TO FREEZE if used with long-running scripts bool SimulationCore::runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) { - if (!hasRobot()) { return false; } - // Map method enum to string name, purely for logging purposes static const char* names[] = { "euler", "midpoint", "heun", "ralston", "rk4", "rk45" }; const std::string methodName = names[static_cast(method)]; @@ -367,7 +365,7 @@ namespace core { } // Telemetry update - _telemetry.update(_simTime, *_robot, _traj, diagnostics::eTelemetryLevel::FULL); + _telemetry.update(_simTime, *_robot, _traj.get(), diagnostics::eTelemetryLevel::FULL); } } } @@ -398,18 +396,18 @@ namespace core { // --- Setters and Getters for Systems and State --- // Setter for the physics system - void SimulationCore::setPhysicsSystem(physics::PhysicsSystem* physics) { _physics = physics; } + void SimulationCore::setPhysicsSystem(physics::PhysicsSystem* physics) { _physics.reset(physics); } // Accessor for the physics system (non-const and const versions) - physics::PhysicsSystem* SimulationCore::physicsSystem() { return _physics; } - const physics::PhysicsSystem* SimulationCore::physicsSystem() const { return _physics; } + physics::PhysicsSystem* SimulationCore::physicsSystem() { return _physics.get(); } + const physics::PhysicsSystem* SimulationCore::physicsSystem() const { return _physics.get(); } // Accessor for the robot system (non-const and const versions) - robots::RobotSystem* SimulationCore::robotSystem() { return _robot; } - const robots::RobotSystem* SimulationCore::robotSystem() const { return _robot; } + robots::RobotSystem* SimulationCore::robotSystem() { return _robot.get(); } + const robots::RobotSystem* SimulationCore::robotSystem() const { return _robot.get(); } // Setter and checker for Robot System - void SimulationCore::setRobotSystem(robots::RobotSystem* robot) { _robot = robot; } + void SimulationCore::setRobotSystem(robots::RobotSystem* robot) { _robot.reset(robot); } bool SimulationCore::hasRobot() const { return _robot && _robot->hasRobot(); } // Loads a robot into the robot system by name @@ -453,7 +451,7 @@ namespace core { } // Setter for the scene objects pointer (used for script object lookup) - void SimulationCore::setObjects(std::vector>* objects) { _objects = objects; } + void SimulationCore::setObjects(std::vector>* objects) { _objects.reset(objects); } // Getter for object scene::Object* SimulationCore::getObject() { if (!_objects) { return nullptr; } @@ -472,11 +470,11 @@ namespace core { } // Setter for the trajectory manager - void SimulationCore::setTrajectoryManager(control::TrajectoryManager* traj) { _traj = traj; } + void SimulationCore::setTrajectoryManager(control::TrajectoryManager* traj) { _traj.reset(traj); } // Accessor for the trajectory manager (non-const and const versions) - control::TrajectoryManager* SimulationCore::trajectoryManager() { return _traj; } - const control::TrajectoryManager* SimulationCore::trajectoryManager() const { return _traj; } + control::TrajectoryManager* SimulationCore::trajectoryManager() { return _traj.get(); } + const control::TrajectoryManager* SimulationCore::trajectoryManager() const { return _traj.get(); } // Setters for the metric buffers void SimulationCore::setJointLogBuffer(robots::JointLogBuffer* buf) { _jointLogBuffer = *buf; } diff --git a/Sim_Engine/External/MathLib/Release/MathLib.dll b/Sim_Engine/External/MathLib/Release/MathLib.dll index b630b0c9cdbe8f2baef6ff51602b46bc40e07022..7fc480f9d4155d3b68fee55ba83dcddd9fb59fba 100644 GIT binary patch delta 110 zcmZp8!rbtLd4m8W(|3!_LX4jlSbevcoyhIbr1j4*)c Date: Sun, 22 Feb 2026 17:38:51 +0000 Subject: [PATCH 16/20] refactor: removed singleton instance in DataManager to make logging per Simulation Core --- .../EngineLib/include/Platform/DataManager.h | 81 +------------------ .../EngineLib/include/Scene/SimulationCore.h | 3 + Sim_Engine/EngineLib/src/Application.cpp | 6 +- .../EngineLib/src/Scene/SimulationCore.cpp | 17 ++-- 4 files changed, 18 insertions(+), 89 deletions(-) diff --git a/Sim_Engine/EngineLib/include/Platform/DataManager.h b/Sim_Engine/EngineLib/include/Platform/DataManager.h index 1b46b32..e69be0d 100644 --- a/Sim_Engine/EngineLib/include/Platform/DataManager.h +++ b/Sim_Engine/EngineLib/include/Platform/DataManager.h @@ -92,93 +92,18 @@ namespace data { class ENGINE_API DataManager { public: - // Singleton instance accessor - static DataManager& instance() { - static DataManager instance; - return instance; - } - - // Enable or disable data logging + DataManager() = default; void setEnabled(bool enabled); - - // Set the current integrator name for logging void setIntegratorName(std::string name) { _integratorName = name; } - - // Set parent folder for data logging void setParentFolder(std::string folder) { _parentFolder = folder; } - - // Start data logging session void capture(Stream s, std::string_view topic, const FieldList& fields); - - // Check if data logging is enabled bool enabled() const { return _enabled; } - private: - DataManager() = default; - bool _enabled = false; - std::string _integratorName = "Unknown"; + std::string _integratorName = "Unknown"; std::string _parentFolder = "Runs"; HDF5StreamWriter _sim; HDF5StreamWriter _ref; }; -} // namespace data - -// ---macro definitions --- - -// DATA_CAPTURE_ENABLE -#ifdef DATA_CAPTURE_ENABLE -#error DATA_CAPTURE_ENABLE already defined before DataManager.h -#endif -// Enable or disable data capture -#define DATA_CAPTURE_ENABLE(b) \ - do { ::data::DataManager::instance().setEnabled((b)); } while(0) - -// SET_SIM_INTEGRATOR -#ifdef SET_SIM_INTEGRATOR -#error SET_SIM_INTEGRATOR already defined before DataManager.h -#endif -// Set the simulation integrator name for logging -#define SET_SIM_INTEGRATOR(name) \ - do { ::data::DataManager::instance().setIntegratorName((name)); } while(0) - - -// --- HDF5 Macros --- - -// HDF5_SIM_DATA -#ifdef HDF5_SIM_DATA -#error HDF5_SIM_DATA already defined before DataManager.h -#endif -// Capture simulation data as HDF5 -#define HDF5_SIM_DATA(topic, fields) \ - do { ::data::DataManager::instance().capture(::data::Stream::Simulation, (topic), (fields)); } while(0) - -// HDF5_REF_DATA -#ifdef HDF5_REF_DATA -#error HDF5_REF_DATA already defined before DataManager.h -#endif -// Capture reference data as HDF5 -#define HDF5_REF_DATA(topic, fields) \ - do { ::data::DataManager::instance().capture(::data::Stream::Reference, (topic), (fields)); } while(0) - -// --- CSV Macros --- - -// CSV_SIM_DATA -#ifdef CSV_SIM_DATA -#error CSV_SIM_DATA already defined before DataManager.h -#endif -// Capture simulation data as CSV -#define CSV_SIM_DATA(topic, fields) \ - do { ::data::DataManager::instance().capture(::data::Stream::Simulation, (topic), (fields)); } while(0) - -// CSV_REF_DATA -#ifdef CSV_REF_DATA -#error CSV_REF_DATA already defined before DataManager.h -#endif -// Capture reference data as CSV -#define CSV_REF_DATA(topic, fields) \ - do { ::data::DataManager::instance().capture(::data::Stream::Reference, (topic), (fields)); } while(0) - - -// --- end of macro definitions --- \ No newline at end of file +} // namespace data \ No newline at end of file diff --git a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h index 72eb3a0..f87fa5f 100644 --- a/Sim_Engine/EngineLib/include/Scene/SimulationCore.h +++ b/Sim_Engine/EngineLib/include/Scene/SimulationCore.h @@ -8,6 +8,7 @@ #include "Platform/SimulationState.h" #include "Analysis/Telemetry.h" #include "Analysis/MetricLogger.h" +#include "Platform/DataManager.h" #include "Platform/Logger.h" @@ -141,5 +142,7 @@ namespace core { robots::JointLogBuffer _jointLogBuffer; // Buffer for logging joint data each step robots::TrajRefBuffer _trajRefBuffer; // Buffer for logging trajectory reference data each step bool _telemetryBegun = false; + + data::DataManager _data; // Data manager for handling telemetry data export and storage }; } // namespace core \ No newline at end of file diff --git a/Sim_Engine/EngineLib/src/Application.cpp b/Sim_Engine/EngineLib/src/Application.cpp index 479170d..1a5aad4 100644 --- a/Sim_Engine/EngineLib/src/Application.cpp +++ b/Sim_Engine/EngineLib/src/Application.cpp @@ -7,7 +7,6 @@ #include "Scene/Camera.h" #include #include "EngineLib/LogMacros.h" -#include "Platform/DataManager.h" #ifdef __gl_h_ #undef __gl_h_ @@ -15,13 +14,12 @@ #include namespace fs = std::filesystem; -// Initialize the static instance pointer to nullptr +// Initialise the static instance pointer to nullptr Application* Application::sInstance = nullptr; -// Constructor: Initializes paths, sets up data manager, and creates the main application window +// Constructor: Initialises paths, sets up data manager, and creates the main application window Application::Application(const std::string& appName) { paths::init(); - data::DataManager::instance().setParentFolder(paths::runs().string()); LOG_INFO("Root path: %s", paths::root().string().c_str()); LOG_INFO("Assets path: %s", paths::assets().string().c_str()); diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index 7e4cff4..2a735b0 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -12,6 +12,7 @@ #include "Interpreter/StoredProgram.h" #include "Interpreter/Parser.h" +#include "Platform/Paths.h" #include "EngineLib/LogMacros.h" #include "Platform/DataManager.h" @@ -44,8 +45,8 @@ namespace core { // 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->getIntegrator()->IntegratorName(_robot->getIntegrator()->getIntegrationMethod()); - } + return _robot->getIntegratorName(); + } // Get the current integration method integration::eIntegrationMethod SimulationCore::integrationMethod() const { if (!_robot) { return integration::eIntegrationMethod::RK4; } @@ -139,13 +140,15 @@ namespace core { _robot->setRefBuffer(&_trajRefBuffer); } + _data.setParentFolder(paths::runs().string()); + // Ensure reference sim system have their integrators configured for the new run setupSimulationIntegrator(); - SET_SIM_INTEGRATOR(_robot->getIntegratorName()); + _data.setIntegratorName(integrationMethodName()); _simRunning = true; _telemetryBegun = false; - DATA_CAPTURE_ENABLE(true); + _data.setEnabled(true); } // Stop the simulation loop @@ -163,7 +166,7 @@ namespace core { exportLogsToHDF5(); // Clear buffers to free memory and prepare for next run - DATA_CAPTURE_ENABLE(false); + _data.setEnabled(false); _simRunning = false; _telemetryBegun = false; } @@ -233,7 +236,7 @@ namespace core { fields.emplace_back("joint_index", (double)exportBuf->joint_index[i]); // Write entry to HDF5 - HDF5_SIM_DATA(header, fields); + _data.capture(data::Stream::Simulation, header, fields); } // Log export duration auto dur = std::chrono::steady_clock::now() - t0; @@ -283,7 +286,7 @@ namespace core { fields.emplace_back("joint_index", (double)_trajRefBuffer.joint_index[i]); // Write this entry to HDF5 - HDF5_REF_DATA(header, fields); + _data.capture(data::Stream::Reference, header, fields); } } From ce795b0dc05d7f8cee5ccf37908318c79bcfa7c4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 22 Feb 2026 20:07:31 +0000 Subject: [PATCH 17/20] fixes: improved data logging shutdown and HDF5 file handling NOTES: - Add DataManager destructor/finalise() for explicit cleanup of HDF5 writers - Include thread ID and ms timestamp in HDF5 filenames to avoid collisions - Add debug helpers and print statements for shutdown tracing - Protect HDF5 file creation with a mutex for thread safety - In batch mode, force single worker thread for determinism - Ensure simulation stops and data is flushed on script completion - Minor code/comment cleanups and improved timestamp formatting --- Sim_Engine/Engine/src/BatchMain.cpp | 5 +- .../EngineLib/include/Platform/DataManager.h | 4 ++ .../src/Interpreter/StoredProgram.cpp | 2 +- .../EngineLib/src/Platform/DataManager.cpp | 66 ++++++++++++++++-- .../EngineLib/src/Scene/SimulationCore.cpp | 14 ++-- .../External/MathLib/Release/MathLib.dll | Bin 61952 -> 61952 bytes 6 files changed, 75 insertions(+), 16 deletions(-) diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp index 08c4527..9e19052 100644 --- a/Sim_Engine/Engine/src/BatchMain.cpp +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -68,17 +68,14 @@ int runBatchMode() { // If not in batch mode only, reserve one core for the main thread #ifndef _BATCH_MODE_ONLY - workers = (cores > 1) ? cores - 1 : 1; + workers = 1; #endif - assert(core->robotSystem() != nullptr); - // Build runner with factory and worker count StudyRunner runner(makeCoreFactory, workers); // The script text for the run(s) std::string scriptText = R"( load(robot, VISPA) - set(integrator, rk4) wait(2.5) trajClear() diff --git a/Sim_Engine/EngineLib/include/Platform/DataManager.h b/Sim_Engine/EngineLib/include/Platform/DataManager.h index e69be0d..1619ffb 100644 --- a/Sim_Engine/EngineLib/include/Platform/DataManager.h +++ b/Sim_Engine/EngineLib/include/Platform/DataManager.h @@ -93,6 +93,10 @@ namespace data { class ENGINE_API DataManager { public: DataManager() = default; + ~DataManager(); + + void finalise(); + void setEnabled(bool enabled); void setIntegratorName(std::string name) { _integratorName = name; } void setParentFolder(std::string folder) { _parentFolder = folder; } diff --git a/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp b/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp index cd3e157..510ae85 100644 --- a/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp +++ b/Sim_Engine/EngineLib/src/Interpreter/StoredProgram.cpp @@ -142,7 +142,7 @@ namespace interpreter { if (_stopRequested) { stop(); return; } // Command checks if (_commands.empty()) { _state = ProgramState::Faulted; return; } - if (!commandsLeft()) { _state = ProgramState::Completed; return; } + if (!commandsLeft()) { _state = ProgramState::Completed; return; _core->stopSimulation(); } // Ensure default object is valid in context scene::Object* o = _defaultObj ? _defaultObj : (_core ? _core->getObject() : nullptr); diff --git a/Sim_Engine/EngineLib/src/Platform/DataManager.cpp b/Sim_Engine/EngineLib/src/Platform/DataManager.cpp index 8850f04..c2893ae 100644 --- a/Sim_Engine/EngineLib/src/Platform/DataManager.cpp +++ b/Sim_Engine/EngineLib/src/Platform/DataManager.cpp @@ -4,6 +4,38 @@ #include "Platform/DataManager.h" namespace data { + // Debugging helpers for HDF5 + static bool DM_DEBUG_HDF5() { + static bool v = []() { +#ifdef _WIN32 + char* buffer = nullptr; + size_t size = 0; + if (_dupenv_s(&buffer, &size, "DSFE_DATA_DEBUG") == 0 && buffer != nullptr) { + free(buffer); + return true; + } + if (buffer) free(buffer); + return false; +#else + return (std::getenv("DSFE_DATA_DEBUG") != nullptr); +#endif + }(); + return v; + } + + // Check HDF5 status and print error stack if there's an error, also print verbose info if debug env var is sets + static inline void hdf5_check(herr_t status, const char* msg) { + if (status < 0) { + std::cerr << "HDF5 ERROR: " << msg << " (status=" << status << ")\n"; + // Print full HDF5 error stack + H5Eprint(H5E_DEFAULT, stderr); + } + else if (DM_DEBUG_HDF5()) { + // verbose when debug env var set + std::cerr << "HDF5 OK: " << msg << " (status=" << status << ")\n"; + } + } + // Escape a string for CSV format static inline std::string escape_csv(const std::string_view s) { bool needQuotes = false; @@ -76,6 +108,8 @@ namespace data { // Get current timestamp as a compact string (e.g. "20240601_153045") static inline std::string timestampCompact() { auto now = std::chrono::system_clock::now(); + auto secs = std::chrono::time_point_cast(now); + auto ms = std::chrono::duration_cast(now - secs).count(); std::time_t t = std::chrono::system_clock::to_time_t(now); std::tm tm{}; @@ -86,7 +120,7 @@ namespace data { localtime_r(&t, &tm); #endif std::ostringstream oss; - oss << std::put_time(&tm, "%Y%m%d_%H%M%S"); + oss << std::put_time(&tm, "%Y%m%d_%H%M%S") << "_" << std::setw(3) << std::setfill('0') << ms; return oss.str(); } @@ -360,15 +394,29 @@ namespace data { << " dir=" << dir.string() << "\n"; } + // Determine type string based on subfolder std::string typeStr; + // Determine type string based on subfolder if (subStr == "Simulation") { typeStr = "sim"; } else if (subStr == "Reference") { typeStr = "ref"; } - _path = (dir / ("dsfe_" + typeStr + "_run_" + timestampCompact() + "_" + intName + ".h5")).string(); + // Get thread ID as string + std::ostringstream tid; + tid << std::this_thread::get_id(); + + // Construct file path: parent/sub/dsfe_[type]_run_[timestamp]_[intName]_[threadid].h5 + _path = (dir / ("dsfe_" + typeStr + "_run_" + + timestampCompact() + "_" + + intName + "_" + + tid.str() + ".h5")).string(); // Create HDF5 file - _fileID = H5Fcreate(_path.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT); + static std::mutex hdf5Mutex; // protect HDF5 library calls + { + std::lock_guard hdf5Lock(hdf5Mutex); + _fileID = H5Fcreate(_path.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT); + } // Check for errors if (_fileID < 0) { std::cerr << "Hdf5StreamWriter: failed to create file: " << _path << "\n"; @@ -406,6 +454,7 @@ namespace data { // stop HDF5 stream writer void HDF5StreamWriter::stop() { + printf("WRITER STOP\n"); std::lock_guard lock(_mtx); if (!_active) return; @@ -502,7 +551,6 @@ namespace data { hid_t ds = ensureKey(key, value); if (ds < 0) { continue; } // unsupported type - // Append value based on type if (std::holds_alternative(value)) { appendString1D(ds, _vlenStrType, std::get(value)); } @@ -587,6 +635,16 @@ namespace data { _file.flush(); // ensure data is written } + DataManager::~DataManager() { + printf("DM DESTROYED\n"); + finalise(); + } + + void DataManager::finalise() { + if (_sim.active()) { _sim.stop(); } + if (_ref.active()) { _ref.stop(); } + } + // Enable or disable data logging void DataManager::setEnabled(bool enabled) { if (enabled == _enabled) return; diff --git a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp index 2a735b0..05c115f 100644 --- a/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp +++ b/Sim_Engine/EngineLib/src/Scene/SimulationCore.cpp @@ -19,7 +19,9 @@ namespace core { // Constructor SimulationCore::SimulationCore() {} - SimulationCore::~SimulationCore() = default; + SimulationCore::~SimulationCore() { + printf("CORE DESTROYED\n"); + } // Update physics for all objects in the scene using the physics system void SimulationCore::updatePhysics(double dt) { @@ -153,7 +155,7 @@ namespace core { // Stop the simulation loop void SimulationCore::stopSimulation() { - if (!_simRunning) return; + if (!_simRunning) { return; } D_RUNTIME("stopping simulation"); // Export references @@ -251,6 +253,8 @@ namespace core { // Log success D_SUCCESS("ExportLogs -> export completed successfully"); LOG_INFO("ExportLogs -> export completed successfully"); + + std::this_thread::sleep_for(std::chrono::seconds(1)); } // Exports the reference trajectory data to HDF5 format using the custom macro for each ref entry @@ -295,11 +299,7 @@ namespace core { // SYNCHRONOUS SCRIPT EXECUTION // -------------------------------------------------- - // Run a script synchronously to completion, blocking the main thread. Returns true if completed successfully. - // NOTE: this is a blocking call that runs a tight loop until the script finishes, so it should only be used for testing or non-interactive scenarios. - // IMPORTANT: I want to make this REALLY clear: - // ---> I have implemented this for short (<5 minute) test scripts where blocking is acceptable - // ---> IT IS NOT intended for general use and WILL CAUSE THE UI TO FREEZE if used with long-running scripts + // Run a script synchronously to completion, blocking the main thread. Returns true if completed successfully bool SimulationCore::runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) { // Map method enum to string name, purely for logging purposes static const char* names[] = { "euler", "midpoint", "heun", "ralston", "rk4", "rk45" }; diff --git a/Sim_Engine/External/MathLib/Release/MathLib.dll b/Sim_Engine/External/MathLib/Release/MathLib.dll index 7fc480f9d4155d3b68fee55ba83dcddd9fb59fba..65d5df5e057c641c13df77beb706c485e55bf238 100644 GIT binary patch delta 110 zcmZp8!rbtLd4m8W(?p-mLX4jlSWWbqoyh Date: Sun, 22 Feb 2026 22:06:12 +0000 Subject: [PATCH 18/20] refactor: added BatchArgs and flexible sweeps to batch mode NOTES: - Introduced BatchArgs struct for batch mode argument handling. - Refactored BatchMain.cpp/Main.cpp for improved parsing and validation. - Batch mode now actually supports flexible sweeps of timesteps and integrators, with output tags generated from run parameters. - HDF5 file naming updated to use run tags (HONESTLY WORTH IT). - Project files updated to include BatchArgs.h. - BatchEntry.h now accepts BatchArgs. - Batch mode is now user-friendly. --- Sim_Engine/Engine.vcxproj | 5 +- Sim_Engine/Engine.vcxproj.filters | 3 + Sim_Engine/Engine/include/BatchArgs.h | 19 ++ Sim_Engine/Engine/include/BatchEntry.h | 3 +- Sim_Engine/Engine/src/BatchMain.cpp | 176 +++++++++--------- Sim_Engine/Engine/src/Main.cpp | 166 ++++++++++++++++- .../EngineLib/include/Platform/DataManager.h | 5 +- .../include/Platform/ISimulationCore.h | 2 + .../EngineLib/include/Scene/SimulationCore.h | 2 + .../EngineLib/src/Platform/DataManager.cpp | 46 +---- .../EngineLib/src/Platform/StudyRunner.cpp | 1 + .../EngineLib/src/Scene/SimulationCore.cpp | 1 + .../External/MathLib/Release/MathLib.dll | Bin 61952 -> 61952 bytes 13 files changed, 282 insertions(+), 147 deletions(-) create mode 100644 Sim_Engine/Engine/include/BatchArgs.h diff --git a/Sim_Engine/Engine.vcxproj b/Sim_Engine/Engine.vcxproj index 6221490..3577783 100644 --- a/Sim_Engine/Engine.vcxproj +++ b/Sim_Engine/Engine.vcxproj @@ -57,7 +57,7 @@ true _DEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) + $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine\include;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) stdcpp20 $(IntDir)vc$(PlatformToolsetVersion).pdb $(IntDir)%(FileName).obj @@ -87,7 +87,7 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" true NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true - $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) + $(SolutionDir)EngineLib;$(SolutionDir)EngineLib\include;$SolutionDir)External\GLFW;$(SolutionDir)External\GLAD;$(SolutionDir)External\GLM;$(SolutionDir)External\assimp;$(SolutionDir)External\MathLib\include;$(SolutionDir)Engine\include;$(SolutionDir)External\Eigen;%(AdditionalIncludeDirectories) stdcpp20 false $(IntDir)vc$(PlatformToolsetVersion).pdb @@ -140,6 +140,7 @@ echo D | xcopy /E /Y /D "$(ProjectDir)Engine\configs\*" "$(TargetDir)configs\" + diff --git a/Sim_Engine/Engine.vcxproj.filters b/Sim_Engine/Engine.vcxproj.filters index cc54faf..c65e0d6 100644 --- a/Sim_Engine/Engine.vcxproj.filters +++ b/Sim_Engine/Engine.vcxproj.filters @@ -94,5 +94,8 @@ include + + include + \ No newline at end of file diff --git a/Sim_Engine/Engine/include/BatchArgs.h b/Sim_Engine/Engine/include/BatchArgs.h new file mode 100644 index 0000000..ec49d47 --- /dev/null +++ b/Sim_Engine/Engine/include/BatchArgs.h @@ -0,0 +1,19 @@ +#pragma once +#include +#include +#include + +// Struct to hold command-line arguments for batch mode +struct BatchArgs { + std::string testPath; + // Base configuration for batch runs (if not provided, defaults will be used) + std::optional baseDtRaw; + std::optional baseDt; + std::optional baseInt; + // Lists for sweep parameters (if not provided, defaults will be used) + std::vector sweepDtRaw; + std::vector sweepDt; + std::vector sweepInt; + // Run name for output organisation + std::optional runName; +}; \ No newline at end of file diff --git a/Sim_Engine/Engine/include/BatchEntry.h b/Sim_Engine/Engine/include/BatchEntry.h index 8d21035..4a24b60 100644 --- a/Sim_Engine/Engine/include/BatchEntry.h +++ b/Sim_Engine/Engine/include/BatchEntry.h @@ -1,2 +1,3 @@ #pragma once -int runBatchMode(); \ No newline at end of file +struct BatchArgs; +int runBatchMode(const BatchArgs& args); \ No newline at end of file diff --git a/Sim_Engine/Engine/src/BatchMain.cpp b/Sim_Engine/Engine/src/BatchMain.cpp index 9e19052..4e9048f 100644 --- a/Sim_Engine/Engine/src/BatchMain.cpp +++ b/Sim_Engine/Engine/src/BatchMain.cpp @@ -1,125 +1,112 @@ // BatchMain.cpp #include "pch.h" #include -#include "include/BatchEntry.h" +#include "BatchEntry.h" +#include "BatchArgs.h" #include "Platform/StudyRunner.h" #include "Numerics/IntegrationMethods.h" #include "Platform/Paths.h" -#include // ensure at top of file -#include // for fprintf +#include +#include +#include +#include +#include + +// Integration method parser from string (throws if unknown) +static integration::eIntegrationMethod parseMethod(const std::string& name) { + if (name == "euler") return integration::eIntegrationMethod::Euler; + if (name == "midpoint") return integration::eIntegrationMethod::Midpoint; + if (name == "heun") return integration::eIntegrationMethod::Heun; + if (name == "ralston") return integration::eIntegrationMethod::Ralston; + if (name == "rk4") return integration::eIntegrationMethod::RK4; + if (name == "rk45") return integration::eIntegrationMethod::RK45; + throw std::runtime_error("Unknown integrator: " + name); +} + +// Helper to format a timestep (dt) as a string for use in tags and filenames +static std::string formatDtForFile(const std::string& raw) { + std::string out; + out.reserve(raw.size() + 4); + for (char c : raw) { + if (c == '.') { out += "p"; } + if (c == '/') { out += "over"; } + else { out += c; } + } + return out; +} -// Helper: create CorePtr (unique_ptr with std::function deleter) +// Core factory function for StudyRunner static CorePtr makeCoreFactory() { - // Create a raw pointer to ISimulationCore using the factory function from EngineCore core::ISimulationCore* raw = CreateSimulationCore_v1(); if (!raw) { return CorePtr(nullptr, [](core::ISimulationCore*) {}); } - - // Robot System check - std::cout << "[BATCH DEBUG] Core created: " << raw << "\n"; - if (raw->robotSystem() == nullptr) { std::cout << "[BATCH DEBUG] robotSystem() is NULL\n"; } - else { std::cout << "[BATCH DEBUG] robotSystem() OK\n"; } - - // Physics System check - if (raw->physicsSystem() == nullptr) { std::cout << "[BATCH DEBUG] physicsSystem() is NULL\n"; } - else { std::cout << "[BATCH DEBUG] physicsSystem() OK\n"; } - - // Construct CorePtr with custom deleter that calls DestroySimulationCore return CorePtr(raw, [](core::ISimulationCore* p) { DestroySimulationCore(p); }); } -int runBatchMode() { - // initialises paths (required for loading robots, and any other file access in the core) +// Batch mode entry point +int runBatchMode(const BatchArgs& args) { paths::init(); - - // Log batch mode entry fprintf(stdout, "BATCH MODE ENTERED\n"); fflush(stdout); + // Basic validation and setup try { - // Define the configurations for the studies to run (combinations of integrator methods, timesteps, and run lengths) + // Validate required baseline + if (!args.baseDt.has_value() || !args.baseInt.has_value()) { throw std::runtime_error("Baseline (--basedt and --baseint) required."); } + if (args.testPath.empty()) { throw std::runtime_error("No test script provided."); } + + // Load script + std::ifstream file(args.testPath); + if (!file) { throw std::runtime_error("Failed to open test file: " + args.testPath); } + std::ostringstream buffer; + buffer << file.rdbuf(); + std::string scriptText = buffer.str(); + + // Build configs std::vector configs; - std::vector methods = { - integration::eIntegrationMethod::Euler, - integration::eIntegrationMethod::Midpoint, - integration::eIntegrationMethod::Heun, - integration::eIntegrationMethod::Ralston, - integration::eIntegrationMethod::RK4, - integration::eIntegrationMethod::RK45 - }; - std::vector dts = { 0.001, 0.002, 0.005 }; - std::vector lengths_mins = { 0.5, 1.0, 5.0 }; - // Create a config for each combination of method, dt, and length - for (auto m : methods) { - for (double dt : dts) { - for (double len : lengths_mins) { - StudyRunner::config c; - c.method = m; - c.dt = dt; - c.len_min = len; - c.tag = std::string("m") + std::to_string((int)m) + "_dt" + std::to_string(dt) + "_L" + std::to_string((int)len); - configs.push_back(c); - } - } - } - - // Determine the number of worker threads to use based on hardware concurrency - size_t cores = std::thread::hardware_concurrency(); - size_t workers = 0; // default: use all cores - // If not in batch mode only, reserve one core for the main thread -#ifndef _BATCH_MODE_ONLY - workers = 1; -#endif - // Build runner with factory and worker count - StudyRunner runner(makeCoreFactory, workers); - - // The script text for the run(s) - std::string scriptText = R"( - load(robot, VISPA) - - wait(2.5) - trajClear() - wait(0.25) - - start() - - parallel(5.0) { - trajSet(link01, TRAP, -60.0, 45.0, 110.0) - trajSet(link02, TRAP, 50.0, 40.0, 100.0) - trajSet(link03, TRAP, -55.0, 45.0, 110.0) - trajSet(link04, TRAP, 40.0, 55.0, 130.0) - trajSet(link05, TRAP, -50.0, 60.0, 140.0) - trajSet(link06, TRAP, 60.0, 65.0, 150.0) - } - - wait(5.0) + // Baseline (always first) + { + StudyRunner::config base; + base.method = parseMethod(args.baseInt.value()); + base.dt = args.baseDt.value(); + base.len_min = 1.0; // Fixed simulation duration (consistent across runs) + base.tag = + (args.runName.has_value() ? args.runName.value() + "_" : "") + + "base_dt" + formatDtForFile(args.baseDtRaw.value()) + + "_int_" + args.baseInt.value(); + configs.push_back(base); + } - parallel(5.0) { - trajSet(link01, TRAP, 65.0, 55.0, 140.0) - trajSet(link02, TRAP, -55.0, 50.0, 130.0) - trajSet(link03, TRAP, 60.0, 55.0, 140.0) - trajSet(link04, TRAP, -75.0, 65.0, 160.0) - trajSet(link05, TRAP, 65.0, 70.0, 170.0) - trajSet(link06, TRAP, -60.0, 75.0, 180.0) + // Sweep grid + for (size_t i = 0; i < args.sweepDt.size(); ++i) { + double dt = args.sweepDt[i]; + const std::string& dtRaw = args.sweepDtRaw[i]; + for (const auto& intName : args.sweepInt) { + StudyRunner::config c; + c.method = parseMethod(intName); + c.dt = dt; + c.len_min = 1.0; + c.tag = + (args.runName.has_value() ? args.runName.value() + "_" : "") + + "dt" + formatDtForFile(dtRaw) + + "_int_" + intName; + configs.push_back(c); } + } - wait(55.0) - trajClear() - wait(0.25) - - stop() - )"; + // Worker configuration + size_t workers = 1; // stable mode (parallel logging disabled) + StudyRunner runner(makeCoreFactory, workers); - // Run the studies and time the total batch duration + // Execute auto T0 = std::chrono::high_resolution_clock::now(); auto results = runner.runStudies(configs, scriptText); auto T1 = std::chrono::high_resolution_clock::now(); - // Log total batch time double total = std::chrono::duration(T1 - T0).count(); fprintf(stderr, "TOTAL BATCH TIME = %.3f sec\n", total); - // Sort results by tag for easier reading + // Sort results by tag std::sort(results.begin(), results.end(), [](const StudyResult& a, const StudyResult& b) { return a.tag < b.tag; @@ -127,13 +114,16 @@ int runBatchMode() { // Print summary for (const auto& r : results) { - std::cout << "tag=" << r.tag + std::cout + << "tag=" << r.tag << " success=" << (r.success ? "yes" : "no") << " int=" << r.intName << " time=" << r.simTime - << " samples=" << r.samples << "\n"; + << " samples=" << r.samples + << "\n"; } } + // Catch any exceptions that escaped from the batch processing and log them catch (const std::exception& e) { std::cerr << "Batch mode exception: " << e.what() << std::endl; return EXIT_FAILURE; diff --git a/Sim_Engine/Engine/src/Main.cpp b/Sim_Engine/Engine/src/Main.cpp index 2e87f5b..490db36 100644 --- a/Sim_Engine/Engine/src/Main.cpp +++ b/Sim_Engine/Engine/src/Main.cpp @@ -1,20 +1,166 @@ #include "pch.h" // File: Main.cpp -// GitHub: SaltyJoss #include #include -#include "include/BatchEntry.h" +#include "BatchEntry.h" +#include "BatchArgs.h" +// 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; + std::stringstream ss(input); + std::string item; + // Split the input string by commas and trim whitespace from each item + while (std::getline(ss, item, ',')) + if (!item.empty()) + result.push_back(item); + + return result; +} + +// Helper function to parse a timestep (dt) from a string, supporting both plain decimal and fractional formats (e.g., "1/180") +static double parseDt(const std::string& input) { + // Case: "1/180" + auto slashPos = input.find('/'); + if (slashPos != std::string::npos) { + double num = std::stod(input.substr(0, slashPos)); + double den = std::stod(input.substr(slashPos + 1)); + return num / den; + } + + // Case: plain decimal + return std::stod(input); +} + +// Main entry point for the application, handling both GUI and batch modes based on command-line arguments int main(int argc, char** argv) { - printf("argc = %d\n", argc); - for (int i = 0; i < argc; ++i) - printf("argv[%d] = %s\n", i, argv[i]); + bool batchMode = false; + + // First pass to check for batch mode flag + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg == "--batch" || arg == "-b") { + batchMode = true; + break; + } + } + + // Non-batch mode: simple pass to handle GUI-specific options + if (!batchMode) { + for (int i = 1; i < argc; ++i) { + std::string arg(argv[i]); + if (arg == "--about") { + std::cout << "DSFE - Dynamic Systems Framework Engine\n"; + return 0; + } + else if (arg == "--help" || arg == "-h") { + std::cout << "Batch mode usage:\n"; + std::cout << " --batch -t