From 6610310e480f5014ef5817eeb8a1a2fef0d46f3c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 12:21:42 +0100 Subject: [PATCH 001/114] feat: Add spatial force buffer to `SpatialDynamicsScratch` structure --- DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h index 43777486..0a12ea49 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h @@ -83,6 +83,7 @@ namespace robots { std::vector> a; // spatial acceleration std::vector> pA; // articulated bias force std::vector> U; // articulated body force + std::vector> f_ext; // spatial force mathlib::VecX_T u; // joint force contribution mathlib::VecX_T d; // joint inertia contribution @@ -110,6 +111,8 @@ namespace robots { a.resize(nJoints); pA.resize(nJoints); U.resize(nJoints); + f_ext.resize(nJoints); + u.resize(nJoints); d.resize(nJoints); @@ -131,6 +134,8 @@ namespace robots { a.clear(); pA.clear(); U.clear(); + f_ext.clear(); + u.resize(0); d.resize(0); From 8015070639d01e5ec3442bb32e7fad2f0d34c3f5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 15:37:56 +0100 Subject: [PATCH 002/114] refactor: Removed old Interpreter (DSL) file names --- .../DSFE_Core/include/Interpreter/Command.h | 31 - .../include/Interpreter/CommandContext.h | 109 -- .../include/Interpreter/CommandFactory.h | 38 - .../include/Interpreter/Commands/LoadCmd.h | 54 - .../Interpreter/Commands/ParallelGroupCmd.h | 39 - .../Interpreter/Commands/RotateByCmd.h | 44 - .../Interpreter/Commands/RotateJointByCmd.h | 56 - .../Interpreter/Commands/RotateJointToCmd.h | 51 - .../Interpreter/Commands/RotateToCmd.h | 43 - .../include/Interpreter/Commands/SaveCmd.h | 55 - .../include/Interpreter/Commands/SelectCmd.h | 39 - .../include/Interpreter/Commands/SetCmd.h | 75 -- .../Interpreter/Commands/SetOmegaCmd.h | 41 - .../include/Interpreter/Commands/SpinCmd.h | 44 - .../include/Interpreter/Commands/StartCmd.h | 41 - .../include/Interpreter/Commands/StopCmd.h | 40 - .../Interpreter/Commands/TrajClearCmd.h | 42 - .../include/Interpreter/Commands/TrajSetCmd.h | 46 - .../include/Interpreter/Commands/WaitCmd.h | 44 - .../DSFE_Core/include/Interpreter/ICommand.h | 42 - .../include/Interpreter/IStoredProgram.h | 83 -- .../include/Interpreter/MainContext.h | 22 - .../DSFE_Core/include/Interpreter/Parser.h | 44 - .../include/Interpreter/ProgramData.h | 115 -- .../include/Interpreter/RegisterCommand.h | 9 - .../include/Interpreter/RunWrapper.h | 19 - .../DSFE_Core/include/Interpreter/SimFwd.h | 16 - .../include/Interpreter/StoredProgram.h | 98 -- .../DSFE_Core/include/Interpreter/Token.h | 31 - .../DSFE_Core/include/Interpreter/Utils.h | 68 -- .../DSFE_Core/include/Robots/DynamicsTypes.h | 191 ---- .../DSFE_Core/include/Robots/RobotDynamics.h | 168 --- .../include/Robots/RobotDynamics.inl | 644 ------------ .../include/Robots/RobotKinematics.h | 50 - .../include/Robots/RobotKinematics.inl | 105 -- .../DSFE_Core/include/Robots/RobotLoader.h | 12 - .../DSFE_Core/include/Robots/RobotMetrics.h | 45 - .../DSFE_Core/include/Robots/RobotModel.h | 210 ---- .../include/Robots/RobotSimSnapshot.h | 81 -- .../DSFE_Core/include/Robots/RobotSystem.h | 325 ------ .../include/Robots/RobotSystemStep.inl | 252 ----- .../include/Robots/SpatialDynamics.h | 95 -- .../include/Robots/SpatialDynamics.inl | 319 ------ .../DSFE_Core/include/Robots/SpatialModel.h | 33 - .../include/Robots/SpatialModelCast.inl | 22 - .../include/Robots/TrajectoryManager.h | 36 - .../DSFE_Core/src/Robots/RobotDynamics.cpp | 11 - .../DSFE_Core/src/Robots/RobotKinematics.cpp | 13 - DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp | 500 --------- .../DSFE_Core/src/Robots/RobotSimSnapshot.cpp | 43 - DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp | 983 ------------------ .../src/Robots/TrajectoryManager.cpp | 74 -- 52 files changed, 5691 deletions(-) delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Command.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/CommandContext.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/CommandFactory.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/ParallelGroupCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/SaveCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/SelectCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/StartCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/StopCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Commands/WaitCmd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/ICommand.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/IStoredProgram.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/MainContext.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Parser.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/RegisterCommand.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/RunWrapper.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/StoredProgram.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Token.h delete mode 100644 DSFE_App/DSFE_Core/include/Interpreter/Utils.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotLoader.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotModel.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotSystem.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl delete mode 100644 DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl delete mode 100644 DSFE_App/DSFE_Core/include/Robots/SpatialModel.h delete mode 100644 DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl delete mode 100644 DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h delete mode 100644 DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp delete mode 100644 DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp delete mode 100644 DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp delete mode 100644 DSFE_App/DSFE_Core/src/Robots/RobotSimSnapshot.cpp delete mode 100644 DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp delete mode 100644 DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Command.h b/DSFE_App/DSFE_Core/include/Interpreter/Command.h deleted file mode 100644 index 6b223879..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Command.h +++ /dev/null @@ -1,31 +0,0 @@ -// DSFE_Core Command.h -#pragma once -#include "EngineCore.h" -#include "ICommand.h" -#include "MainContext.h" - -using namespace interpreter; - -namespace commands { - // Class representing a generic command - class DSFE_API Command : public ICommand { - public: - // Set the command context - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - - program_data::CmdResult update(CommandContext& cntx, double dt) override; - - void execute() override; - - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - - interpreter::IStoredProgram* getProgram() const override { return _program; } - void setProgram(interpreter::IStoredProgram* program) override { _program = program; } - - protected: - IStoredProgram* _program = nullptr; - CommandContext* _cntx = nullptr; - }; -} // namespace commands diff --git a/DSFE_App/DSFE_Core/include/Interpreter/CommandContext.h b/DSFE_App/DSFE_Core/include/Interpreter/CommandContext.h deleted file mode 100644 index dd85661e..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/CommandContext.h +++ /dev/null @@ -1,109 +0,0 @@ -// DSFE_Core CommandContext.h -#pragma once - -#include "EngineCore.h" -#include "SimFwd.h" -#include "Interpreter/Utils.h" - -#include "Platform/Logger.h" - -namespace commands { - struct DSFE_API ActiveRigidRot { - mathlib::Vec3 axisUnit{ 0.0, 0.0, 0.0 }; - mathlib::Quat qStart{ 1.0, 0.0, 0.0, 0.0 }; - mathlib::Quat qTarget{ 1.0, 0.0, 0.0, 0.0 }; - double maxOmega = 0.0; // rad/s - double epsAngle = 0.5 * constants::PI / 180; // rad - bool active = false; - }; - - struct DSFE_API ActiveJointRot { - std::string link; - double start = 0.0; - double target = 0.0; // rad - double maxOmega = 0.0; // rad/s - double epsAngle = 0.5 * constants::PI / 180; // rad - bool wrapShortest = true; - bool active = false; - }; - - // Class representing the command context - class DSFE_API CommandContext { - public: - CommandContext(core::ISimulationCore* core); - - // --- INITIALISATION METHODS --- - utils::OpResult startSim(); - utils::OpResult setFixedDt(double dt); - utils::OpResult loadSingleBody(const std::string& bodyName); - utils::OpResult loadMultibody(const std::string& bodyName); - - // --- GLOBAL STATE METHODS --- - - // Sets the angular units for rotation commands - void setAngularUnits(utils::AngularUnits units); - // Gets the current angular units - utils::AngularUnits getAngularUnits() const; - - // Sets the maximum absolute angular velocity (omega) clamp - void setOmegaClamp(double maxAbsOmega); - // Gets the current omega clamp value - double getOmegaClamp() const; - - // Stops all angular velocity for the robot - utils::OpResult stopAllOmega(); // stops all angular velocity - - utils::OpResult setJointOmega(const std::string& childLink, double omegaDegPerSec); // deg/s - utils::OpResult stopJointOmega(const std::string& childLink); - - // --- HELPER METHODS --- - core::ISimulationCore* Core() const; - robots::RobotSystem& Robot() const; - - - // --- ROTATION COMMAND METHODS --- - - 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 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); - - //utils::OpResult beginRigidRotateTo(scene::Object* obj, mathlib::Vec3 axisUnit, double maxOmegaDegPerSec, double angleDeg); - utils::OpResult beginJointRotateTo(const std::string& link, double maxOmegaDegPerSec, double angleDeg); - - // --- READ-ONLY ACCESSORS --- - bool hasLink(std::size_t linkIndex) const; - - private: - core::ISimulationCore* _core = nullptr; - - utils::AngularUnits _angularUnits = utils::AngularUnits::DegPerSec; - double _omegaClamp = 0.0; // Default: no clamp - - ActiveRigidRot _rig; - ActiveJointRot _jnt; - - double NormaliseOmega(double omega) const; - double convertOmegaToInternal(double omega) const; - - mathlib::Vec3 normaliseDirection(const mathlib::Vec3& dir) const; - - double getJointAngleRad(const std::string& link) const; - - std::unordered_map _jointAngles; - - double _currentAngle = 0.0; // current angle for rotation commands - std::string _currentLinkName; // current link name for joint commands - - mathlib::Vec3 angularVelocityPrev = mathlib::Vec3::Zero(); - mathlib::Vec3 linearVelocityPrev = mathlib::Vec3::Zero(); - - double _dtheta = 0.0; // angle displacement - double _dt = 0.0; // time interval - double _omega = 0.0; // angular velocity - }; -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/CommandFactory.h b/DSFE_App/DSFE_Core/include/Interpreter/CommandFactory.h deleted file mode 100644 index b4a2dc54..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/CommandFactory.h +++ /dev/null @@ -1,38 +0,0 @@ -// DSFE_Core CommandFactory.h -#pragma once - -#include "EngineCore.h" -#include "ICommand.h" -#include -#include - -namespace commands { - // Type alias for command creator function - using Creator = std::unique_ptr(*)(const std::string&, const std::vector&); - - // CommandFactory class for registering and creating commands - class DSFE_API CommandFactory { - public: - // Get the singleton instance of CommandFactory - static CommandFactory& Instance(); - - // Public API - bool registerCommand(const std::string name, Creator creator); - // Create a command by name - ICommand* create(const std::string_view& name, const std::string& id, const std::vector& args) const; - // Check if a command is registered - bool hasCommand(const std::string_view& name) const; - // Get a list of registered command names - std::vector commandNames() const; - - // Delete copy constructor and assignment operator to prevent copies - CommandFactory(const CommandFactory&) = delete; - CommandFactory& operator=(const CommandFactory&) = delete; - - private: - //singleton instance - CommandFactory() = default; - - std::unordered_map _registry; - }; -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h deleted file mode 100644 index 08fe298d..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h +++ /dev/null @@ -1,54 +0,0 @@ -// DSFE_Core LoadCmd.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - // Enum for load target type - enum class LoadTargetType { - SingleBody, - MultiBody - }; - - // Struct for load target - struct DSFE_API LoadTarget { - LoadTargetType type = LoadTargetType::SingleBody; - std::string path; - }; - - // Class representing the LOAD command - class DSFE_API LoadCmd final : public Command { - public: - // Constructor - LoadCmd(const std::string& id, const std::vector& tokens); - - // Get the command name - std::string_view getName() const { return "load"; } - - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - - // Get current result - program_data::CmdResult currentResult() const override { return getResult(); } - - // Execute the command - void execute() override; - - private: - LoadTarget _target{}; - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - std::string _path; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - // --- Free Function to Create LoadCmd --- - std::unique_ptr CreateLoadCmd(const std::string& id, const std::vector& tokens); -} // namespace commands diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/ParallelGroupCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/ParallelGroupCmd.h deleted file mode 100644 index b8f2d169..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/ParallelGroupCmd.h +++ /dev/null @@ -1,39 +0,0 @@ -// DSFE_Core ParallelGroupCmd.h -#pragma once - -#include "EngineCore.h" -#include -#include - -#include "Interpreter/Command.h" - -namespace commands { - class DSFE_API ParallelGroupCmd final : public Command { - public: - // Any: succeed if any command succeeds; All: succeed only if all commands succeed - enum class Policy { Any, All }; - // Constructor - ParallelGroupCmd(Policy policy, std::vector> cmds, double timeout = 0.0); - - // Delete copy constructor and assignment operator - ParallelGroupCmd(const ParallelGroupCmd&) = delete; - ParallelGroupCmd& operator=(const ParallelGroupCmd&) = delete; - // Default move constructor and assignment operator - ParallelGroupCmd(ParallelGroupCmd&&) noexcept = default; - ParallelGroupCmd& operator=(ParallelGroupCmd&&) noexcept = default; - - CmdResult update(CommandContext& cntx, double dt) override; - CmdResult currentResult() const override { return _result; } - - private: - void execute() override; - - Policy _policy; - std::vector> _cmds; - bool _started = false; - double _elapsed = 0.0; - double _timeoutSec = 0.0; - - CmdResult _result = { CmdState::NotStarted, {}, "" }; - }; -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h deleted file mode 100644 index f53ed1d1..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h +++ /dev/null @@ -1,44 +0,0 @@ -// DSFE_Core RotateByCmd.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - - // Class representing the ROTATE command - class DSFE_API RotateByCmd final : public Command { - public: - // Constructor - RotateByCmd(utils::AxisMask axis, double maxOmegaDeg, double deltaDeg); - - std::string_view getName() const { return "rotateBy"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; - - utils::AxisMask _axes{}; - double _deltaDeg; - double _omegaDeg; - double _totalRotated = 0.0; - bool _started = false; - - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - // Free function to create a RotateCmd - std::unique_ptr CreateRotateByCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h deleted file mode 100644 index fa78218d..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h +++ /dev/null @@ -1,56 +0,0 @@ -// DSFE_Core RotateJointByCmd.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - class DSFE_API RotateJointByCmd final : public Command { - public: - // Constructor - RotateJointByCmd(std::string link, double omegaDeg, double deltaDeg); - - std::string_view getName() const { return "rotateJointBy"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; - - std::string _link; - double _deltaDeg = 0.0; - double _omegaDeg = 0.0; - double _totalRotated = 0.0; - - bool _started = false; - double _elapsed = 0.0; - double _timeoutSec = 10.0; - - double _deltaRad = 0.0; - double _maxOmegaRad = 0.0; - - double _targetRad = 0.0; - double _thetaStartRad = 0.0; - - double _settleT = 0.0; - double _noProgressT = 0.0; - double _bestAbsErr = 0.0; - - - CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - // Free function to create a RotateCmd - std::unique_ptr CreateRotateJointByCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h deleted file mode 100644 index 9445eaa6..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h +++ /dev/null @@ -1,51 +0,0 @@ -// DSFE_Core RotateJointToCmd.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - // Class representing the ROTATE command - class DSFE_API RotateJointToCmd final : public Command { - public: - // Constructor - RotateJointToCmd(std::string link, double maxOmegaDeg, double angleDeg); - - std::string_view getName() const { return "rotateJointTo"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; - std::string _link; - double _angleDeg; // angle relative to the start position - double _maxOmegaDeg; - - bool _started = false; - double _elapsed = 0.0; - double _timeoutSec = 10.0; - - double _targetRad = 0.0; - double _maxOmegaRad = 0.0; - - double _settleT = 0.0; - double _noProgressT = 0.0; - double _bestAbsErr = 0.0; - - CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - // Free function to create a RotateCmd - std::unique_ptr CreateRotateJointToCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h deleted file mode 100644 index 8585ff19..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h +++ /dev/null @@ -1,43 +0,0 @@ -// DSFE_Core RotateToCmd.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - - // Class representing the ROTATE command - class DSFE_API RotateToCmd final : public Command { - public: - // Constructor - RotateToCmd(utils::AxisMask axes, double maxOmegaDeg, double angleDeg); - - std::string_view getName() const { return "rotateTo"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; - - utils::AxisMask _axes; - double _angleDeg = 0.0; - double _maxOmegaDeg = 0.0; - bool _started = false; - - CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - // Free function to create a RotateCmd - std::unique_ptr CreateRotateToCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SaveCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SaveCmd.h deleted file mode 100644 index df36d0c8..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SaveCmd.h +++ /dev/null @@ -1,55 +0,0 @@ -// DSFE_Core SaveCmd.h -#pragma once - -#include "EngineCore.h" -#include -#include -#include - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" - -namespace commands { - // Types of saves that can be performed by the SaveCmd - enum class eSaveType { - SimData, - Plots - }; - - struct ENGINE_API SaveCmdArgs { - eSaveType type; - std::string filename; - bool isIntegratorName; - }; - - class ENGINE_API SaveCmd : public Command { - public: - // Constructor - SaveCmd(const std::string& id, const std::vector& tokens); - - std::string_view getName() const { return "save"; } - void setContext(UIContext& cntx) { _cntx = &cntx; } - - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - - CommandContext* _cntx = nullptr; - SaveCmdArgs _target; - std::string _filename = ""; // Filename to save to - bool _integratorName = false; // Whether to include integrator name in the filename - bool _started = false; - - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateSaveCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SelectCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SelectCmd.h deleted file mode 100644 index ec055999..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SelectCmd.h +++ /dev/null @@ -1,39 +0,0 @@ -// DSFE_Core SelectCmd.h -#pragma once - -#include "EngineCore.h" -#include -#include -#include - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" - -namespace commands { - class DSFE_API SelectCmd final : public Command { - public: - // Constructor - SelectCmd(); - - std::string_view getName() const { return "select"; } - void setContext(CommandContext& cntx) { _cntx = &cntx; } - - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - - CommandContext* _cntx = nullptr; - - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateSelectCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h deleted file mode 100644 index 9f2c81a7..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h +++ /dev/null @@ -1,75 +0,0 @@ -// DSFE_Core SetCmd.h -#pragma once - -#include "EngineCore.h" -#include -#include -#include -#include - -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -#include "Platform/Logger.h" - -namespace commands { - - enum class SetTargetType { - IntegratorMethod, - Omega, - FixedDt, - Gravity - }; - - struct DSFE_API SetTarget { - SetTargetType type = SetTargetType::IntegratorMethod; - IntegratorMethod method = IntegratorMethod::RK4; // Default method - mathlib::Vec3 omega{ 0.0, 0.0, 0.0 }; - double fixedDt = 0.0; - double gravity = 0.0; - }; - - // Class representing the SET command - class DSFE_API SetCmd final : public Command { - public: - // Constructor - SetCmd(const std::string& id, const std::string& tokens); - // Get the command name - std::string_view getName() const { return "set"; } - // Set the command context - void setContext(CommandContext& cntx) { _cntx = &cntx; } - - // Getters and Setters for Result - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - - // Get current result - program_data::CmdResult currentResult() const override { return getResult(); } - - // Get current method - IntegratorMethod getCurrentMethod() const { return _method; } - - // Execute the command - void execute() override; - - private: - SetTarget _target{}; - - std::string _id; - std::string _tokens; - - IntegratorMethod _method = IntegratorMethod::RK4; - - CommandContext* _cntx = nullptr; - - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - // --- Free Function to Create SetCmd --- - std::unique_ptr CreateSetCmd(const std::string& id, const std::vector& tokens); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h deleted file mode 100644 index e4503089..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h +++ /dev/null @@ -1,41 +0,0 @@ -// DSFE_Core SetOmegaCmd.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - class SetOmegaCmd final : public Command { - public: - SetOmegaCmd(std::string link, const double omega); - ~SetOmegaCmd() override = default; - - std::string_view getName() const { return "setomega"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - program_data::CmdResult update(CommandContext& cntx, double dt) override; - void execute() override; - - std::string _link; - double _omega; - - bool _started = false; - - program_data::CmdResult _result{ CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateSetOmegaCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h deleted file mode 100644 index 3d786c31..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h +++ /dev/null @@ -1,44 +0,0 @@ -// DSFE_Core SpinCmd.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - - // Class representing the ROTATE command - class DSFE_API SpinCmd final : public Command { - public: - // Constructor - SpinCmd(utils::AxisMask axes, double omegaDeg, double duration); - - std::string_view getName() const { return "SpinCmd"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; - - utils::AxisMask _axes; - double _omegaDeg = 0.0; - double _duration = 0.0; - double _remainingTime = 0.0; - bool _started = false; - - CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message); - void markCompleted(); - bool hasStarted() const; - }; - - // Free function to create a RotateCmd - std::unique_ptr CreateSpinCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/StartCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/StartCmd.h deleted file mode 100644 index ddc11d29..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/StartCmd.h +++ /dev/null @@ -1,41 +0,0 @@ -// DSFE_Core StartCmd.h -#pragma once - -#include "EngineCore.h" -#include -#include -#include - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - class DSFE_API StartCmd final : public Command { - public: - // Constructor - StartCmd(); - - std::string_view getName() const { return "start"; } - void setContext(CommandContext& cntx) { _cntx = &cntx; } - - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - - CommandContext* _cntx = nullptr; - bool _started = false; - - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateStartCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/StopCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/StopCmd.h deleted file mode 100644 index 2a9b8916..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/StopCmd.h +++ /dev/null @@ -1,40 +0,0 @@ -// DSFE_Core StopCmd.h -#pragma once - -#include "EngineCore.h" -#include -#include -#include - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - class DSFE_API StopCmd final : public Command { - public: - // Constructor - StopCmd(); - - std::string_view getName() const { return "stop"; } - void setContext(CommandContext& cntx) { _cntx = &cntx; } - - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - - bool _started = false; - - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateStopCmd(const std::string& id, const std::vector& args); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h deleted file mode 100644 index 3f89ed4c..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h +++ /dev/null @@ -1,42 +0,0 @@ -// DSFE_Core TrajClearCmd.h -#pragma once - -#include "EngineCore.h" - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - - class TrajClearCmd final : public Command { - public: - explicit TrajClearCmd() = default; - ~TrajClearCmd() override = default; - - std::string_view getName() const { return "trajClear"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - program_data::CmdResult update(CommandContext& cntx, double dt) override; - void execute() override; - - core::ISimulationCore* _core = nullptr; - - bool _done = false; - bool _started = false; - - program_data::CmdResult _result{ CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateTrajClearCmd(const std::string& id, const std::vector& args); - -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h deleted file mode 100644 index 3b667d60..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h +++ /dev/null @@ -1,46 +0,0 @@ -// DSFE_Core TrajSetCmd.h -#pragma once - -#include "EngineCore.h" - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - - class TrajSetCmd final : public Command { - public: - TrajSetCmd(std::string link, std::string type, std::vector params); - ~TrajSetCmd() override = default; - - std::string_view getName() const { return "trajSet"; } - void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - program_data::CmdResult update(CommandContext& cntx, double dt) override; - void execute() override; - - std::string _link; - std::string _type; - std::vector _params; - - bool _done = false; - bool _started = false; - - program_data::CmdResult _result{ CmdState::NotStarted, {}, "" }; - - static std::string upperCopy(std::string s); - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateTrajSetCmd(const std::string& id, const std::vector& args); - -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/WaitCmd.h b/DSFE_App/DSFE_Core/include/Interpreter/Commands/WaitCmd.h deleted file mode 100644 index afc062a1..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/WaitCmd.h +++ /dev/null @@ -1,44 +0,0 @@ -// DSFE_Core WaitCmd.h -#pragma once - -#include "EngineCore.h" -#include -#include -#include - -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - class DSFE_API WaitCmd final : public Command { - public: - // Constructor - WaitCmd(double t); - - std::string_view getName() const { return "stop"; } - void setContext(CommandContext& cntx) { _cntx = &cntx; } - - program_data::CmdResult getResult() const { return _result; } - void setResult(const program_data::CmdResult& result) { _result = result; } - program_data::CmdResult currentResult() const override { return getResult(); } - - private: - void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; - - utils::AxisMask _axes{}; - - double _remainingTime = 0.0; - bool _started = false; - - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; - - protected: - void markFailed(const std::string& message) override; - void markCompleted() override; - bool hasStarted() const override; - }; - - std::unique_ptr CreateWaitCmd(const std::string& id, const std::vector& args); -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/ICommand.h b/DSFE_App/DSFE_Core/include/Interpreter/ICommand.h deleted file mode 100644 index ae2c882f..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/ICommand.h +++ /dev/null @@ -1,42 +0,0 @@ -// DSFE_Core ICommand.h -#pragma once - -#include "EngineCore.h" -#include "IStoredProgram.h" -#include -#include - -namespace commands { - // Forward declaration of ICommand for use in IStoredProgram - class CommandContext; - - // ICommand interface - class DSFE_API ICommand { - public: - // Virtual destructor - virtual ~ICommand() = default; - - // Context setters - virtual void setContext(CommandContext& cntx) = 0; - - // Update command - virtual program_data::CmdResult update(CommandContext& cntx, double dt) = 0; - - // Get current result - virtual program_data::CmdResult currentResult() const = 0; - - // Execute command - virtual void execute() = 0; - - virtual interpreter::IStoredProgram* getProgram() const = 0; - virtual void setProgram(interpreter::IStoredProgram* program) = 0; - - // Mark the command as failed with a message - virtual void markFailed(const std::string& message) = 0; - // Mark the command as completed - virtual void markCompleted() = 0; - // Check if the command has started - virtual bool hasStarted() const = 0; - }; - -} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/IStoredProgram.h b/DSFE_App/DSFE_Core/include/Interpreter/IStoredProgram.h deleted file mode 100644 index 7007dfa1..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/IStoredProgram.h +++ /dev/null @@ -1,83 +0,0 @@ -// DSFE_Core IStoredProgram.h -#pragma once - -#include "EngineCore.h" -#include "ProgramData.h" -#include "Interpreter/Utils.h" -#include -#include - -// Forward declarations -namespace commands { class DSFE_API ICommand; } -namespace scene { class DSFE_API Object; } - -using namespace program_data; - -namespace interpreter { - // IStoredProgram interface - class DSFE_API IStoredProgram { - public: - // Virtual destructor - virtual ~IStoredProgram() = default; - - // Add a command to the program - virtual void add(std::unique_ptr cmd) = 0; - virtual void add(commands::ICommand* cmd) = 0; - - // Reset program to initial state - virtual void reset() = 0; - - // Clear all stored instructions - virtual void clear() = 0; - - // Start program execution - virtual void start() = 0; - // Start simulation - virtual void startSim() = 0; - - // Stop program execution - virtual void stop() = 0; - // Stop simulation - virtual void stopSim() = 0; - - // Puase program execution - virtual void pause() = 0; - // Wait for simulation to run for dt seconds - virtual void waitSim(double dt) = 0; - - // Step the program by dt - virtual void step(double dt) = 0; - // Get current program status - virtual ProgramStatus status() const = 0; - - // State checkers - virtual bool isEmpty() const = 0; - virtual bool isRunning() const = 0; - virtual bool isPaused() const = 0; - virtual bool isStopped() const = 0; - virtual bool isCompleted() const = 0; - virtual bool isFaulted() const = 0; - - // Update the command state - virtual CmdResult updateState() = 0; - - // Set & Get Current line number - virtual void setCurrentLineNumber(int lineNumber) = 0; - virtual int getCurrentLineNumber() const = 0; - - // Set & Get Integrator Method - virtual void setIntegratorMethod(IntegratorMethod method) = 0; - virtual IntegratorMethod getIntegratorMethod() const = 0; - - // Set & Get Omega - virtual void setOmega(mathlib::Vec3 omega, utils::AngularUnits units) = 0; - - // Set & Get Fixed Dt - virtual void setFixedDt(double dt) = 0; - virtual double getFixedDt() const = 0; - - // Set & Get Gravity - virtual void setGravity(double g) = 0; - virtual double getGravity() const = 0; - }; -} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/MainContext.h b/DSFE_App/DSFE_Core/include/Interpreter/MainContext.h deleted file mode 100644 index e01e8ef3..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/MainContext.h +++ /dev/null @@ -1,22 +0,0 @@ -// DSFE_Core MainContext.h -#pragma once - -#include "EngineCore.h" -#include "SimFwd.h" -#include "Interpreter/CommandContext.h" - -namespace commands { - // Main context to combine multiple command contexts for different subsystems (currently only command context) - class DSFE_API MainContext { - public: - MainContext(core::ISimulationCore* core) - : _motion(core) {} - - // Accessors - commands::CommandContext& motion() { return _motion; } - const commands::CommandContext& motion() const { return _motion; } - - private: - commands::CommandContext _motion; - }; -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Parser.h b/DSFE_App/DSFE_Core/include/Interpreter/Parser.h deleted file mode 100644 index 9246bd66..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Parser.h +++ /dev/null @@ -1,44 +0,0 @@ -// DSFE_Core Parser.h -#pragma once - -#include "EngineCore.h" -#include "ProgramData.h" -#include "IStoredProgram.h" -#include "Token.h" -#include -#include -#include - -#include "Platform/Logger.h" - -namespace interpreter { - // Class representing a parsed command - class DSFE_API Parser { - public: - Parser(IStoredProgram* program); - void parse(std::string code); - - private: - IStoredProgram* _program = nullptr; - program_data::ProgramData _programData; - Command _currentCmd; - - std::vector _tokens; - std::vector lines; - size_t pos = 0; - - // --- Helper Functions --- - - // Helpers for parsing - static bool requiresIdentifier(std::string_view cmdName); - static bool matchIdentifier(const std::string& s); - - // Tokenise and classify code into commands - void tokeniseAndClassifyCode(const std::string& code); - - // --- Command and Program Builders --- - - void buildProgram(); - void buildCommand(Command& cmd); - }; -} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h b/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h deleted file mode 100644 index 5ac25d7d..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h +++ /dev/null @@ -1,115 +0,0 @@ -// DSFE_Core ProgramData.h -#pragma once -#include "EngineCore.h" -#include -#include -#include -#include - -namespace program_data { - // Struct representing source location - struct DSFE_API SrcLocation { - std::string filename; // Name of the source file - int line = 0; // Line number in the source file - int column = 0; // Column number in the source file - }; - - // Struct representing a single instruction - struct DSFE_API Command { - std::string rawLine; // The original line of code - std::string cmdName; // The command name - std::string identifier; // The command identifier - std::vector tokens; // The command arguments/tokens - int lineNumber = 0; // Line number in the source code - - // For parallel blocks - bool isParallelBlock = false; - double timeoutSec = 0.0; - std::vector inner; - }; - - // Struct representing program data - struct DSFE_API ProgramData { - std::vector cmd; // Vector storing the instructions - }; - - // Numerical integrator methods - enum class IntegratorMethod { - Euler, - Midpoint, - Heun, - Ralston, - RK4, - RK45, - ImplicitEuler, - ImplicitMidpoint, - GLRK2, - GLRK3, - AD_ImplicitEuler, - AD_ImplicitMidpoint, - AD_GLRK2, - AD_GLRK3 - }; - - // Enum for preset colours - enum class BlockColour { - Red, - Green, - Blue, - Yellow, - Cyan, - Magenta, - White, - Grey, - DarkGrey, - Black, - Custom - }; - - // Enum representing the state of the program - enum ProgramState { - Empty, - Running, - Paused, - Stopped, - Completed, - Faulted - }; - - // Struct for program status - struct DSFE_API ProgramStatus{ - ProgramState state = ProgramState::Empty; - size_t pc = 0; - }; - - // Command states - enum CmdState { - NotStarted, - Executing, - Executed, - Failed - }; - - // Command signals (not used yet, but will be) - enum CmdSignalType { - CmdSignal_None, - CmdSignal_Start, - CmdSignal_Stop, - CmdSignal_Pause, - CmdSignal_Resume, - CmdSignal_Jump - }; - - // Command signal data struct - struct DSFE_API CmdSignalData { - CmdSignalType signal = CmdSignal_None; - size_t jumpTarget = 0; // for jump signals - }; - - // Command result struct - struct DSFE_API CmdResult { - CmdState state = CmdState::NotStarted; - CmdSignalData signalData; - std::string message; - }; -} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/RegisterCommand.h b/DSFE_App/DSFE_Core/include/Interpreter/RegisterCommand.h deleted file mode 100644 index e2a42dda..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/RegisterCommand.h +++ /dev/null @@ -1,9 +0,0 @@ -// DSFE_Core RegisterCommand.h -#pragma once - -#include "CommandFactory.h" - -namespace commands { - // Free function to register all commands - void RegisterAllCommands(CommandFactory& factory); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/RunWrapper.h b/DSFE_App/DSFE_Core/include/Interpreter/RunWrapper.h deleted file mode 100644 index 589bddbb..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/RunWrapper.h +++ /dev/null @@ -1,19 +0,0 @@ -// DSFE_Core RunWrapper.h -#pragma once - -#include "EngineCore.h" -#include "Parser.h" -#include "IStoredProgram.h" - -namespace interpreter { - // Class that wraps the parsing and storing of a program - class DSFE_API RunWrapper { - public: - RunWrapper(Parser* parser, IStoredProgram* program); - // Parse and store the program from a code string - void runProgram(const std::string& code); - private: - Parser* _parser; - IStoredProgram* _program; - }; -} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h b/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h deleted file mode 100644 index fbc718fe..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h +++ /dev/null @@ -1,16 +0,0 @@ -// DSFE_Core SimFwd.h -#pragma once - -#include "EngineCore.h" - -#include -#include -#include -#include -#include - -#include "Platform/Logger.h" - -// Forward declarations for the main classes used in the interpreter -namespace core { struct ISimulationCore; } -namespace robots { class RobotSystem; } diff --git a/DSFE_App/DSFE_Core/include/Interpreter/StoredProgram.h b/DSFE_App/DSFE_Core/include/Interpreter/StoredProgram.h deleted file mode 100644 index 60ce74d2..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/StoredProgram.h +++ /dev/null @@ -1,98 +0,0 @@ -// DSFE_Core StoredProgram.h -#pragma once -#include "EngineCore.h" -#include "IStoredProgram.h" -#include "ICommand.h" -#include "Interpreter/MainContext.h" -#include -#include - -namespace interpreter { - // Class representing a stored program in the interpreter. - class DSFE_API StoredProgram : public IStoredProgram { - public: - // Constructor - StoredProgram(core::ISimulationCore* core); - ~StoredProgram() override; - - // Delete copy constructor and assignment operator to prevent copies - StoredProgram(const StoredProgram&) = delete; - StoredProgram& operator=(const StoredProgram&) = delete; - - // Delete move constructor and assignment operator to prevent moves - StoredProgram(StoredProgram&&) = delete; - StoredProgram& operator=(StoredProgram&&) = delete; - - void add(std::unique_ptr cmd) override; - void add(commands::ICommand* cmd) override; - - void reset() override; - - void clear() override; - - void start() override; - void startSim() override; - - void stop() override; - void stopSim() override; - - void pause() override; - void waitSim(double dt) override; - - // Step the program by dt - void step(double dt) override; - - // Get current program status - ProgramStatus status() const override; - - // State checkers - bool isEmpty() const override { return _commands.empty(); } - bool isRunning() const override { return _state == ProgramState::Running; } - bool isPaused() const override { return _state == ProgramState::Paused; } - bool isStopped() const override { return _state == ProgramState::Stopped; } - bool isCompleted() const override { return _state == ProgramState::Completed; } - bool isFaulted() const override { return _state == ProgramState::Faulted; } - - // Set & Get Current line number - void setCurrentLineNumber(int lineNumber) override { _currentLineNumber = lineNumber; } - int getCurrentLineNumber() const override { return _currentLineNumber; } - - // Set & Get Integrator Method - void setIntegratorMethod(IntegratorMethod method) override; - IntegratorMethod getIntegratorMethod() const override; - - // Set & Get Omega - void setOmega(mathlib::Vec3 omega, utils::AngularUnits units) override; - - // Set & Get Fixed Dt - void setFixedDt(double dt) override; - double getFixedDt() const override; - - // Set & Get Gravity - void setGravity(double gravity) override; - double getGravity() const override; - - private: - core::ISimulationCore* _core = nullptr; - commands::MainContext _cntx; - - // Bool for tracking if the program has reached the end - bool atEnd() const; - // Bool for tracking if there are commands left to execute - bool commandsLeft() const; - - ProgramState _state = ProgramState::Stopped; - bool _stopRequested = false; - - CmdResult updateState() override; - int _currentLineNumber = 0; - int PC = 0; // Program Counter - - std::vector> _commands; - - IntegratorMethod _integratorMethod = IntegratorMethod::RK4; // Default integrator method - double _gravity = 0.0; - double _dt = 0.0; - mathlib::Vec3 _rgb = mathlib::Vec3{ 1.0f, 0.0f, 0.0f }; - }; -} // namespace interpreter diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Token.h b/DSFE_App/DSFE_Core/include/Interpreter/Token.h deleted file mode 100644 index b1991bd7..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Token.h +++ /dev/null @@ -1,31 +0,0 @@ -// DSFE_Core Token.h -#pragma once - -#include "EngineCore.h" -#include - -namespace interpreter { - enum class TokenType { - Unknown, - Comment, - Identifier, - Number, - LParen, - RParen, - LBrace, - RBrace, - Comma, - EndOfLine, - EndOfFile, - String - }; - - // Struct representing a token in the parser - struct DSFE_API Token { - TokenType type = TokenType::Unknown; - std::string value; - double numberValue = 0.0; - int lineNumber = 0; - int columnNumber = 0; - }; -} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Utils.h b/DSFE_App/DSFE_Core/include/Interpreter/Utils.h deleted file mode 100644 index 5c5d1e01..00000000 --- a/DSFE_App/DSFE_Core/include/Interpreter/Utils.h +++ /dev/null @@ -1,68 +0,0 @@ -// DSFE_Core Utils.h -#pragma once - -#include "EngineCore.h" -#include "SimFwd.h" -#include - -#include "Platform/Logger.h" - -namespace utils { - // Struct for operation result - struct DSFE_API OpResult { - bool ok = true; - std::string message; - bool done = false; - - static OpResult Success(bool done=false) { return { true, {}, done}; } - static OpResult Failure(const std::string& msg) { return OpResult{ false, msg, false}; } - }; - - // Struct for axis mask - struct DSFE_API AxisMask { - bool x = false; - bool y = false; - bool z = false; - - bool any() const { return x || y || z; } - }; - - enum class AngularUnits { - DegPerSec, - RadPerSec - }; - - // --- String Utilities --- - std::vector split(const std::string_view s, const std::string_view delimiters); - std::string_view trim(std::string_view str); - std::string toLower(std::string_view str); - std::string toUpper(std::string_view str); - std::string stripBraces(std::string s); - void ignoreCaseCompare(std::string& str); - bool startsWith(const std::string& str, const std::string& prefix); - bool endsWith(const std::string& str, const std::string& suffix); - bool contains(const std::string& str, const std::string& substr); - - // --- Type Checking Utilities --- - bool isInteger(const std::string_view s); - bool isFloat(const std::string_view s); - bool isDouble(const std::string_view s); - bool isBoolean(const std::string_view s); - - // --- Conversion Utilities --- - std::optional toBoolean(const std::string_view s); - - // --- Command Utilities --- - double parseDouble(const std::string_view s); - float parseFloat(const std::string s); - mathlib::Vec3 parseVec3(const std::string& str); - AxisMask parseAxisMask(const std::string& s); - //bool tryParseObjID(const std::string& s, scene::ObjectID& out); - - // --- Unit Conversion Utilities --- - double degToRad(double degrees); - mathlib::Vec3 degToRad(mathlib::Vec3& degrees); - - double radToDeg(double radians); - mathlib::Vec3 radToDeg(mathlib::Vec3& radians); -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h deleted file mode 100644 index 0a12ea49..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ /dev/null @@ -1,191 +0,0 @@ -// DSFE_Core DynamicsTypes.h -#pragma once - -#include "EngineCore.h" -#include -#include - -#include "Robots/RobotMetrics.h" - -namespace robots { - // Scratch buffers for dense dynamics - template - struct DenseDynamicsScratch { - mathlib::MatX_T M; // mass matrix - mathlib::VecX_T rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) - mathlib::VecX_T h; // Coriolis and centrifugal bias vector - mathlib::VecX_T tau; // control torque vector - mathlib::VecX_T I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) - - std::vector> T_world; - std::vector> jointWorldPoses; - - size_t jointCap = 0; - size_t linkCap = 0; - - // Resizes the scratch buffers - void resize(size_t nJoints, size_t nLinks) { - // Do not resize if the current capacities are sufficients - if (jointCap == nJoints - && linkCap == nLinks) { - return; - } - - // Dense buffers - M.resize(nJoints, nJoints); - rhs.resize(nJoints); - h.resize(nJoints); - tau.resize(nJoints); - I_eff_controller.resize(nJoints); - T_world.resize(nLinks); - jointWorldPoses.resize(nJoints); - - jointCap = nJoints; - linkCap = nLinks; - } - - // Sets all buffers to zero or identity - // * (ONLY FOR DEBUGGING PURPOSES, CALL clear() FOR PRODUCTION USE) - void zero() { - // Dense buffers - M.setZero(); - rhs.setZero(); - h.setZero(); - tau.setZero(); - I_eff_controller.setZero(); - for (auto& T : T_world) T.setIdentity(); - for (auto& T : jointWorldPoses) T.setIdentity(); - } - - void clear() { - // Dense buffers - M.resize(0, 0); - rhs.resize(0); - h.resize(0); - tau.resize(0); - I_eff_controller.resize(0); - T_world.clear(); - jointWorldPoses.clear(); - jointCap = 0; - linkCap = 0; - } - }; - - // Scratch buffers for spatial dynamics computations - template - struct SpatialDynamicsScratch { - std::vector> Xup; // spatial transformation from parent to current link - std::vector> IA; // articulated body inertia - std::vector> Ia; // articulated body inertia in the link frame - - std::vector> v; // spatial velocity - std::vector> c; // spatial bias acceleration - std::vector> a; // spatial acceleration - std::vector> pA; // articulated bias force - std::vector> U; // articulated body force - std::vector> f_ext; // spatial force - - mathlib::VecX_T u; // joint force contribution - mathlib::VecX_T d; // joint inertia contribution - - std::vector>> dXup_dq; // derivative of spatial transformation w.r.t. joint angles - - std::vector>> dv_dq; // derivative of spatial velocity w.r.t. joint angles - std::vector>> dv_dqd; // derivative of spatial velocity w.r.t. joint velocities - std::vector>> dc_dq; // derivative of spatial bias acceleration w.r.t. joint angles - std::vector>> dc_dqd; // derivative of spatial bias acceleration w.r.t. joint velocities - - size_t jointCap = 0; - - // Resizes the scratch buffers - void resize(size_t nJoints) { - // Do not resize if the current capacities are sufficient - if (jointCap == nJoints) { return; } - - // Spatial buffers - Xup.resize(nJoints); - IA.resize(nJoints); - Ia.resize(nJoints); - v.resize(nJoints); - c.resize(nJoints); - a.resize(nJoints); - pA.resize(nJoints); - U.resize(nJoints); - f_ext.resize(nJoints); - - u.resize(nJoints); - d.resize(nJoints); - - dXup_dq.resize(nJoints); - dv_dq.resize(nJoints); - dv_dqd.resize(nJoints); - dc_dq.resize(nJoints); - dc_dqd.resize(nJoints); - - jointCap = nJoints; - } - - void clear() { - Xup.clear(); - IA.clear(); - Ia.clear(); - v.clear(); - c.clear(); - a.clear(); - pA.clear(); - U.clear(); - f_ext.clear(); - - u.resize(0); - d.resize(0); - - dXup_dq.clear(); - dv_dq.clear(); - dv_dqd.clear(); - dc_dq.clear(); - dc_dqd.clear(); - - jointCap = 0; - } - }; - - // Output structure for dynamics computations - template - struct DynamicsResult { - mathlib::VecX_T dxdt; - mathlib::VecX_T qdd; - - RobotMetrics metrics; - - void resize(size_t n) { - dxdt.resize(2 * n); - qdd.resize(n); - metrics.resize(n); - } - }; - - // Central scratch structure that contains all buffers needed for dynamics computations, both dense and spatial - template - struct DynamicsScratch { - DenseDynamicsScratch dense; - SpatialDynamicsScratch spatial; - // TODO add kinematics scratch - - // Gravity scratch buffer - mathlib::VecX_T g; - - // Resizes all scratch buffers using the given number of joints and links - void resize(size_t nJoints, size_t nLinks) { - dense.resize(nJoints, nLinks); - spatial.resize(nJoints); - g.resize(nJoints); - } - - // Clears all scratch buffers - void clear() { - dense.clear(); - spatial.clear(); - g.resize(0); - } - }; -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h deleted file mode 100644 index 0de235ba..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ /dev/null @@ -1,168 +0,0 @@ -// DSFE_Core RobotDynamics.h -#pragma once - -#include "EngineCore.h" -#include - -#include "Robots/DynamicsTypes.h" -#include "Robots/RobotMetrics.h" - -#include "Robots/SpatialDynamics.h" -#include "Robots/SpatialModel.h" -#include "Robots/RobotKinematics.h" -#include "Robots/RobotSimSnapshot.h" - -#include "Robots/TrajectoryManager.h" - -#include -#include - -#include "EngineLib/LogMacros.h" - -// Forward declarations -namespace control { class TrajectoryManager; } -namespace integration { class IntegrationService; enum class eIntegrationMethod; } - -namespace robots { - // Forward declarations - struct RobotLink; - struct RobotJoint; - enum class eTorqueMode; - - // Dynamics class responsible for computing inertia, mass matrix, gravity torque, control torques, and state derivatives - class DSFE_API RobotDynamics { - public: - // Constructor - RobotDynamics(); - - // Computes the inertia tensor of a robot link - template - mathlib::Mat3_T computeLinkInertiaTensor(const RobotLink& link) const; - - // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint - template - Scalar computeJointInertiaContribution( - const RobotJoint& joint, - const RobotLink& link, - const mathlib::Pose_T& jointWorldPose, - const mathlib::Pose_T& linkWorldPose - ) const; - - // Computes the full mass matrix M(q) based on the current state and robot configuration - template - void computeMassMatrix( - const RobotConstModel& robot, - const std::vector>& T_world, - const std::vector>& jointWorldPoses, - mathlib::MatX_T& M_out - ) const; - - // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration - template - mathlib::VecX_T computeCoriolisVector( - const RobotConstModel& robot, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - const std::vector>& T_world, - const mathlib::MatX_T& M - ) const; - - // Computes the gravity torque for a joint based on the current state and robot configuration - template - mathlib::VecX_T computeGravityTorque( - const RobotConstModel& robot, - const std::vector>& T_world, - const std::vector>& jointWorldPoses - ) const; - - // Computes the analytical Jacobian matrix J(q) for the robot based on the current state and robot configuration - template - void analyticalJacobian( - const RobotConstModel& robot, - const mathlib::VecX_T& x, - mathlib::MatX_T& J_out, - DenseDynamicsScratch& scratch - ); - - // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration - template - mathlib::VecX_T derivative_dense( - Scalar t, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ); - - template - mathlib::VecX_T derivative_spatial( - const robots::SpatialModel& model, - Scalar t, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ); - - template - void jacobian_spatial( - const robots::SpatialModel& model, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - const mathlib::VecX_T& kp, - const mathlib::VecX_T& kd, - mathlib::MatX_T& F_out, - DynamicsScratch& scratch - ); - - // Computes the derivative of the state vector with control gains based on the current state and robot configurations - template - mathlib::VecX_T derivative_with_gains( - Scalar t, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - const mathlib::VecX_T& kp, - const mathlib::VecX_T& kd, - DynamicsScratch& scratch, - DynamicsResult& out - ); - - // Computes the Jacobian matrix with control gains based on the current state and robot configuration - template - void jacobian_with_gains( - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - const mathlib::VecX_T& kp, - const mathlib::VecX_T& kd, - mathlib::MatX_T& F_out, - DenseDynamicsScratch& scratch - ); - - // Set the gravity strength for the robot system - void setGravity(double gravity) { _gravity = gravity; } - const double getGravity() const { return _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 - std::unique_ptr _kinematics = nullptr; - - static bool isControlledJoint(eJointType t) { - return - t == eJointType::REVOLUTE || - t == eJointType::PRISMATIC; - } - - double _dt = 1.0 / 180.0; // default timestep for dynamics updates - - double _gravity{ 0.0 }; - bool _baseIsFree = false; - double _lastBaseForwardForce{ 0.0 }; - }; -} // namespace robots - - -#include "Robots/RobotDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl deleted file mode 100644 index 3469ab99..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ /dev/null @@ -1,644 +0,0 @@ -// DSFE_Core RobotDynamics.inl -#pragma once - -namespace robots { - // Computes the inertia tensor of a robot link - template - mathlib::Mat3_T RobotDynamics::computeLinkInertiaTensor(const RobotLink& link) const { - const robots::Inertia& I = link.inertial.inertia; - - // Construct the inertia tensor matrix - mathlib::Mat3_T M = mathlib::Mat3_T::Zero(); - M << - I.ixx, I.ixy, I.ixz, - I.ixy, I.iyy, I.iyz, - I.ixz, I.iyz, I.izz; - - return M; // [kg*m^2], (3x3) inertia tensor in link frame - } - - // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint - template - Scalar RobotDynamics::computeJointInertiaContribution( - const RobotJoint& joint, - const RobotLink& link, - const mathlib::Pose_T& jointWorldPose, - const mathlib::Pose_T& linkWorldPose - ) const { - const Scalar m = (Scalar)link.inertial.mass; - - // Rotation from link frame to world frame - mathlib::Mat3_T R_joint = jointWorldPose.template block<3, 3>(0, 0); - // Joint axis in world frame - mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_joint * joint.axis); - - // Position of joint in world frame - mathlib::Vec3_T joint_pos_world = jointWorldPose.template block<3, 1>(0, 3); - - // Rotation from link frame to world frame - mathlib::Mat3_T R_link = linkWorldPose.template block<3, 3>(0, 0); - mathlib::Vec3_T link_pos_world = linkWorldPose.template block<3, 1>(0, 3); - mathlib::Vec3_T com_world = R_link * Vec3_T(link.inertial.com_xyz) + link_pos_world; - - // Translational contribution (parallel axis theorem) - mathlib::Vec3_T r = com_world - joint_pos_world; - - // Translational contribution to inertia about the joint axis - Scalar I_trans = m * (axis_world.cross(r)).squaredNorm(); - - // Rotational contribution - mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); - mathlib::Mat3_T I_world = R_link * I_local * R_link.transpose(); - - // Rotational contribution to inertia about the joint axis - Scalar I_rot = axis_world.transpose() * I_world * axis_world; - Scalar I_eff_i = I_trans + I_rot; - - return Eigen::numext::maxi(I_eff_i, Scalar(1e-6)); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities - } - - // Computes the full mass matrix M(q) based on the current state and robot configuration - template - void RobotDynamics::computeMassMatrix( - const RobotConstModel& robot, - const std::vector>& T_world, - const std::vector>& jointWorldPoses, - mathlib::MatX_T& M_out - ) const { - const size_t n = robot.joints.size(); - M_out.resize(n, n); - M_out.setZero(); - - // Compute its contribution to the mass matrix for each link - for (size_t k = 0; k < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; - const Scalar m = link.inertial.mass; - - if (m <= Scalar(0)) { continue; } - - const mathlib::Mat3_T R = T_world[k].template block<3, 3>(0, 0); // Rotation from link frame to world frame - const mathlib::Vec3_T p = T_world[k].template block<3, 1>(0, 3); // Center of mass of the link in world frame - const mathlib::Vec3_T com = R * link.inertial.com_xyz + p; // Center of mass in world frame - - mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); // inertia tensor in link frame - mathlib::Mat3_T I_world = R * I_local * R.transpose(); // inertia tensor in world frame - - // Compute Jacobian columns for each joint and accumulate mass matrix contributions - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j_i = robot.joints[i]; - if (j_i.type == eJointType::FIXED) { continue; } - if (!robot.jointAffectsLink(i, k)) { continue; } // skip if joint i does not affect link k - - const mathlib::Pose_T& T_joint_i = jointWorldPoses[i]; // pose of joint i in world frame - - // Rotation from joint i frame to world frame - const mathlib::Mat3_T R_i = T_joint_i.template block<3, 3>(0, 0); // rotation from joint i frame to world frame - const mathlib::Vec3_T p_i = T_joint_i.template block<3, 1>(0, 3); // joint position in world frame - const mathlib::Vec3_T z_i = mathlib::safeNormalised(R_i * j_i.axis); // joint axis in world frame - - mathlib::Vec3_T J_vi = z_i.cross(com - p_i); // linear velocity Jacobian column for joint i - mathlib::Vec3_T J_wi = z_i; // angular velocity Jacobian column for joint i - - // Computes the contribution to the mass matrix from this link for joints i and j - for (size_t j = 0; j < n; ++j) { - const RobotJoint& j_j = robot.joints[j]; - if (j_j.type == eJointType::FIXED) { continue; } - - if (!robot.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k - - const mathlib::Pose_T& T_joint_j = jointWorldPoses[j]; // pose of joint i in world frame - - const mathlib::Mat3_T R_j = T_joint_j.template block<3, 3>(0, 0); - const mathlib::Vec3_T p_j = T_joint_j.template block<3, 1>(0, 3); - const mathlib::Vec3_T z_j = mathlib::safeNormalised(R_j * j_j.axis); - - mathlib::Vec3_T J_vj = z_j.cross(com - p_j); // linear velocity Jacobian column for joint j - mathlib::Vec3_T J_wj = z_j; // angular velocity Jacobian column for joint j - - M_out(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; // [kg*m^2] - } - } - } - } - - // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration - template - mathlib::VecX_T RobotDynamics::computeCoriolisVector( - const RobotConstModel& robot, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - const std::vector>& T_world, - const mathlib::MatX_T& M - ) const { - const size_t n = robot.joints.size(); - const Scalar eps = Scalar(1e-6); // small value to prevent division by zero - mathlib::VecX_T q_eps = q; - - std::vector> dM_dq(n, mathlib::MatX_T::Zero(n, n)); // partial derivatives of M with respect to each joint angle - mathlib::VecX_T x_eps(2 * n); // state vector for kinematics - - std::vector> T_world_eps; // forward kinematics for perturbed configurations - T_world_eps.resize(robot.links.size()); - - std::vector> jointWorldPoses_eps; - jointWorldPoses_eps.resize(n); - - mathlib::MatX_T M_plus(n, n); - - // Finite difference approximation of dM/dq for each joint - for (size_t k = 0; k < n; ++k) { - q_eps = q; // reset to original configuration for each joint perturbation - q_eps[k] += eps; // perturb joint k by a small amount - - // Construct the state vector for the perturbed configuration - for (size_t i = 0; i < n; ++i) { - x_eps[i] = q_eps[i]; - x_eps[n + i] = qd[i]; - } - - // Compute forward kinematics for the perturbed state - _kinematics->computeForwardKinematics_fromState(robot, x_eps, T_world_eps); - jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, robot); - computeMassMatrix(robot, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration - - if (!M_plus.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } - if (!M_plus.isApprox(M_plus, Scalar(1e-8))) { throw std::runtime_error("Mass matrix lost symmetry"); } - - dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with - } - - // Compute Coriolis and centrifugal bias vector h using Christoffel symbols of the first kind - VecX_T h = VecX_T::Zero(n); - for (size_t i = 0; i < n; ++i) { - for (size_t j = 0; j < n; ++j) { - for (size_t k = 0; k < n; ++k) { - Scalar C_ijk = 0.5 * (dM_dq[k](i, j) + dM_dq[j](i, k) - dM_dq[i](j, k)) * qd[k]; // Christoffel symbol of the first kind for indices (i, j, k) - h(i) += C_ijk * qd[j] * qd[k]; // contribution to Coriolis and centrifugal bias for joint i from joints j and k - } - } - } - return h; // [Nm], Coriolis and centrifugal bias vector for the robot at configuration q and velocity qd - } - - // Computes the gravity torque for a joint based on the current state and robot configuration - template - mathlib::VecX_T RobotDynamics::computeGravityTorque( - const RobotConstModel& robot, - const std::vector>& T_world, - const std::vector>& jointWorldPoses - ) const { - const size_t n = robot.joints.size(); - mathlib::VecX_T tau_G = mathlib::VecX_T::Zero(n); - Scalar g{ _gravity }; // [m/s^2], gravity acceleration magnitude - - // For each joint, sum the gravity contributions from all links - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = robot.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - - Scalar tau_g_i = Scalar(0); // [Nm], gravity torque contribution for joint i - - const mathlib::Pose_T& T_joint = jointWorldPoses[i]; // pose of joint i in world frame - - const mathlib::Mat3_T R_i = T_joint.template block<3, 3>(0, 0); - const mathlib::Vec3_T p_i = T_joint.template block<3, 1>(0, 3); - const mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_i * robot.joints[i].axis); - - // For each link, compute the gravitational force and its torque contribution about joint i - for (size_t k = 0; k < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; - const Scalar m = (Scalar)link.inertial.mass; - if (m <= Scalar(0)) { continue; } - - if (!robot.jointAffectsLink(i, k)) { continue; } - - // Link's center of mass in world frame - const mathlib::Mat3_T R_k = T_world[k].template block<3, 3>(0, 0); - const mathlib::Vec3_T com_world = R_k * link.inertial.com_xyz + T_world[k].template block<3, 1>(0, 3); - - // Gravitational force on the link - mathlib::Vec3_T g_world; - g_world = mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame - const mathlib::Vec3_T F_g = m * g_world; // [N], gravitational force on the link in world frame - const mathlib::Vec3_T r = com_world - p_i; // [m] - - // Torque = r × F_g projected onto joint axis - tau_g_i += axis_world.dot(r.cross(F_g)); - } - tau_G[i] = tau_g_i; - } - return tau_G; // [Nm], gravity torques for each joint - } - - // Computes the analytical Jacobian matrix J(q) for the robot based on the current state and robot configuration - template - void RobotDynamics::analyticalJacobian( - const RobotConstModel& robot, - const mathlib::VecX_T& x, - mathlib::MatX_T& J_out, - DenseDynamicsScratch& scratch - ) { - const size_t n = robot.joints.size(); - - Eigen::Map> q_local(x.data(), n); - Eigen::Map> qd_local(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(robot, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); - computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); - - J_out.setZero(2 * n, 2 * n); // [rad/rad] position part, [rad/s / rad/s] velocity part - J_out.block(0, n, n, n).setIdentity(); - - mathlib::MatX_T dTau_dq = mathlib::MatX::Zero(n, n); - mathlib::MatX_T dTau_dv = mathlib::MatX::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = robot.joints[i]; - if (joint.type == eJointType::FIXED) { continue; } - - const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency - const Scalar z = static_cast(joint.zeta_target); // damping ratio - const Scalar eps = static_cast(1e-6); - - const Scalar I_eff = mathlib::LSE_smoothMax(scratch.M(i, i), eps); - - const Scalar k_p = I_eff * wn * wn; - const Scalar k_d = Scalar(2) * z * I_eff * wn; - - const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient - const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient - const Scalar eps_f = static_cast(1e-2); - - dTau_dq(i, i) = -k_p; - - Scalar qd_i = qd_local[i]; - Scalar tanh_term = mathlib::tanh(qd_i / eps_f); - Scalar stiff_friction_slope = c * (Scalar(1) - tanh_term * tanh_term) / eps_f; - dTau_dv(i, i) = -k_d - b + stiff_friction_slope; - } - - auto solver = scratch.M.ldlt(); - mathlib::MatX_T da_dq = solver.solve(dTau_dq); - mathlib::MatX_T da_dv = solver.solve(dTau_dv); - - J_out.block(n, 0, n, n) = da_dq; - J_out.block(n, n, n, n) = da_dv; - } - - // Computes the Coriolis and centrifugal torque for a joint based on the current state and robot configuration - template - mathlib::VecX_T RobotDynamics::derivative_dense( - Scalar t, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - mathlib::VecX_T dx(2 * n); - - // Map the input state vector to joint angles and velocities - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); - - computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the robot at configuration q - scratch.dense.h = mathlib::VecX_T::Zero(n); // Temp test to isolate potential issues - //scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector - - if (!scratch.dense.M.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } - - scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = snap.model->joints[i]; - - // Fixed joints - if (joint.type == eJointType::FIXED) { - out.metrics.q[i] = q[i]; - out.metrics.qd[i] = qd[i]; - out.metrics.qdd[i] = 0.0; - continue; - } - - const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency - const Scalar z = static_cast(joint.zeta_target); // damping ratio - - const Scalar err = snap.q_ref[i] - q[i]; // [rad], position error - const Scalar err_d = snap.qd_ref[i] - qd[i]; // [rad/s], velocity error - - const Scalar eps = static_cast(1e-6); - - const Scalar I_eff = mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps); // [kg*m^2], effective inertia for joint i with floor to prevent singularities - const Scalar k_p = I_eff * wn * wn; // [Nm/rad], proportional gain - const Scalar k_d = Scalar(2.0) * z * I_eff * wn; // [Nm/(rad/s)], derivative gain - - const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient - const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient - const Scalar eps_f = static_cast(1e-2); - - Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i - tau_i += scratch.g[i]; // Gravity compensation - tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias - // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation - // tau_i += tau_f; - - scratch.dense.tau[i] = tau_i; - - out.metrics.q[i] = mathlib::real(q[i]); - out.metrics.qd[i] = mathlib::real(qd[i]); - - out.metrics.err[i] = mathlib::real(err); - out.metrics.errd[i] = mathlib::real(err_d); - - out.metrics.I_eff[i] = mathlib::real(I_eff); - out.metrics.tau[i] = mathlib::real(tau_i); - } - - // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g - - // Solve for Accelerations - Eigen::LDLT> solver(scratch.dense.M); - - if (solver.info() != Eigen::Success) { - LOG_ERROR("LDLT decomposition failed for mass matrix M. Matrix may be singular or ill-conditioned."); - throw std::runtime_error("LDLT decomposition failed for mass matrix M"); - } - - out.qdd = solver.solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics - - if (!out.qdd.allFinite()) { - LOG_ERROR("Non-finite joint accelerations computed. Check for singularities or numerical issues in the mass matrix."); - throw std::runtime_error("Non-finite joint accelerations computed from dynamics"); - } - - out.metrics.qdd = out.qdd; - - // Fill derivatives - dx.head(n) = qd; - dx.tail(n) = out.qdd; - - return dx; - } - - template - mathlib::VecX_T RobotDynamics::derivative_spatial( - const robots::SpatialModel& model, - Scalar t, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = model.joints.size(); - mathlib::VecX_T dx(2 * n); - - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); - - // Quick guard: check for non-finite states and bail with zero derivative - for (size_t i = 0; i < n; ++i) { - double q_r = mathlib::real(q[i]); - double qd_r = mathlib::real(qd[i]); - if (!std::isfinite(q_r) || !std::isfinite(qd_r)) { - LOG_ERROR("Non-finite state detected in derivative_spatial: q[%zu]=%g qd[%zu]=%g", i, q_r, i, qd_r); - // Return zero derivative to avoid propagating NaNs - dx.setZero(); - out.qdd.setZero(); - return dx; - } - } - - SpatialDynamics::computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(n); - mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(n); - mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, qd_zero, qdd_zero, scratch); - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { - scratch.dense.tau[i] = Scalar(0); - continue; - } - - const Scalar wn = static_cast(snap.model->joints[i].wn_target); - const Scalar z = static_cast(snap.model->joints[i].zeta_target); - - const Scalar err = snap.q_ref[i] - q[i]; - const Scalar err_d = snap.qd_ref[i] - qd[i]; - - const Scalar eps = static_cast(1e-6); - - const Scalar I_eff = mathlib::LSE_smoothMax(M(i, i), eps); - const Scalar k_p = I_eff * wn * wn; - const Scalar k_d = Scalar(2) * z * I_eff * wn; - - const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient - const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient - const Scalar eps_f = static_cast(1e-3); - - Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - Scalar tau_f = c * mathlib::tanh(qd[i] / Scalar(0.1)) + b * qd[i]; // simple friction model with viscous and Coulomb friction - tau_i += tau_f; - - const Scalar Q_max = static_cast(snap.model->joints[i].limits.maxEffort); - LOG_INFO_ONCE("Max effort for joint %zu: %g Nm", i, mathlib::real(Q_max)); - - tau_i = Q_max * mathlib::tanh(tau_i / Q_max); // saturate control torque to max effort using smooth tanh saturation - - scratch.dense.tau[i] = tau_i; - - out.metrics.q[i] = mathlib::real(q[i]); - out.metrics.qd[i] = mathlib::real(qd[i]); - - out.metrics.err[i] = mathlib::real(err); - out.metrics.errd[i] = mathlib::real(err_d); - - out.metrics.I_eff[i] = mathlib::real(I_eff); - out.metrics.tau[i] = mathlib::real(tau_i); - } - - out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); - out.metrics.qdd = out.qdd; - dx.head(n) = qd; - dx.tail(n) = out.qdd; - return dx; - } - - template - void RobotDynamics::jacobian_spatial( - const robots::SpatialModel& model, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - const mathlib::VecX_T& kp, - const mathlib::VecX_T& kd, - mathlib::MatX_T& F_out, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - - F_out.setZero(2 * n, 2 * n); - F_out.block(0, n, n, n).setIdentity(); - - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); - - SpatialDynamics::computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - scratch.dense.M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - - mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); - mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { continue; } - dTau_dq(i, i) = -kp[i]; - const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient - const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient - const Scalar eps_f = static_cast(1e-2); - - dTau_dv(i, i) = -kd[i] - b; - } - - Eigen::LDLT> solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving - - mathlib::MatX_T dqdd_dtau_q = solver.solve(dTau_dq); // compute the partial derivative of qdd with respect to q - - if (solver.info() != Eigen::Success) { - LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); - throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); - } - - mathlib::MatX_T dqdd_dtau_v = solver.solve(dTau_dv); // compute the partial derivative of qdd with respect to qd - - if (solver.info() != Eigen::Success) { - LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); - throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); - } - - F_out.block(n, 0, n, n) = dqdd_dtau_q; // fill the Jacobian block for qdd with respect to q - F_out.block(n, n, n, n) = dqdd_dtau_v; // fill the Jacobian block for qdd with respect to qd) - } - - // Computes the derivative of the state vector with control gains based on the current state and robot configurations - template - mathlib::VecX_T RobotDynamics::derivative_with_gains( - Scalar t, - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - const mathlib::VecX_T& kp, - const mathlib::VecX_T& kd, - DynamicsScratch& scratch, - DynamicsResult& out - ) { - const size_t n = snap.model->joints.size(); - mathlib::VecX_T dx(2 * n); - - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); - - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); - scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); - - computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); - scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); - - scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); - } - - scratch.dense.tau.setZero(); - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = snap.model->joints[i]; - if (joint.type == eJointType::FIXED) continue; - - const Scalar eps = static_cast < Scalar>(1e-6); - const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient - const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient - const Scalar eps_f = static_cast(1e-2); - - Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; - tau_i += scratch.g[i] + scratch.dense.h[i]; - tau_i -= b * qd[i]; - tau_i -= c * mathlib::tanh(qd[i] / eps_f); - - scratch.dense.tau[i] = tau_i; - } - - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; - out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); - out.metrics.qdd = out.qdd; - - dx.head(n) = qd; - dx.tail(n) = out.qdd; - return dx; - } - - // Computes the Jacobian matrix with control gains based on the current state and robot configuration - template - void RobotDynamics::jacobian_with_gains( - const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, - const mathlib::VecX_T& kp, - const mathlib::VecX_T& kd, - mathlib::MatX_T& F_out, - DenseDynamicsScratch& scratch - ) { - const size_t n = snap.model->joints.size(); - - F_out.setZero(2 * n, 2 * n); - F_out.block(0, n, n, n).setIdentity(); - - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); - - // Compute a local mass matrix for this exact stage evaluation frame - _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); - computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); - - mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); - mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); - - for (size_t i = 0; i < n; ++i) { - const RobotJoint& joint = snap.model->joints[i]; - if (joint.type == eJointType::FIXED) continue; - - dTau_dq(i, i) = -kp[i]; - - const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient - const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient - const Scalar eps_f = static_cast(1e-2); - - Scalar tanh_term = mathlib::tanh(qd[i] / eps_f); - Scalar stiff_friction = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; - dTau_dv(i, i) = -kd[i] - b + stiff_friction; - } - - auto solver = scratch.M.ldlt(); - F_out.block(n, 0, n, n) = solver.solve(dTau_dq); - F_out.block(n, n, n, n) = solver.solve(dTau_dv); - } -} // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h deleted file mode 100644 index d462dd03..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ /dev/null @@ -1,50 +0,0 @@ -// DSFE_Core RobotKinematics.h -#pragma once - -#include "EngineCore.h" -#include -#include -#include "Robots/RobotSimSnapshot.h" - -#include "EngineLib/LogMacros.h" - -namespace robots { - // Forward declarations - struct RobotLink; - struct RobotJoint; - - // Kinematics class responsible for computing forward kinematics and related transformations - class DSFE_API RobotKinematics { - public: - // Constructor - RobotKinematics(); - - // Computes the forward kinematics for the robot based on the current state and robot configuration - template - void computeForwardKinematics_fromState( - const RobotConstModel& robot, - const mathlib::VecX_T& x, - std::vector>& T_world_out - ) const; - - // Computes the joint world poses for all joints based on the current state and robot configuration - template - std::vector> calcJointWorldPoses( - const std::vector>& T_world, - const RobotConstModel& robot - ); - - // Computes the forward kinematics for a single joint motion based on the joint axis and angle - template - mathlib::Pose_T jointMotionTransform( - const mathlib::Vec3_T& axis_joint, - Scalar q - ) const; - - // Converts roll-pitch-yaw angles (in radians) to a quaternion representation - template - mathlib::Quat_T rpyRadToQuat(const mathlib::Vec3_T& rpyRad); - }; -} - -#include "Robots/RobotKinematics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl deleted file mode 100644 index 84ed0d79..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl +++ /dev/null @@ -1,105 +0,0 @@ -// DSFE_Core RobotKinematics.inl -#pragma once - -namespace robots { - // Computes the forward kinematics for the robot based on the current state and robot configuration - template - void RobotKinematics::computeForwardKinematics_fromState( - const RobotConstModel& robot, - const mathlib::VecX_T& x, - std::vector>& T_world_out - ) const { - const auto& joints = robot.joints; - const auto& links = robot.links; - const size_t n = joints.size(); - - T_world_out.resize(robot.links.size()); - - if (T_world_out.empty()) { - LOG_ERROR("T_world_out is empty"); - return; - } - - mathlib::Pose_T T = mathlib::Pose_T::Identity(); // world -> base - T_world_out[0] = T; // base link - - // Compute the transform to the next link using each joint - for (size_t i = 0; i < n; ++i) { - const auto& joint = joints[i]; - const Scalar q = x[i]; // joint angle from state vector - - mathlib::Pose_T T_origin = mathlib::Pose_T::Identity(); // transform from parent link to joint frame (fixed) - mathlib::Quat_T q_origin = joint.origin_q.template cast(); // convert quaternion to correct scalar type - - T_origin.template block<3, 3>(0, 0) = q_origin.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON - T_origin.template block<3, 1>(0, 3) = joint.origin_xyz.template cast(); // translation from parent link to joint frame - - // Compute joint motion transform based on joint axis and angle - Pose_T T_motion = mathlib::Pose_T::Identity(); - if (joint.type == eJointType::REVOLUTE) { - T_motion = jointMotionTransform(joint.axis.template cast(), q); // rotation about joint axis - } - else if (joint.type == eJointType::PRISMATIC) { - T_motion.template block<3, 1>(0, 3) = mathlib::safeNormalised(joint.axis) * q; // translation along joint axis - } - - // compose transforms - T = T * T_origin * T_motion; // parent -> joint -> motion -> child - - int childIdx = robot.linkIndex(joint.child); - if (childIdx < 0 || childIdx >= T_world_out.size()) { - LOG_ERROR("Invalid child link index for joint {}: {}", joint.name.c_str(), childIdx); - continue; - } - - T_world_out[childIdx] = T; // world -> child link - } - } - - // Computes the joint world poses for all joints based on the current state and robot configuration - template - std::vector> RobotKinematics::calcJointWorldPoses( - const std::vector>& T_world, - const RobotConstModel& robot - ) { - std::vector> jointWorldPoses(robot.joints.size()); - - for (size_t i = 0; i < robot.joints.size(); ++i) { - const RobotJoint& joints = robot.joints[i]; - int childIdx = robot.linkIndex(joints.child); - - if (childIdx < 0 || childIdx >= T_world.size()) { - LOG_ERROR("Invalid child link index for joint {}: {}", joints.name.c_str(), childIdx); - continue; - } - - jointWorldPoses[i] = T_world[childIdx]; - } - return jointWorldPoses; - } - - // Computes the forward kinematics for a single joint motion based on the joint axis and angle - template - mathlib::Pose_T RobotKinematics::jointMotionTransform( - const mathlib::Vec3_T& axis_joint, - Scalar q - ) const { - mathlib::Pose_T T = mathlib::Pose_T::Identity(); // homogeneous transformation matrix (4x4) - T.template block<3, 3>(0, 0) = mathlib::AngleAxis(q, mathlib::safeNormalised(axis_joint)); // set upper-left 3x3 block to rotation matrix - return T; // (4x4) homogeneous transformation - } - - // Converts roll-pitch-yaw angles (in radians) to a quaternion representation - template - mathlib::Quat_T RobotKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { - const Scalar roll = rpyRad.x(); - const Scalar pitch = rpyRad.y(); - const Scalar yaw = rpyRad.z(); - - const Quat_T qx(Eigen::AngleAxis(roll, mathlib::Vec3_T(Scalar(1), Scalar(0), Scalar(0)))); - const Quat_T qy(Eigen::AngleAxis(pitch, mathlib::Vec3_T(Scalar(0), Scalar(1), Scalar(0)))); - const Quat_T qz(Eigen::AngleAxis(yaw, mathlib::Vec3_T(Scalar(0), Scalar(0), Scalar(1)))); - - return (qz * qy * qx).normalized(); - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotLoader.h b/DSFE_App/DSFE_Core/include/Robots/RobotLoader.h deleted file mode 100644 index 0ded2ad7..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotLoader.h +++ /dev/null @@ -1,12 +0,0 @@ -// DSFE_Core RobotLoader.h -#pragma once - -#include "EngineCore.h" -#include "Robots/RobotModel.h" - -namespace robots { - class DSFE_API RobotLoader { - public: - static RobotModel loadFromJSON(const std::string& filepath); - }; -} // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h b/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h deleted file mode 100644 index 0e1d1608..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h +++ /dev/null @@ -1,45 +0,0 @@ -// DSFE_Core RobotMetrics.h -#pragma once - -#include "EngineCore.h" - - -namespace robots { - // Per-joint metrics - template - struct RobotMetrics { - // State - mathlib::VecX_T q; - mathlib::VecX_T qd; - mathlib::VecX_T qdd; - - mathlib::VecX_T err; - mathlib::VecX_T errd; - - // Dynamics - mathlib::VecX_T I_eff; - mathlib::VecX_T tau; - - // Constraints / realism - mathlib::VecX_T tau_barrier; - mathlib::VecX_T tau_sat; - - // Energy, Work, & Power - mathlib::VecX_T KE; - mathlib::VecX_T PE; - mathlib::VecX_T E_total; - mathlib::VecX_T W_actuator; - - // Stability flags - std::vector sat_flag; - - void resize(size_t n) { - q.resize(n); qd.resize(n); qdd.resize(n); - err.resize(n); errd.resize(n); - I_eff.resize(n); tau.resize(n); - tau_barrier.resize(n); tau_sat.resize(n); - KE.resize(n); PE.resize(n); E_total.resize(n); W_actuator.resize(n); - sat_flag.resize(n, 0); - } - }; -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Robots/RobotModel.h deleted file mode 100644 index ee2dd43e..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ /dev/null @@ -1,210 +0,0 @@ -// DSFE_Core RobotModel.h -#pragma once -#include "EngineCore.h" - -#include -#include -#include -#include -#include "Platform/Logger.h" -#include "EngineLib/LogMacros.h" - -namespace robots { - // --- Robot Model Kinematic Models --- - enum class eKinematicsModel { - URDF, - DH - }; - // --- URDF Joint Types --- - enum class eJointType { - FIXED = 0, - REVOLUTE = 1, - PRISMATIC = 2, - FREE = 3 - }; - // --- 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 --- - - // Inertia tensor struct, representing the inertia of a link about its center of mass, expressed in the link's local frame - struct Inertia { double ixx = 0, ixy = 0, ixz = 0, iyy = 0, iyz = 0, izz = 0; }; - - // Inertial properties of a link - struct Inertial { - double mass = 0.0; - mathlib::Vec3 com_xyz{ 0.0,0.0,0.0 }; - Inertia inertia{}; - }; - - // Collision shape struct, supporting basic shapes (box, cylinder) and mesh (not implemented yet) - struct CollisionShape { - std::string type; - - // Collision Geometry - mathlib::Vec3 origin_xyz{ 0.0, 0.0, 0.0 }; - mathlib::Vec3 origin_rpy{ 0.0, 0.0, 0.0 }; - - // Collision Geometry Parameters - mathlib::Vec3 size{ 0.0, 0.0, 0.0 }; // cylinder -> size = [radius, length, 0], box -> size = [x, y, z] - std::string meshFile; // for mesh collision shapes, not implemented yet - mathlib::Vec4 material{ 1.0, 0.0, 0.2, 1.0 }; - float metallic = 0.5f; - float roughness = 0.5f; - }; - - // Per-mesh entry with individual material properties - struct VisualMeshEntry { - std::string meshFile; - mathlib::Vec4 material{ 1.0, 0.0, 0.2, 1.0 }; - float metallic = 0.5f; - float roughness = 0.5f; - bool hasMaterial = false; // true if material was explicitly specified - }; - - // Visual struct, representing the visual geometry of a link - struct Visual { - // Visual Geometry - mathlib::Vec3 origin_xyz{ 0.0, 0.0, 0.0 }; - mathlib::Vec3 origin_rpy{ 0.0, 0.0, 0.0 }; - - // Visual Geometry Parameters - std::vector meshFiles; // for multiple visual meshes per link (legacy, string-only) - std::vector meshEntries; // for multiple visual meshes with per-mesh material - }; - - // RobotLink struct, representing a single link in the robot model - struct RobotLink { - std::string name; - - // Geometries (for rendering) - Visual visual{}; - std::vector collisions; - Inertial inertial{}; - }; - - // --- Robot Model Joints --- - - // Joint limits struct, representing the physical limits of a joint - struct JointLimit { - bool continuous = false; - double minAngle = 0.0; - double maxAngle = 0.0; - double maxqd = PI_d; - double maxEffort = 0.0; // max torque/force - // Soft limits - double omegaRefMaxRad_s = 0.0; - }; - - // Joint dynamics parameters, representing the damping and friction properties of a joint - struct JointDynamics { - double damping = 0.0; - double friction = 0.0; - }; - - // RobotJoint struct, representing a single joint in the robot model - struct RobotJoint { - // Joint name and parent-child link names - std::string name = ""; - std::string parent = ""; - std::string child = ""; - - // URDF joint type - eJointType type = eJointType::REVOLUTE; - - // Parent joint axis and pivot (for visualization of the joint frame) - mathlib::Vec3 axisParent{ 0.0, 0.0, 1.0 }; - mathlib::Vec3 pivotParent{ 0.0, 0.0, 0.0 }; - - // URDF joint frame (parent → joint) - mathlib::Vec3 origin_xyz{ 0.0, 0.0, 0.0 }; // translation from parent link frame to joint frame, expressed in parent link frame - mathlib::Vec3 origin_rpy{ 0.0, 0.0, 0.0 }; // roll, pitch, yaw in radians - mathlib::Quat origin_q{ 1,0,0,0 }; // Rotation matrix from link frame to base frame, derived from rpy_deg in JSON - - // Axis expressed IN JOINT FRAME - mathlib::Vec3 axis{ 0.0, 0.0, 1.0 }; - - // --- Limits --- - JointLimit limits; - JointDynamics dynamics; - - // --- State --- - double q = 0.0; // rad - double qd = 0.0; // rad/s - double torque = 0.0; // Nm or N - double eta = 0.0f; // Integral state - - // --- Control --- - double q_ref = 0.0; // rad - double qd_ref = 0.0; // rad/s - double qdd_ref = 0.0; // rad/s^2 - - // --- Control Parameters --- - double wn_target = 0.0; // rad/s - double zeta_target = 0.0; // damping ratio - - // --- Precomputed transforms --- - mathlib::Mat4 jointToChildRest = mathlib::Mat4::Identity(); - mathlib::Mat4 parentToJoint = mathlib::Mat4::Identity(); - }; - - // --- Robot Model --- - - // RobotModel struct, representing the entire robot model - struct RobotModel { - std::string name = "UnnamedRobot"; - float scale = 1.0f; - - // Links and joints - 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; - - // Visualization options - eVisualFrame visualFrame = eVisualFrame::JOINT; - std::unordered_map materials; - mathlib::Mat4 baseFrame = mathlib::Mat4::Identity(); // transform from world frame to robot base frame, can be set in JSON - - bool baseFrameIsEngineAligned = false; - - // Create an Eigen vector of joint angles - VecX makeJointVector() const { - 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].q; } - return q; - } - - // Set joint angles from an Eigen vector - void setJointVector(const VecX& q) { - const int n = static_cast(joints.size()); - if (q.size() != n) { - LOG_ERROR("Joint vector size mismatch: expected %d, got %d", n, q.size()); - D_ERROR("Joint vector size mismatch: expected %d, got %d", n, q.size()); - return; - } - for (int i = 0; i < n; ++i) { - double a = q(i); - joints[i].q = a; - } - } - }; -} // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h b/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h deleted file mode 100644 index bb176b7f..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h +++ /dev/null @@ -1,81 +0,0 @@ -// DSFE_Core RobotSimSnapshot.h -#pragma once - -#include "EngineCore.h" -#include "Robots/RobotModel.h" -#include - -namespace robots { - // Immutable robot data needed by solver threads - struct DSFE_API RobotConstModel { - std::string name; - bool baseFrameIsAligned = false; - double scale = 1.0; - mathlib::Mat4 baseFrame = mathlib::Mat4::Identity(); - - bool jointAffectsLink(size_t jIdx, size_t lIdx) const; - - std::vector links; - std::vector joints; - - std::unordered_map linkNameToIndex; - int linkIndex(const std::string& linkName) const; - }; - - // Runtime snapshot for one integration/derivative step - template - struct RobotSimSnapshot_T { - - const RobotConstModel* model = nullptr; - - mathlib::VecX_T q; // joint angles - mathlib::VecX_T qd; // joint velocities - - mathlib::VecX_T q_ref; // reference joint angles - mathlib::VecX_T qd_ref; // reference joint velocities - mathlib::VecX_T qdd_ref; // reference joint accelerations - - mathlib::Mat4_T robotRootPose = mathlib::Mat4_T::Identity(); - - bool baseIsFree = false; - - Scalar lastBaseForwardForce = Scalar(0); - Scalar gravity = Scalar(0); - - eTorqueMode torqueMode = eTorqueMode::CONTROLLED; - - Scalar dt = Scalar(0); - Scalar simTime = Scalar(0); - }; - using RobotSimSnapshot = RobotSimSnapshot_T; - - template - inline RobotSimSnapshot_T castSnapshot( - const RobotSimSnapshot_T& src - ) { - RobotSimSnapshot_T dst; - - dst.model = src.model; - - dst.q = src.q.template cast(); - dst.qd = src.qd.template cast(); - - dst.q_ref = src.q_ref.template cast(); - dst.qd_ref = src.qd_ref.template cast(); - dst.qdd_ref = src.qdd_ref.template cast(); - - dst.robotRootPose = src.robotRootPose.template cast(); - - dst.baseIsFree = src.baseIsFree; - - dst.lastBaseForwardForce = ToScalar(src.lastBaseForwardForce); - dst.gravity = ToScalar(src.gravity); - - dst.torqueMode = src.torqueMode; - - dst.dt = ToScalar(src.dt); - dst.simTime = ToScalar(src.simTime); - - return dst; - } -} // namespace robots \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h deleted file mode 100644 index 8bb9ba53..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ /dev/null @@ -1,325 +0,0 @@ -// DSFE_Core RobotSystem.h -#pragma once - -#include "EngineCore.h" -#include "Robots/RobotModel.h" - -#include "Robots/SpatialModel.h" -#include "Robots/RobotSimSnapshot.h" -#include "Robots/DynamicsTypes.h" - -#include -#include "Robots/RobotKinematics.h" -#include "Robots/RobotDynamics.h" -#include "Robots/SpatialDynamics.h" - -#include "Analysis/MetricLogger.h" -#include "Numerics/IntegrationService.h" - -// Forward declarations -namespace control { class TrajectoryManager; } - -namespace robots { - // Forward declarations - enum class eTorqueMode; - - // Joint state structure - struct DSFE_API JointState { - double theta; - double omega; - }; - - // Metrics structure - enum class eRole { - Simulation, - Baseline - }; - - // Step Result struct - template - struct RobotStepResult_T { - integration::StepOut_T stepOut; - RobotSimSnapshot_T snap; - DynamicsResult dynamics; - mathlib::VecX_T tau_rnea; - }; - - inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) - - class DSFE_API RobotSystem { - public: - RobotSystem(); - ~RobotSystem(); - - // --- Utility Methods --- - - static double clampJointAngle(const RobotJoint& joint, double angleRad); - template - static T clampJointAngle_T(const RobotJoint& joint, T angleRad); - - // ---- Accessors --- - - const robots::RobotModel& model() const; - const std::vector& worldTransforms() const { return _worldTransforms; } - - const std::vector& links() const { return _robot.links; } - std::vector& links() { return _robot.links; } - const std::vector& joints() const { return _robot.joints; } - std::vector& joints() { return _robot.joints; } - - std::size_t linkCount() const { return _robot.links.size(); } - std::size_t jointCount() const { return _robot.joints.size(); } - - std::string findRootLink() const; - - bool hasLinkName(const std::string& linkName) const { return _linkIndex.find(linkName) != _linkIndex.end(); } - const std::string& robotName() const { return _robot.name; } - bool hasRobot() const { return _hasRobot; } - - void setGravity(double g); - const double getGravity() const { return _gravity; } - - void setNaturalFrequency(double wn) { _wn = wn; } - double getNaturalFrequency() const { return _wn; } - void resetNaturalFrequencyToTarget() { for (auto& joint : _robot.joints) { joint.wn_target = _wn; } } - - void setDampingRatio(double zeta) { _zeta = zeta; } - double getDampingRatio() const { return _zeta; } - void resetDampingRatioToTarget() { - for (auto& joint : _robot.joints) { joint.zeta_target = _zeta; } - } - - // Get pointer to this RobotSystem - const RobotSystem& getRobot() const { return *this; } - - // ---- Joint State Methods --- - - void computeRobotKinematics(std::vector& world); - - bool tryGetJointAngleRad(const std::string& childLink, double& outAngle) const; - bool trySetJointAngleRad(const std::string& childLink, double angleRad); - - bool tryGetJointOmegaRad(const std::string& childLink, double& outOmega) const; - bool trySetJointOmegaRad(const std::string& childLink, double omegaRad); - bool injectJointOmegaRad(const std::string& childLink, double omega); - - bool tryGetJointTargetRad(const std::string& childLink, double& outTargetRad) const; - bool trySetJointTargetRad(const std::string& childLink, double targetRad); - - bool tryGetJointOmegaMaxRad(const std::string& childLink, double& maxOmegaRad) const; - bool trySetJointOmegaMaxRad(const std::string& childLink, double maxOmegaRad); - - bool tryAddJointTargetRad(const std::string& childLink, double deltaRad); - - bool isJointAtTargetRad(const std::string& childLink, double tolRad) const; - bool isJointAtTargetDeg(const std::string& childLink, double tolDeg) const; - - bool isJointNearAngleRad(const std::string& childLink, double targetRad, double tolRad) const; - bool isJointNearAngleDeg(const std::string& childLink, double targetDeg, double tolDeg) const; - - bool trySetJointOmegaRefRad(const std::string& childLink, double omegaRefRad); - bool trySetJointAlphaRefRad(const std::string& childLink, double alphaRefRad); - - bool trySetJointOmegaRefMaxRad(const std::string& childLink, double omegaRefMaxRad); - - bool tryZeroJointRefDerivatives(); - - // --- SIMULATION STEP METHOD --- - - template - RobotSimSnapshot_T takeSnapshot(T simTime) const; - template - void step_AD(double dt, double simTime); - - void step(double dt, double simTime); - void updateTrajectoryInputs(control::TrajectoryManager& traj, double t); - - // --- ROBOT LOADING AND RESET METHODS --- - - void loadRobot(const std::string& name); - void resetRobot(); - void stopAll(); - - // --- ROBOT LINK AND ROOT POSE METHODS --- - - bool setRobotLinkRotation(const std::string& childLinkName, double angleDeg); - mathlib::Mat4 setRobotRoot(const mathlib::Vec3& pos, const mathlib::Quat& rot); - void setRobotRootPose(const mathlib::Vec3& pos, const mathlib::Quat& rot); - void setRobotRootHome(const mathlib::Vec3& pos, const mathlib::Quat& rot); - - bool setDefaultPoseDeg(); - void setCurrentJointIndex(int index) { _currentJointIndex = index; } - - // --- GET AND SET INTEGRATION METHOD --- - - integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } - std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } - void setStandardIntegrator(integration::eIntegrationMethod m); - - integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } - std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } - void setADIntegrator(integration::eAutoDiffIntegrationMethod m); - - integration::IntegrationService* getIntegrator(); - const integration::IntegrationService* getIntegrator() const; - - integration::DifferentiableIntegrator* getADIntegrator(); - const integration::DifferentiableIntegrator* getADIntegrator() const; - - bool autoDiffEnabled() const { return _useAutoDiff; } - void enableAutoDiff(bool enable) { _useAutoDiff = enable; } - - std::shared_ptr runtimeIntegratorState(); - std::shared_ptr runtimeIntegratorState() const; - - void setRefBuffer(robots::TrajRefBuffer* buf) { _refBuffer = buf; } - void setLogBuffer(robots::JointLogBuffer* buf) { _logBuffer = buf; } - void setRole(eRole role) { _role = role; } - - // Setter and getter the torque mode for the robot system - void setTorqueMode(eTorqueMode mode); - eTorqueMode getTorqueMode() const { return _robot.torqueMode; } - - // Swap for the current log buffer, returning a ptr to new active buffer - std::unique_ptr 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 buildLinkIndex(); - void buildSpatialModel(); - - template - RobotStepResult_T step_impl( - const mathlib::VecX_T& x, - Scalar dt, Scalar t, IntegratorT& integrator, - DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult - ); - - template - void postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& scratch, const RobotStepResult_T& result); - - std::unique_ptr _kinematics; - std::unique_ptr _dynamics; - - std::unique_ptr _integrator; - integration::eIntegrationMethod _curIntMethod{}; - - std::unique_ptr _AD_integrator; - integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; - - eRole _role = eRole::Simulation; - - double _wn = 0.0; // configurable natural frequency for PD control (rad/s) - double _zeta = 0.0; // configurable damping ratio for PD control (unitless) - - bool _useAutoDiff = false; - - // Compute the forward drive (velocity) of the robot's root link based on the current state and robot configuration - double computeForwardDrive() const; - // Integrate the floating base translation based on the current state and robot configuration - void integrateBaseTranslation(double dt); - // Integrate the floating base rotation (yaw-only for now) based on the current state and robot configuration - void updateBaseRootPose(); - - // State packing and unpacking - mathlib::VecX packState() const; - void unpackState(const mathlib::VecX& x); - - template - void unpackState(const mathlib::VecX_T& x); - - // State packing and unpacking using a DualNumber vector. - mathlib::VecX_T> packState_AD() const; - void unpackState_AD(const mathlib::VecX_T>& x); - - // Reference state packing and unpacking - mathlib::VecX packRefState() const; - void unpackRefState(const mathlib::VecX& xr); - - // Enforce joint limits after integration - void enforceJointLimits(RobotJoint& j); - - // Simulation time - double _simTime = 0.0; - - // Robot model, and robot mode - RobotModel _robot; - eTorqueMode _torqueMode = _robot.torqueMode; - - SpatialModel _spatialModel; - RobotConstModel _constModel; - - DynamicsScratch _dynScratch; - DynamicsResult _dynResult; - - DynamicsScratch> _dynScratch_AD; - DynamicsResult> _dynResult_AD; - - // World to robot base transform (meters) - std::vector _worldTransforms; - - // Flags and precomputed data - bool _hasRobot = false; - mathlib::Mat4 _robotRootPose = Mat4::Identity(); - mathlib::Mat4 _robotRootHome = Mat4::Identity(); - mathlib::VecX _robotQHome = mathlib::VecX(); // home/reset joint angles (radians) - bool _robotHomeValid = false; // is home position valid - - // Index maps for quick lookup of links and joints by name - std::unordered_map _linkIndex; - std::unordered_map _jointIndex; - // List of joint indices that correspond to the robot's degrees of freedom (excluding fixed joints) - std::vector _dofJointIndices; - - std::string _loadedName; - int _currentJointIndex = -1; - - // Reference state - mathlib::VecX _xRef; - bool _refInit = false; - bool _isReference = false; - - // precomputed clamp lookup tables - mutable std::vector _clampTheta; - mutable std::vector _clampOmega; - - // Gravity acceleration (m/s^2) - double _gravity = 0.0; - - // FLoating base state - bool _baseIsFree = false; - - // Linear - mathlib::Vec3 _basePos{ 0,0,0 }; - mathlib::Vec3 _baseVel{ 0,0,0 }; - mathlib::Vec3 _baseAcc{ 0,0,0 }; - - // Angular (yaw-only for now, extend later) - double _baseYaw = 0.0; - double _baseYawRate = 0.0; - double _baseYawAcc = 0.0; - - // Tunables - double _baseMass = 62.0; // kg (H1 ~60–65) - double _baseLinearDamping = 6.0; // Ns/m - double _baseYawDamping = 2.0; // Nms/rad - double _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 -#include "RobotSystemStep.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl deleted file mode 100644 index 55f85b73..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ /dev/null @@ -1,252 +0,0 @@ -// DSFE_Core RobotSystemStep.inl -#pragma once - -namespace robots { - template - T RobotSystem::clampJointAngle_T(const RobotJoint& joint, T angleRad) { - if (joint.limits.continuous) { return mathlib::wrapRad(angleRad); } - else { return std::clamp(angleRad, T(joint.limits.minAngle), T(joint.limits.maxAngle)); } - } - - // Method to take a snapshot of the current robot state - template - RobotSimSnapshot_T RobotSystem::takeSnapshot(T simTime) const { - RobotSimSnapshot_T snap; - snap.model = &_constModel; - const size_t n = (size_t)_robot.joints.size(); - - snap.q.resize(n); - snap.qd.resize(n); - - snap.q_ref.resize(n); - snap.qd_ref.resize(n); - snap.qdd_ref.resize(n); - - for (size_t i = 0; i < n; ++i) { - const auto& j = _robot.joints[i]; - - snap.q[i] = j.q; - snap.qd[i] = j.qd; - - snap.q_ref[i] = j.q_ref; - snap.qd_ref[i] = j.qd_ref; - snap.qdd_ref[i] = j.qdd_ref; - } - - snap.robotRootPose = _robotRootPose.template cast(); - snap.baseIsFree = _baseIsFree; - snap.lastBaseForwardForce = T(_lastBaseForwardForce); - snap.gravity = T(_gravity); - - snap.torqueMode = _robot.torqueMode; - - snap.dt = T(_dynamics->dt()); - snap.simTime = simTime; - - return snap; - } - - template - RobotStepResult_T RobotSystem::step_impl( - const mathlib::VecX_T& x, - Scalar dt, Scalar t, IntegratorT& integrator, - DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult - ) { - RobotStepResult_T result; - result.snap = takeSnapshot(t); - auto& snap = result.snap; - const size_t n = snap.model->joints.size(); - - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); - - mathlib::VecX_T qdd(n); - for (size_t i = 0; i < n; ++i) { qdd[i] = _robot.joints[i].qdd_ref; } - - std::vector> T_start(snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); - std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); - - SpatialModel spatialModel = _spatialModel.template cast(); - auto& dynScratch = dynamicScratch; - auto& dynResult = dynamicResult; - - SpatialDynamics::computeSpatialKinematicsAndBias( - spatialModel, - q, qd, - dynScratch.spatial.Xup, - dynScratch.spatial.v, dynScratch.spatial.c - ); - - // CRBA only for controller inertia scaling - mathlib::MatX_T M_start = SpatialDynamics::CRBA( - spatialModel, - dynScratch.spatial.Xup, - dynScratch - ); - - // Cache frozen joint gains for this step - mathlib::VecX_T kp_frozen(n), kd_frozen(n); - for (size_t i = 0; i < n; ++i) { - const auto& joint = snap.model->joints[i]; - //LOG_INFO("Snap model joint name = %s", joint.name.c_str()); - if (joint.type == eJointType::FIXED) { continue; } - - dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); - const Scalar I_eff = dynScratch.dense.I_eff_controller[i]; - - kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; - kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; - } - - // Compute RNEA torques for feedforward control - mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( - spatialModel, - q, qd, qdd, - dynScratch - ); // [Nm] - result.tau_rnea = tau_rnea; - //LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); - - // Define the derivative function for integration, capturing necessary variables by reference - auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { - return _dynamics->derivative_spatial( - spatialModel, - t, xIn, - snap, - dynScratch, dynResult - ); - }; - // Define the Jacobian function for integration, capturing necessary variables by reference - auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { - _dynamics->jacobian_spatial( - spatialModel, - xIn, snap, - kp_frozen, kd_frozen, - J_out, dynScratch - ); - }; - - if (!x.allFinite()) { LOG_ERROR("[step_impl] input state already non-finite"); } - - if constexpr (std::is_same_v, integration::IntegrationService>) { - mathlib::VecX x_real = x.template cast(); - auto step = integrator.step(_curIntMethod, x_real, static_cast(t), static_cast(dt), f_deriv, f_J); - result.stepOut.x_next = step.x_next.template cast(); - result.stepOut.dt_taken = step.dt_taken; - result.stepOut.dt_sug = step.dt_sug; - } - else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { - result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); - } - - for (int i = 0; i < result.stepOut.x_next.size(); ++i) { - const auto v = mathlib::real(result.stepOut.x_next[i]); - if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } // TODO add Scalar isnan and isinf checks to mathlib and use those instead (need to handle both float and double cases) - } - - result.dynamics = dynResult; - return result; - } - - template - void RobotSystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const RobotStepResult_T& result) { - const size_t n = result.snap.model->joints.size(); - - Eigen::Map q_next(x.data(), n); - Eigen::Map qd_next(x.data() + n, n); - - // Enforce joint limits - /*for (auto& j : _robot.joints) { enforceJointLimits(j); }*/ - - // Recompute kinematics and dynamics at the new state for logging and control purposes - std::vector T_world(result.snap.model->links.size()); - _kinematics->computeForwardKinematics_fromState(*result.snap.model, x, T_world); - - // Alternative would be just - - // Compute mass matrix at the new state - mathlib::MatX M = dynScratch.dense.M.unaryExpr([](const auto& v) { return mathlib::real(v); }); - - // Extract real parts of relevant variables for logging and control - mathlib::VecX q_real = result.snap.q.unaryExpr([](const auto& v) { return mathlib::real(v); }); - mathlib::VecX qd_real = result.snap.qd.unaryExpr([](const auto& v) { return mathlib::real(v); }); - mathlib::VecX q_ref_real = result.snap.q_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); - mathlib::VecX qd_ref_real = result.snap.qd_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); - mathlib::VecX tau_rnea_real = result.tau_rnea.unaryExpr([](const auto& v) { return mathlib::real(v); }); - - // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd - double sys_KE = 0.5 * qd_real.transpose() * M * qd_real; // [J], kinetic energy of the robot at configuration q and velocity qd - - // Compute system potential energy at configuration q (relative to gravity) - double sys_PE = 0.0; - double g = _dynamics->getGravity(); - - for (size_t k = 0; k < _robot.links.size(); ++k) { - const RobotLink& link = _robot.links[k]; - const double m = link.inertial.mass; - if (m <= 0.0) { continue; } - Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); - sys_PE += m * g * com_world.z(); - } - - const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system - - // Log metrics to buffer if logging is enabled - robots::JointLogBuffer* buf = nullptr; - if (_useInternalLogging) { int idx = _activeLogBufIdx.load(std::memory_order_acquire); buf = &_logBuffers[idx]; } - else { buf = _logBuffer; } - - if (buf) { - auto dynResult = result.dynamics; - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = _robot.joints[i]; - - const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); - const double err = q_ref_real[i] - q_real[i]; - const double err_d = qd_ref_real[i] - qd_real[i]; - JointLogBuffer::JointLogEntry e{}; - - e.sim_time = _simTime; - e.dt_taken = mathlib::real(result.stepOut.dt_taken); - e.dt_sug = mathlib::real(result.stepOut.dt_sug); - e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = mathlib::real(dynResult.metrics.qdd[i]); - e.err = err; e.err_d = err_d; - e.I_eff = I_eff; - e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = 0.0; - e.tau_sat = mathlib::real(dynResult.metrics.tau_sat[i]); - e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; - e.clamp_theta = mathlib::real(_clampTheta[i]); e.clamp_omega = mathlib::real(_clampOmega[i]); - e.sat_flag = mathlib::real(dynResult.metrics.sat_flag[i]); e.joint_index = (int)i; - buf->push_entry(e); - } - } - } - - template - void RobotSystem::step_AD(double dt, double simTime) { - if (!hasRobot()) { return; } - using Dual = mathlib::DualNumber_T; - _simTime = simTime; - const size_t n = _robot.joints.size(); - mathlib::VecX_T x = packState_AD(); - - assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit - for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } - - auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator, _dynScratch_AD, _dynResult_AD); - unpackState_AD(result.stepOut.x_next); - _dynamics->setDt(result.stepOut.dt_taken); - - auto x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); - postStepUpdate(x_real, _dynScratch_AD, result); - - // Update base pose if free-floating - if (_baseIsFree) { - integrateBaseTranslation(dt); - updateBaseRootPose(); - } - // Update kinematics - computeRobotKinematics(_worldTransforms); - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h deleted file mode 100644 index 9963d617..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ /dev/null @@ -1,95 +0,0 @@ -// DSFE_Core SpatialDynamics.h -#pragma once - -#include "EngineCore.h" -#include "Robots/SpatialModel.h" -#include "Robots/DynamicsTypes.h" - -namespace robots { - class DSFE_API SpatialDynamics { - public: - template - static void computeSpatialKinematicsAndBias( - const SpatialModel& model, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - std::vector>& Xup_out, - std::vector>& v_out, - std::vector>& c_out - ); - - template - static void computeAccelerations_RNEA( - const SpatialModel& model, - const mathlib::VecX_T& qdd, - const std::vector>& Xup, - const std::vector>& c, - const mathlib::VecX_T& g, - std::vector>& a_out - ); - - template - static void computeBackwardForces_RNEA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& v, - const std::vector>& a, - mathlib::VecX_T& tau_out - ); - - template - static mathlib::VecX_T RNEA( - const SpatialModel& model, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - const mathlib::VecX_T& qdd, - DynamicsScratch& scratch - ); - - template - static mathlib::MatX_T CRBA( - const SpatialModel& model, - const std::vector>& Xup, - DynamicsScratch& scratch - ); - - template - static void computeArticulatedBodies_ABA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& v, - const std::vector>& c, - const mathlib::VecX_T& tau, - std::vector>& IA_out, - std::vector>& pA_out, - std::vector>& Ia_out, - mathlib::VecX_T& u_out, - mathlib::VecX_T& d_out, - std::vector>& U_out - ); - - template - static void computeAccelerations_ABA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& c, - const mathlib::VecX_T& u_out, - const mathlib::VecX_T& d_out, - const std::vector>& U, - const SpatialVec_T& a0, - std::vector>& a_out, - mathlib::VecX_T& qdd_out - ); - - template - static mathlib::VecX_T ABA( - const SpatialModel& model, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - const mathlib::VecX_T& tau, - DynamicsScratch& scratch - ); - }; -} - -#include "Robots/SpatialDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl deleted file mode 100644 index 5382736c..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ /dev/null @@ -1,319 +0,0 @@ -// DSFE_Core SpatialDynamics.inl -#pragma once - -namespace robots { - template - void SpatialDynamics::computeSpatialKinematicsAndBias( - const SpatialModel& model, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - std::vector>& Xup_out, - std::vector>& v_out, - std::vector>& c_out - ) { - const size_t n = model.joints.size(); - v_out.resize(n); - Xup_out.resize(n); - c_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - // Joint Transform XJ - mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); - - if (j.type == eJointType::REVOLUTE) { - mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.angular()); - mathlib::Mat3_T R = mathlib::AngleAxis(q[i], axis); - mathlib::Vec3_T r = mathlib::Vec3_T::Zero(); - XJ = mathlib::spatialTransform(R, r); - } - else if (j.type == eJointType::PRISMATIC) { - mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.linear()); - mathlib::Vec3_T r = q[i] * axis; - mathlib::Mat3_T R = mathlib::Mat3_T::Identity(); - XJ = mathlib::spatialTransform(R, r); - } - - Xup_out[i] = XJ * j.Xtree; // Combined Transform - mathlib::SpatialVec_T vJ = j.S * qd[i]; // Joint Velocity - - // Root Link - if (j.parent < 0) { v_out[i] = vJ; } - else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } - - c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term - - using corScalar = typename std::decay_t; - static_assert(std::is_same_v, "c_out scalar type does not match model scalar type"); - } - } - - template - void SpatialDynamics::computeAccelerations_RNEA( - const SpatialModel& model, - const mathlib::VecX_T& qdd, - const std::vector>& Xup, - const std::vector>& c, - const mathlib::VecX_T& g, - std::vector>& a_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - - mathlib::SpatialVec_T a0; // base acceleration (gravity) - a0.v << - g.template segment<3>(0), - g.template segment<3>(3); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec_T aJ = j.S * qdd[i]; // Joint Acceleration - - // Root Link - if (j.parent < 0) { - a_out[i] = Xup[i] * a0 + aJ + c[i]; - continue; - } - - a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; - } - } - - template - void SpatialDynamics::computeBackwardForces_RNEA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& v, - const std::vector>& a, - mathlib::VecX_T& tau_out - ) { - const size_t n = model.joints.size(); - tau_out.resize(n); - - std::vector> f(n); - - // Forward Force Computation - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - mathlib::SpatialVec_T I_v = j.inertia * v[i]; - mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); - f[i].v = j.inertia * a[i].v + coriolis.v; - } - - // Backward Recursion Computation - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - tau_out[i] = j.S.dot(f[i]); - mathlib::SpatialMat_T XupT = Xup[i].transpose(); - if (j.parent >= 0) { f[j.parent] += XupT * f[i]; } - } - } - - template - mathlib::VecX_T SpatialDynamics::RNEA( - const SpatialModel& model, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - const mathlib::VecX_T& qdd, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - - // TODO Remove these temp scratches AFTER debugging - std::vector> v(n); - std::vector> Xup(n); - std::vector> c(n); - std::vector> a(n); - mathlib::VecX_T tau; - - // Compute spatial velocities and transforms - computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); - // Compute spatial accelerations - computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); - // Compute inverse dynamics (joint torques) - computeBackwardForces_RNEA(model, Xup, v, a, tau); - - return tau; - } - - template - mathlib::MatX_T SpatialDynamics::CRBA( - const SpatialModel& model, - const std::vector>& Xup, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - scratch.dense.M.setZero(n, n); - std::vector> Ic(n); // spatial inertia for each link - - // Initialise spatial inertia for each link based on the robot model - for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } - - // Upward pass: propagate spatial inertia from child links to parent joints - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - int p = j.parent; - if (p >= 0) { - mathlib::MatX_T XupT = Xup[i].transpose(); - Ic[p] += XupT * Ic[i] * Xup[i]; - } - } - - // Downward pass: compute mass matrix contributions for each joint - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } - mathlib::SpatialVec_T F = Ic[i] * j.S; - scratch.dense.M(i, i) = j.S.dot(F); - - int jIdx = (int)i; - while (model.joints[jIdx].parent >= 0) { - int p = model.joints[jIdx].parent; - mathlib::SpatialMat_T XupT = Xup[jIdx].transpose(); // TODO Make Eigen-copatible operator overloads for spatial transforms to avoid the errors from this transpose operation in a matrix multiplication context - F = XupT * F; - scratch.dense.M(i, p) = model.joints[p].S.dot(F); - scratch.dense.M(p, i) = scratch.dense.M(i, p); - jIdx = p; - } - } - return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) - } - - template - void SpatialDynamics::computeArticulatedBodies_ABA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& v, - const std::vector>& c, - const mathlib::VecX_T& tau, - std::vector>& IA_out, - std::vector>& pA_out, - std::vector>& Ia_out, - mathlib::VecX_T& u_out, - mathlib::VecX_T& d_out, - std::vector>& U_out - ) { - const size_t n = model.joints.size(); - - // Resize scratch buffers - IA_out.resize(n); - pA_out.resize(n); - Ia_out.resize(n); - U_out.resize(n); - u_out.resize(n); - d_out.resize(n); - - // Upward pass: compute articulated body inertias and bias forces - for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - - if (j.type == eJointType::FIXED) { - Ia_out[i] = IA_out[i]; - if (j.parent >= 0) { - mathlib::SpatialMat_T XupT = Xup[i].transpose(); - IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; - pA_out[j.parent] += XupT * pA_out[i]; - } - continue; - } - - U_out[i] = IA_out[i] * j.S; - d_out[i] = dot(j.S, U_out[i]); - if (d_out[i] < Scalar(1e-12)) { - d_out[i] = Scalar(1e-12); - } - - u_out[i] = tau[i] - dot(j.S, pA_out[i]); - Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; - - // pA = pA + Ia * c + U * (u/d) - pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); - - if (j.parent >= 0) { - mathlib::SpatialMat_T XupT = Xup[i].transpose(); - IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; - pA_out[j.parent] += XupT * pA_out[i]; - } - } - } - - template - void SpatialDynamics::computeAccelerations_ABA( - const SpatialModel& model, - const std::vector>& Xup, - const std::vector>& c, - const mathlib::VecX_T& u_out, - const mathlib::VecX_T& d_out, - const std::vector>& U, - const SpatialVec_T& a0, - std::vector>& a_out, - mathlib::VecX_T& qdd_out - ) { - const size_t n = model.joints.size(); - a_out.resize(n); - qdd_out.resize(n); - - for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - - if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } - else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } - - if (j.type == eJointType::FIXED) { - qdd_out[i] = Scalar(0); - continue; - } - - qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; - a_out[i] += j.S * qdd_out[i]; - } - } - - template - mathlib::VecX_T SpatialDynamics::ABA( - const SpatialModel& model, - const mathlib::VecX_T& q, - const mathlib::VecX_T& qd, - const mathlib::VecX_T& tau, - DynamicsScratch& scratch - ) { - const size_t n = model.joints.size(); - mathlib::VecX_T qdd = mathlib::VecX_T::Zero(n); - - mathlib::SpatialVec_T a0; // base acceleration (gravity) - a0.v << - scratch.g.template segment<3>(0), - scratch.g.template segment<3>(3); - - computeSpatialKinematicsAndBias( - model, q, qd, - scratch.spatial.Xup, - scratch.spatial.v, - scratch.spatial.c - ); - - for (size_t i = 0; i < n; ++i) { - scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia - scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); - } - - // Compute articulated body inertias and bias forces - computeArticulatedBodies_ABA( - model, scratch.spatial.Xup, - scratch.spatial.v, scratch.spatial.c, tau, - scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U - ); - - // Compute joint accelerations using the articulated body algorithm - computeAccelerations_ABA( - model, scratch.spatial.Xup, scratch.spatial.c, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, - a0, scratch.spatial.a, qdd - ); - - return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h b/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h deleted file mode 100644 index 3775f8d4..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h +++ /dev/null @@ -1,33 +0,0 @@ -// DSFE_Core SpatialModel.h -#pragma once - -#include -#include -#include "Robots/RobotModel.h" - -namespace robots { - // Spatial joint struct - template - struct SpatialJoint { - int parent = -1; - - eJointType type = eJointType::FIXED; - - mathlib::SpatialMat_T Xtree; - mathlib::SpatialMat_T inertia; - mathlib::SpatialVec_T S; - - std::string name; - }; - - // Spatial model struct - template - struct SpatialModel { - std::vector> joints; - std::unordered_map linkNameToIndex; - - template - SpatialModel cast() const; - }; -} // namespace robots -#include "SpatialModelCast.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl b/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl deleted file mode 100644 index 8ceb6c06..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl +++ /dev/null @@ -1,22 +0,0 @@ -// DSFE_Core SpatialModelCast.inl -#pragma once - -namespace robots { - template - template - SpatialModel SpatialModel::cast() const { - SpatialModel out; - out.joints.resize(joints.size()); - for (size_t i = 0; i < joints.size(); ++i) { - const auto& j = joints[i]; - auto& out_j = out.joints[i]; - out_j.parent = j.parent; - out_j.type = j.type; - out_j.Xtree = j.Xtree.template cast(); - out_j.inertia = j.inertia.template cast(); - out_j.S = j.S.template cast(); - out_j.name = j.name; - } - return out; - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h b/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h deleted file mode 100644 index 03c440fd..00000000 --- a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h +++ /dev/null @@ -1,36 +0,0 @@ -// DSFE_Core TrajectoryManager.h -#pragma once -#include "EngineCore.h" -#include -#include - -namespace robots { class DSFE_API RobotSystem; } - -namespace control { - class DSFE_API TrajectoryManager { - public: - TrajectoryManager() = default; - ~TrajectoryManager() = default; - // non-copyable - TrajectoryManager(const TrajectoryManager&) = delete; - TrajectoryManager& operator=(const TrajectoryManager&) = delete; - // movable is fine - TrajectoryManager(TrajectoryManager&&) noexcept = default; - TrajectoryManager& operator=(TrajectoryManager&&) noexcept = default; - - void clear(const std::string& link); - void clearAll(); - - bool empty() const { return _active.empty(); } - bool tryEval(const std::string& link, double t, control::TrajState& out) const; - bool hasActive(const std::string& link) const; - - void set(const std::string& link, std::unique_ptr traj); - void apply(robots::RobotSystem& robot, double t); - - std::size_t activeCount() const { return _active.size(); } - - private: - std::unordered_map> _active; - }; -} // namespace control \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp deleted file mode 100644 index 34204c43..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/RobotDynamics.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "pch.h" -// File: RobotDynamics.cpp -// GitHub: SaltyJoss -#include "Robots/RobotDynamics.h" - -namespace robots { - // Constructor - RobotDynamics::RobotDynamics() - : _kinematics(std::make_unique()) { - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp deleted file mode 100644 index c7b181ac..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/RobotKinematics.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "pch.h" -// File: RobotKinematics.cpp -// GitHub: SaltyJoss -#include "Robots/RobotKinematics.h" - -using namespace mathlib; -using namespace constants; - -namespace robots { - // Constructor - RobotKinematics::RobotKinematics() { - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp deleted file mode 100644 index d6501e38..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp +++ /dev/null @@ -1,500 +0,0 @@ -// DSFE_Core RobotLoader.cpp -#include "pch.h" - -#include "Robots/RobotLoader.h" -#include - -#include "EngineLib/LogMacros.h" -#include - -using json = nlohmann::json; -using kinematics::JointType_DH; -using kinematics::DH_Params; - -using namespace constants; - -namespace robots { - // --- Static Helper Functions --- - - // 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(); - } - - // Parse DH joint type from string - static JointType_DH parseDHType(const std::string& s) { - std::string t = s; - for (char& c : t) { c = static_cast(std::tolower((unsigned char)c)); } - if (t == "revolute" || t == "r") { return JointType_DH::Revolute; } - return JointType_DH::Prismatic; - } - - // Read a vec3 from a JSON array - static Vec3 readVec3(const json& j, const char* key, Vec3 fallback = {}) { - if (!j.contains(key) || !j[key].is_array() || j[key].size() != 3) { return fallback; } - return Vec3(j[key][0].get(), j[key][1].get(), j[key][2].get()); - } - - // --- RobotLoader Link and Joint Parsing --- - - // link parsing - - // Parse materials block if present - // Each material is defined as: "materials": { "mat_name": [r, g, b, a] } - static void loadMaterials(const json& data, RobotModel& robot) { - if (!data.contains("material")) { return; } - const auto& mat = data["material"]; - - // New nested format: "material" -> "Color" -> { name: [r,g,b,a] } - if (mat.contains("Color") && mat["Color"].is_object()) { - for (auto& [name, col] : mat["Color"].items()) { - if (col.is_array() && col.size() == 4) { - robot.materials[name] = Vec4( - col[0].get(), - col[1].get(), - col[2].get(), - col[3].get() - ); - } - else { - LOG_WARN("Material Color '%s' has invalid format, expected array of 4 floats", name.c_str()); - } - } - } - else { - // Legacy flat format: "material" -> { name: [r,g,b,a] } - for (auto& [name, col] : mat.items()) { - if (col.is_array() && col.size() == 4) { - robot.materials[name] = Vec4( - col[0].get(), - col[1].get(), - col[2].get(), - col[3].get() - ); - } - } - } - } - - // Helper to parse material properties from a JSON object into color/metallic/roughness - static void parseMaterialObject(const json& m, const std::unordered_map& materials, const std::string& context, - Vec4& outColor, float& outMetallic, float& outRoughness) { - if (m.contains("Color") && m["Color"].is_string()) { - const std::string colorName = m["Color"].get(); - auto it = materials.find(colorName); - if (it != materials.end()) { - outColor = it->second; - } - else { - LOG_WARN("%s references undefined color '%s', using default Grey", context.c_str(), colorName.c_str()); - outColor = Vec4(0.4, 0.4, 0.4, 1.0); - } - } - if (m.contains("Metallic") && m["Metallic"].is_number()) { - outMetallic = m["Metallic"].get(); - } - if (m.contains("Roughness") && m["Roughness"].is_number()) { - outRoughness = m["Roughness"].get(); - } - } - - // Parse visual geometry, supporting both single mesh and multiple meshes, as well as material properties - static void parseVisual(const json& linkData, const std::unordered_map& materials, RobotLink& link) { - if (!linkData.contains("visual")) { return; } - const auto& v = linkData["visual"]; - - // Visual geometry origin - link.visual.origin_xyz = readVec3(v, "origin_xyz", link.visual.origin_xyz); - link.visual.origin_rpy = readVec3(v, "origin_rpy", link.visual.origin_rpy); - - // Single Mesh - if (v.contains("mesh") && v["mesh"].is_string()) { - VisualMeshEntry entry; - entry.meshFile = v["mesh"].get(); - link.visual.meshEntries.push_back(entry); - } - - // Multiple meshes - if (v.contains("meshes") && v["meshes"].is_array()) { - for (const auto& m : v["meshes"]) { - if (!m.is_string()) { - LOG_WARN("Invalid mesh entry in link %s (expected string)", link.name.c_str()); - continue; - } - - VisualMeshEntry entry; - entry.meshFile = m.get(); - link.visual.meshEntries.push_back(entry); - } - } - - Vec4 material{ 0.7, 0.0, 0.2, 1.0 }; // Default material if not specified - float metallic = 0.5f; - float roughness = 0.5f; - bool hasMaterial = false; - - // Material assignement - if (v.contains("material") && v["material"].is_object()) { - parseMaterialObject(v["material"], materials, "Link " + link.name, - material, metallic, roughness); - hasMaterial = true; - } - else if (v.contains("material") && v["material"].is_string()) { - const std::string matName = v["material"].get(); - auto it = materials.find(matName); - - if (it != materials.end()) { - material = it->second; - hasMaterial = true; - } - else { - LOG_WARN("Link %s references undefined material '%s', using default colour", link.name.c_str(), matName.c_str()); - } - } - - // Assign material properties to all mesh entries - for (auto& entry : link.visual.meshEntries) { - if (hasMaterial) { - entry.material = material; - entry.metallic = metallic; - entry.roughness = roughness; - entry.hasMaterial = hasMaterial; - } - } - } - - // Parse collision geometry material properties - static void parseCollisionMaterial(const json& collisionData, const RobotModel& robot, CollisionShape& shape) { - if (!collisionData.contains("material")) { return; } - const auto& m = collisionData["material"]; - - if (m.is_object()) { - parseMaterialObject(m, robot.materials, "Collision", - shape.material, shape.metallic, shape.roughness); - } - } - - // Parse collision geometry, supporting multiple collision shapes per link - static void parseCollisions(const json& linkData, const RobotModel& robot, RobotLink& link) { - if (!linkData.contains("collision")) { return; } - for (const auto& c : linkData["collision"]) { - if (!linkData["collision"].is_array()) { - LOG_WARN("Collision block is not an array in link %s", link.name.c_str()); - return; - } - - CollisionShape s; - s.type = c.value("type", ""); - s.origin_xyz = readVec3(c, "origin_xyz", s.origin_xyz); - s.origin_rpy = readVec3(c, "origin_rpy", s.origin_rpy); - - if (s.type == "cylinder") { - s.size.x() = c.value("radius", 0.0f); // radius - s.size.y() = c.value("length", 0.0f); // length - } - else if (s.type == "box") { s.size = readVec3(c, "size", s.size); } - else if (s.type == "mesh") { s.meshFile = c.value("mesh", ""); } - - parseCollisionMaterial(c, robot, s); - link.collisions.push_back(s); - } - } - - // Parse inertial properties, including mass, center of mass, and inertia tensor - static void parseInertial(const json& linkData, RobotLink& link) { - if (!linkData.contains("inertial")) { return; } - const auto& I = linkData["inertial"]; - link.inertial.mass = I.value("mass", link.inertial.mass); - link.inertial.com_xyz = readVec3(I, "com_xyz", link.inertial.com_xyz); - - if (I.contains("inertia")) { - const auto& J = I["inertia"]; - link.inertial.inertia.ixx = J.value("ixx", link.inertial.inertia.ixx); - link.inertial.inertia.ixy = J.value("ixy", link.inertial.inertia.ixy); - link.inertial.inertia.ixz = J.value("ixz", link.inertial.inertia.ixz); - link.inertial.inertia.iyy = J.value("iyy", link.inertial.inertia.iyy); - link.inertial.inertia.iyz = J.value("iyz", link.inertial.inertia.iyz); - link.inertial.inertia.izz = J.value("izz", link.inertial.inertia.izz); - } - } - - // joint parsing - - // Parse joint origin, supporting both the "origin" block (with "origin_xyz" and "origin_rpy" inside) and the flat format with "origin_xyz" and "origin_rpy" directly in the joint block - static void parseJointOrigin(const json& jointData, RobotJoint& joint) { - if (jointData.contains("origin")) { - const auto& o = jointData["origin"]; - joint.origin_xyz = readVec3(o, "origin_xyz", joint.origin_xyz); - joint.origin_rpy = readVec3(o, "origin_rpy", joint.origin_rpy); - } else { - joint.origin_xyz = readVec3(jointData, "origin_xyz", Vec3::Zero()); - joint.origin_rpy = readVec3(jointData, "origin_rpy", Vec3::Zero()); - } - joint.origin_q = rpyRadToQuat(joint.origin_rpy); - } - - // Parse joint axis, supporting both the "axis" block (with "axis_xyz" inside) and the flat format with "axis" directly in the joint block - static void parseJointAxis(const json& jointData, RobotJoint& joint) { - joint.axis = Vec3(0.0f, 0.0f, 1.0f); // default axis - if (jointData.contains("axis") && jointData["axis"].is_array() && jointData["axis"].size() == 3) { - const auto& a = jointData["axis"]; - joint.axis = Vec3( - a[0].get(), - a[1].get(), - a[2].get() - ); - if (joint.axis.norm() < 1e-6f) { - LOG_WARN("Joint %s has zero-length axis, defaulting to (0,0,1)", joint.name.c_str()); - joint.axis = Vec3(0.0f, 0.0f, 1.0f); - } - else { joint.axis.normalize(); } - } - - // Parse joint type (e.g., "revolute", "prismatic") if provided. - if (jointData.contains("type") && jointData["type"].is_string()) { - const std::string typeStr = jointData["type"].get(); - if (typeStr == "revolute" || typeStr == "REVOLUTE") { - joint.type = eJointType::REVOLUTE; - } - else if (typeStr == "prismatic" || typeStr == "PRISMATIC") { - joint.type = eJointType::PRISMATIC; - } - else if (typeStr == "fixed" || typeStr == "FIXED") { - joint.type = eJointType::FIXED; - } - else if (typeStr == "free" || typeStr == "FREE") { - joint.type = eJointType::FREE; - } - else { - LOG_WARN("Joint %s has unknown type '%s', defaulting to REVOLUTE", joint.name.c_str(), typeStr.c_str()); - } - } - } - - // Parse joint limits, including continuous revolute joints and prismatic joints - static void parseJointLimits(const json& jointData, RobotJoint& joint) { - joint.limits.continuous = false; - joint.limits.minAngle = 0.0f; - joint.limits.maxAngle = 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; } - - const auto& L = jointData["limits"]; - - joint.limits.continuous = L.value("continuous", false); - joint.limits.maxqd = L.value("velocity", joint.limits.maxqd); - joint.limits.maxEffort = L.value("effort", joint.limits.maxEffort); - - if (!joint.limits.continuous) { - if (L.contains("lower") && L["lower"].is_number()) { joint.limits.minAngle = L["lower"].get(); } - else { LOG_WARN("Joint %s limits missing 'lower'", joint.name.c_str()); } - - if (L.contains("upper") && L["upper"].is_number()) { joint.limits.maxAngle = L["upper"].get(); } - else { LOG_WARN("Joint %s limits missing 'upper'", joint.name.c_str()); } - - if (joint.limits.maxAngle < joint.limits.minAngle) { - LOG_WARN("Joint %s has upper < lower (swapping).", joint.name.c_str()); - std::swap(joint.limits.minAngle, joint.limits.maxAngle); - } - } - else { joint.limits.minAngle = -3.14159265f; joint.limits.maxAngle = 3.14159265f; } - } - - // Parse joint dynamics parameters - static void parseJointDynamics(const json& jointData, RobotJoint& joint) { - joint.dynamics.damping = 0.0; - joint.dynamics.friction = 0.0; - - if (!jointData.contains("dynamics") || !jointData["dynamics"].is_object()) { LOG_ERROR("Could not find joint dynamic data"); return; } - - const auto& D = jointData["dynamics"]; - joint.dynamics.damping = D.value("damping", joint.dynamics.damping); - joint.dynamics.friction = D.value("friction", joint.dynamics.friction); - joint.wn_target = D.value("wn_target", joint.wn_target); - joint.zeta_target = D.value("zeta_target", joint.zeta_target); - - if (joint.dynamics.damping < 0.0) { joint.dynamics.damping = 0.0; } - if (joint.dynamics.friction < 0.0) { joint.dynamics.friction = 0.0; } - } - - // Check if a joint is fixed based on its type string - static bool isFixedJoint(const json& jointData) { - if (!jointData.contains("type")) return false; - const std::string t = jointData["type"].get(); - return (t == "fixed" || t == "FIXED"); - } - - // Parse DH parameters if present - static bool parseDHParameters(const json& jointData, DH_Params& out) { - // Accept "dh" ONLY (your JSON uses "dh") - if (!jointData.contains("dh") || !jointData["dh"].is_object()) return false; - - const auto& dh = jointData["dh"]; - out.a = dh.value("a", 0.0); - out.alpha = dh.value("alpha", 0.0); - out.d = dh.value("d", 0.0); - out.theta = dh.value("theta0", 0.0); - out.type = parseDHType(dh.value("type", "revolute")); - return true; - } - - // Decide kinematics model based on presence of DH parameters - static eKinematicsModel decideKinematicsModel(const json& data) { - if (!data.contains("joints") || !data["joints"].is_array()) { return eKinematicsModel::URDF; } // no joints -> URDF - for (const auto& jointData : data["joints"]) { - if (!jointData.contains("dh") || !jointData["dh"].is_object()) { return eKinematicsModel::URDF; } // any missing -> URDF - } - return eKinematicsModel::DH; - } - - // --- RobotLoader Loading, Public API --- - - RobotModel RobotLoader::loadFromJSON(const std::string& filepath) { - RobotModel robot; - LOG_INFO("Loading robot model from JSON: %s", filepath.c_str()); - D_INFO("Loading robot model from JSON: %s", filepath.c_str()); - - std::ifstream file(filepath); - if (!file.is_open()) { - LOG_ERROR("Failed to open JSON file: %s", filepath.c_str()); - D_FAIL("Failed to open JSON file: %s", filepath.c_str()); - return robot; - } - json data = json::parse(file); - - robot.name = data["name"].get(); - loadMaterials(data, robot); - - // Visual frame (optional, defaults to JOINT) - if (data.contains("visual_frame")) { - const std::string vf = data["visual_frame"].get(); - if (vf == "joint") { robot.visualFrame = eVisualFrame::JOINT; } - else if (vf == "link") { robot.visualFrame = eVisualFrame::LINK; } - else if (vf == "world") { robot.visualFrame = eVisualFrame::WORLD; } - else { LOG_WARN("Unknown visual_frame '%s', defaulting to JOINT", vf.c_str()); } - } - else { - robot.visualFrame = eVisualFrame::JOINT; - } - - // Load robot scale (default 1.0) - robot.scale = data.value("scale", 1.0f); - - // Load base frame if present - if (data.contains("base_frame")) { - robot.baseFrameIsEngineAligned = false; - const auto& bf = data["base_frame"]; - - // Read translation and rotation (RPY) from JSON, with defaults - Vec3 t = readVec3(bf, "origin_xyz", Vec3::Zero()); - Vec3 r = readVec3(bf, "origin_rpy", Vec3::Zero()); - Quat q = rpyRadToQuat(r); - - robot.baseFrame = Mat4::Identity(); - robot.baseFrame.block<3, 3>(0, 0) = q.toRotationMatrix(); - robot.baseFrame.block<3, 1>(0, 3) = t; - - LOG_INFO("Base frame loaded from JSON: translation=(%.3f, %.3f, %.3f), rotation_rpy=(%.3f, %.3f, %.3f)", - t.x(), t.y(), t.z(), - r.x(), r.y(), r.z()); - } - else { - robot.baseFrameIsEngineAligned = true; - robot.baseFrame = Mat4::Identity(); - LOG_INFO("No base frame specified in JSON, using identity (engine-aligned) by default."); - } - - if (robot.name == "Z1") { - robot.baseFrameIsEngineAligned = true; - } - - // Load links - for (auto& linkData : data["links"]) { - RobotLink link; - link.name = linkData.value("name", ""); - - parseVisual(linkData, robot.materials, link); - parseCollisions(linkData, robot, link); - parseInertial(linkData, link); - - robot.links.push_back(link); - - LOG_INFO("Link: %s | Mass: %.2f", link.name.c_str(), link.inertial.mass); - D_INFO("Link: %s | Mass: %.2f", link.name.c_str(), link.inertial.mass); - } - - // Decide kinematics model - robot.kinematicsModel = decideKinematicsModel(data); - - // Load joints - for (auto& jointData : data["joints"]) { - RobotJoint joint; - - // Load basic joint info - joint.name = jointData["name"].get(); - joint.parent = jointData["parent"].get(); - joint.child = jointData["child"].get(); - - parseJointOrigin(jointData, joint); - - // If it's a fixed joint, we can skip axis/limits/dynamics/control parsing and just set defaults. - if (isFixedJoint(jointData)) { - joint.type = eJointType::FIXED; - joint.axis = Vec3::Zero(); - joint.limits.continuous = false; - joint.limits.minAngle = 0.0; - joint.limits.maxAngle = 0.0; - } - else { - parseJointAxis(jointData, joint); - parseJointLimits(jointData, joint); - parseJointDynamics(jointData, joint); - } - - robot.joints.push_back(joint); - - // If robot is DH-mode, also parse DH table - if (robot.kinematicsModel == eKinematicsModel::DH) { - DH_Params dh{}; - if (!parseDHParameters(jointData, dh)) { - LOG_WARN("Joint %s missing 'dh' unexpectedly; forcing URDF mode.", joint.name.c_str()); - robot.kinematicsModel = eKinematicsModel::URDF; - robot.dhParams.clear(); - } - else { - robot.dhParams.push_back(dh); - } - } - - if (abs(joint.limits.minAngle) == abs(joint.limits.maxAngle) && !joint.limits.continuous) { - LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); - D_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); - } - else { - LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: %.2f, | Friction: %.2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); - D_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: % .2f, | Friction : % .2f", - joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction);; - } - } - - if (robot.kinematicsModel == eKinematicsModel::URDF) { robot.dhParams.clear(); } - - LOG_INFO("Robot loaded: %d links, %d joints", (int)robot.links.size(), (int)robot.joints.size()); - D_SUCCESS("Robot loaded: %d links, %d joints", (int)robot.links.size(), (int)robot.joints.size()); - - return robot; - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSimSnapshot.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSimSnapshot.cpp deleted file mode 100644 index 947611d4..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSimSnapshot.cpp +++ /dev/null @@ -1,43 +0,0 @@ -// DSFE_Core RobotSimSnapshot.cpp -#include "pch.h" - -#include "Robots/RobotSimSnapshot.h" - -namespace robots { - // Method to check if a joint affects a link - bool RobotConstModel::jointAffectsLink(size_t jIdx, size_t lIdx) const { - if (jIdx >= joints.size() || lIdx >= links.size()) { return false; } - - const std::string& targetJointChild = joints[jIdx].child; - const std::string& targetLinkName = links[lIdx].name; - - // Check if the joint is an ancestor of the link in the kinematic tree - std::string current = targetLinkName; - - while (true) { - if (current == targetJointChild) { return true; } // joint affects this link - bool movedUp = false; - for (const auto& joint : joints) { - if (joint.child == current) { - current = joint.parent; // move up to the parent link - movedUp = true; - break; - } - } - if (!movedUp) { break; } // reached the root link without finding the joint - } - - return false; // joint does not affect this link - } - - // Method to get the index of a link by name, returns -1 if not found - int RobotConstModel::linkIndex(const std::string& linkName) const { - auto it = linkNameToIndex.find(linkName); - if (it != linkNameToIndex.end()) { - return it->second; - } - else { - return -1; // not found - } - } -} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp b/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp deleted file mode 100644 index 6b3e41fe..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSystem.cpp +++ /dev/null @@ -1,983 +0,0 @@ -// DSFE_Core RobotSystem.cpp -#include "pch.h" - -#include "Robots/RobotSystem.h" -#include "Robots/RobotLoader.h" - -#include -#include -#include - -#include -#include "Robots/TrajectoryManager.h" -#include "Platform/Paths.h" - -#include "EngineLib/LogMacros.h" -#include "Platform/DataManager.h" - -using namespace mathlib; -using namespace constants; - -namespace robots { - // Constructor - RobotSystem::RobotSystem() - : _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), - _AD_integrator(std::make_unique()), _curIntMethod_AD(integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler), - _kinematics(std::make_unique()), _dynamics(std::make_unique()), - _torqueMode(eTorqueMode::CONTROLLED) { - if (!_integrator ) { LOG_WARN("RobotSystem got null IntegrationService*"); } - } - // Destructor - RobotSystem::~RobotSystem() = default; - - const robots::RobotModel& RobotSystem::model() const { return _robot; } - - // Helper function to convert std::vector to Eigen::VectorXd - static VecX toVecX(const std::vector& a) { - VecX v(a.size()); - for (size_t i = 0; i < a.size(); ++i) { v(i) = a[i]; } - return v; - } - - // --- HELPER METHODS --- - - // Method to clamp a joint angle to its limits - double RobotSystem::clampJointAngle(const RobotJoint& joint, double angleRad) { - if (joint.limits.continuous) { return wrapRad(angleRad); } - else { return std::clamp(angleRad, joint.limits.minAngle, joint.limits.maxAngle); } - } - - // 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; - - // Check if we're in the "soft zone" near the velocity limit - const double absw = std::abs(omega); // [rad/s] - const double wSoft = 0.90 * wMax; // [rad/s] - - if (absw <= wSoft) { return; } - - // Check if we're above the hard limit (with some tolerance) - const double t = (absw - wSoft) / (wMax - wSoft); // [0, 1] as we go from wSoft to wMax - const double T = 0.2; // [s], time constant for how quickly the wall ramps up - const double wall = (I_eff / T) * (t * t) * (absw - wSoft); // [Nm] - - // Apply opposing torque to reduce |omega| - tau -= wall * (omega >= 0.0 ? 1.0 : -1.0); - } - - // --- ROBOT STATE INTEGRATION METHODS --- - - // Method to build a name-to-index map for robot links - void RobotSystem::buildLinkIndex() { - _linkIndex.clear(); - for (size_t i = 0; i < _robot.links.size(); i++) { - _linkIndex[_robot.links[i].name] = (int)i; - LOG_INFO("Link %zu: %s -> index %d", i, _robot.links[i].name.c_str(), (int)i); - } - } - - // Method to build the spatial model (kinematic tree) from the robot model - void RobotSystem::buildSpatialModel() { - _spatialModel.joints.clear(); - const size_t n = _robot.joints.size(); - _spatialModel.joints.resize(n); - - for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = _robot.joints[i]; - auto& sj = _spatialModel.joints[i]; - - sj.name = j.name; - sj.type = j.type; - - // Find parent joint - sj.parent = -1; - for (size_t p = 0; p < n; ++p) { - if (_robot.joints[p].child == j.parent) { - sj.parent = (int)p; - break; - } - } - - // Build XTree - mathlib::Mat3 R = j.origin_q.toRotationMatrix(); - mathlib::Vec3 r = j.origin_xyz; - sj.Xtree = mathlib::spatialTransform(R, r); - - // Build Spatial Inertia - int childLinkIdx = -1; - for (size_t l = 0; l < _robot.links.size(); ++l) { - if (_robot.links[l].name == j.child) { - childLinkIdx = (int)l; - break; - } - } - - if (childLinkIdx >= 0) { - const RobotLink& link = _robot.links[childLinkIdx]; - mathlib::Mat3 I_com; - - const auto& I = link.inertial.inertia; - I_com << - I.ixx, I.ixy, I.ixz, - I.ixy, I.iyy, I.iyz, - I.ixz, I.iyz, I.izz; - - sj.inertia = mathlib::spatialInertia( - link.inertial.mass, - link.inertial.com_xyz, - I_com - ); - } - - // Build S vector (motion subspace) - switch (j.type) { - case eJointType::REVOLUTE: - sj.S = mathlib::SpatialVec(j.axis.normalized(), mathlib::Vec3::Zero()); - break; - case eJointType::PRISMATIC: - sj.S = mathlib::SpatialVec(mathlib::Vec3::Zero(), j.axis.normalized()); - break; - default: - sj.S = mathlib::SpatialVec(); - break; - } - } - LOG_INFO("SpatialModel built: joints=%d", (long long)_spatialModel.joints.size()); - } - - // Method to pack robot joint states into a state vector - mathlib::VecX RobotSystem::packState() const { - const size_t n = static_cast(_robot.joints.size()); - mathlib::VecX x(2 * n); - - // Pack angles and velocities - for (size_t i = 0; i < n; ++i) { - auto& j = _robot.joints[i]; - - // Current states - x[i] = j.q; - x[i + n] = j.qd; - } - return x; // state vector - } - - // Method to unpack state vector into robot joints - void RobotSystem::unpackState(const mathlib::VecX& x) { - const size_t n = static_cast(_robot.joints.size()); - - // Resize clamping vectors if necessary - if (_clampTheta.size() != n) { _clampTheta.assign(n, 0); } - if (_clampOmega.size() != n) { _clampOmega.assign(n, 0); } - - // For each joint - for (size_t i = 0; i < n; ++i) { - auto& j = _robot.joints[i]; - - // Current states - double theta_in = x[i]; // [rad] - double omega_in = x[i + n]; // [rad/s] - - // Clamp joint angle - double theta_out = clampJointAngle(j, theta_in); - - // max |omega| - double wMax_hw = std::abs(j.limits.maxqd); - double omega_out = omega_in; - - // Velocity limit clamping - if (wMax_hw > 0.0f) { - const double eps = 0.05f; - if (std::abs(omega_in) > (1.0f + eps) * wMax_hw) { - omega_out = std::clamp(omega_in, -wMax_hw, wMax_hw); - } - } - - // Velocity limit enforcement - if (theta_out != theta_in) { - const double upperLimit = j.limits.maxAngle; - const double lowerLimit = j.limits.minAngle; - if (theta_out >= upperLimit && omega_in > 0.0f) { omega_out = 0.0f; } - if (theta_out <= lowerLimit && omega_in < 0.0f) { omega_out = 0.0f; } - } - - // Record clamping - _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; - _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; - // Update joint states - j.q = theta_out; - j.qd = omega_out; - } - } - - // Method to pack robot joint states into a state vector - mathlib::VecX_T> RobotSystem::packState_AD() const { - using Dual = DualNumber_T; // only hardcoded since I am testing the same arm, TODO provide a better final way to derive the NVar val. - const size_t n = static_cast(_robot.joints.size()); - mathlib::VecX_T x(2 * n); - - // Pack angles and velocities - for (size_t i = 0; i < n; ++i) { - auto& j = _robot.joints[i]; - - // Current states - x[i] = Dual(j.q, { 0.0 }); - x[i + n] = Dual(j.qd, { 0.0 }); - } - return x; // state vector - } - - // Method to unpack state vector into robot joints - void RobotSystem::unpackState_AD(const mathlib::VecX_T>& x) { - using Dual = DualNumber_T; - const size_t n = static_cast(_robot.joints.size()); - - // Resize clamping vectors if necessary - if (_clampTheta.size() != n) { _clampTheta.assign(n, 0); } - if (_clampOmega.size() != n) { _clampOmega.assign(n, 0); } - - // For each joint - for (size_t i = 0; i < n; ++i) { - auto& j = _robot.joints[i]; - - // Current states - Dual theta_in = x[i]; // [rad] - Dual omega_in = x[i + n]; // [rad/s] - - // Clamp joint angle - Dual theta_out = clampJointAngle_T(j, theta_in); - - // max |omega| - const double wMax_hw = mathlib::abs(j.limits.maxqd); - Dual omega_out = omega_in; - - // Velocity limit clamping - if (wMax_hw > 0.0) { - omega_out = wMax_hw * mathlib::tanh(omega_in / wMax_hw); // smoothly clamp omega to wMax_hw using a tanh function - } - - // Velocity limit enforcement - if (theta_out != theta_in) { - const double upperLimit = j.limits.maxAngle; - const double lowerLimit = j.limits.minAngle; - if (theta_out >= upperLimit && omega_in > Dual(0)) { omega_out = Dual(0); } - if (theta_out <= lowerLimit && omega_in < Dual(0)) { omega_out = Dual(0); } - } - - // Record clamping - _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; - _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; - // Update joint states - j.q = mathlib::real(theta_out); - j.qd = mathlib::real(omega_out); - } - } - - // Method to pack reference state vector (target angles and velocities) for control - mathlib::VecX RobotSystem::packRefState() const { - const size_t n = (int)_robot.joints.size(); - mathlib::VecX x(2 * n); - for (size_t i = 0; i < n; ++i) { - auto& j = _robot.joints[i]; - - // Pack reference angles and velocities - x[i] = j.q_ref; - x[i + n] = j.qd_ref; - } - return x; // reference state vector - } - - // Method to unpack reference state vector into robot joints - void RobotSystem::unpackRefState(const mathlib::VecX& x) { - const size_t n = (int)_robot.joints.size(); - for (size_t i = 0; i < n; ++i) { - auto& j = _robot.joints[i]; - j.q_ref = x[i]; // [rad] - j.qd_ref = x[i + n]; // [rad/s] - j.q_ref = clampJointAngle(j, j.q_ref); // [rad] - } - } - - // Method to enforce joint limits after integration - void RobotSystem::enforceJointLimits(RobotJoint& j) { - if (j.limits.continuous) { return; } - - const double lo = j.limits.minAngle; - const double hi = j.limits.maxAngle; - - 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 - void RobotSystem::step(double dt, double simTime) { - if (!_hasRobot) { return; } - - if (_useAutoDiff) { - step_AD(dt, simTime); - return; - } - - _simTime = simTime; - const size_t n = _robot.joints.size(); - mathlib::VecX x = packState(); - - auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); - - unpackState(result.stepOut.x_next); - _dynamics->setDt(result.stepOut.dt_taken); - - const auto scratchCopy = _dynScratch; - const auto resultCopy = result; - - postStepUpdate(resultCopy.stepOut.x_next, scratchCopy, resultCopy); - - // Update base pose if free-floating - if (_baseIsFree) { - integrateBaseTranslation(dt); - updateBaseRootPose(); - } - - // Update kinematics - computeRobotKinematics(_worldTransforms); - } - - // Method to step the reference trajectory and update joint reference states - void RobotSystem::updateTrajectoryInputs(control::TrajectoryManager& traj, double t) { - if (!_hasRobot) { return; } - - const size_t n = _robot.joints.size(); - if (n <= 0) { return; } - - // Sample trajectories ("ground truth" inputs) - for (size_t i = 0; i < n; ++i) { - RobotJoint& j = _robot.joints[i]; - control::TrajState s{}; - // Try to evaluate trajectory - if (traj.tryEval(std::string(j.child), t, s)) { - j.q_ref = clampJointAngle(j, s.q); // set ref angle - j.qd_ref = s.qd; - j.qdd_ref = s.qdd; - } - // Store inputs - else { - j.qdd_ref = 0.0f; - j.qd_ref = 0.0f; - } - - auto* buf = _refBuffer; - if (buf) { - // Sim Metadata - buf->sim_time.push_back(t); - // Reference states - 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); - } - } - } - - // --- ROBOT LOADING AND RESET METHODS --- - - // Method to load a robot model by name - void RobotSystem::loadRobot(const std::string& name) { - if (name == _loadedName) { - LOG_INFO("Robot '%s' is already loaded, skipping load.", name.c_str()); - D_INFO("Robot '%s' is already loaded, skipping load.", name.c_str()); - return; - } - - // Reset control parameters to target values so that if the new robot has different defaults, we start with those - resetNaturalFrequencyToTarget(); - resetDampingRatioToTarget(); - - // Construct path to robot JSON file - const std::filesystem::path jsonPath = paths::assets() / "objects" / "Robotic_Arm_Models" / name / (name + ".json"); - if (!std::filesystem::exists(jsonPath)) { - LOG_ERROR("Robot JSON file not found -> %s", jsonPath.string().c_str()); - D_ERROR("Robot JSON file not found -> %s", jsonPath.string().c_str()); - return; - } - - // Load robot model from JSON - _robot = robots::RobotLoader::loadFromJSON(jsonPath.string()); - const size_t n = _robot.joints.size(); - const size_t m = _robot.links.size(); - - _constModel.name = _robot.name; - _constModel.scale = _robot.scale; - - _constModel.baseFrame = _robot.baseFrame; - _constModel.baseFrameIsAligned = _robot.baseFrameIsEngineAligned; - - _constModel.links = _robot.links; - _constModel.joints = _robot.joints; - - _constModel.linkNameToIndex.clear(); - for (size_t i = 0; i < _constModel.links.size(); ++i) { - _constModel.linkNameToIndex[_constModel.links[i].name] = (int)i; - } - - LOG_INFO_ONCE( - "CONST MODEL: links=%lld joints=%lld", - (long long)_constModel.links.size(), - (long long)_constModel.joints.size() - ); - - _loadedName = name; - _baseIsFree = false; - - // Check if any joint is free-floating to determine if the base is free - for (const auto& joint : _robot.joints) { - if (joint.type == eJointType::FREE) { - _baseIsFree = true; - break; - } - } - - _robotRootHome = _robot.baseFrame; - _robotRootPose = _robotRootHome; - - _robotQHome = _robot.makeJointVector(); - _robotHomeValid = true; - - buildLinkIndex(); - buildSpatialModel(); - _hasRobot = true; - - resetRobot(); - - LOG_INFO("Loaded robot model -> %s", name.c_str()); - D_SUCCESS("Loaded robot model -> %s", name.c_str()); - } - - // Method to reset the robot to its home position - void RobotSystem::resetRobot() { - if (!_hasRobot || !_robotHomeValid) { LOG_ERROR("Reset aborterd."); return; } - _robotRootPose = _robotRootHome; - _robot.setJointVector(_robotQHome); - - for (auto& joint : _robot.joints) { - joint.qd = 0.0; - joint.q_ref = joint.q; - joint.qd_ref = 0.0; - joint.qdd_ref = 0.0; - } - - // Reset base state if free-floating - _basePos = Vec3(0, 0, 0); - _baseVel = Vec3(0, 0, 0); - _baseAcc = Vec3(0, 0, 0); - - // Assuming base orientation is represented as a yaw angle for simplicity - _baseYaw = 0.0; - _baseYawRate = 0.0; - _baseYawAcc = 0.0; - - _dynScratch.clear(); - _dynScratch_AD.clear(); - - _dynResult.resize(0); - _dynResult_AD.resize(0); - - _dynScratch.resize(_robot.joints.size(), _robot.links.size()); - _dynScratch_AD.resize(_robot.joints.size(), _robot.links.size()); - _dynResult.resize(_robot.joints.size()); - _dynResult_AD.resize(_robot.joints.size()); - - _dynScratch.g.setConstant(_gravity); - - // Reset adaptive integrator so it doesn't carry a stale step size - _integrator->resetAdaptiveState(); - - computeRobotKinematics(_worldTransforms); - D_INFO("Robot reset to home position."); - D_SUCCESS("Robot reset to home position."); - } - - void RobotSystem::stopAll() { - if (!_hasRobot) return; - for (auto& joint : _robot.joints) { - joint.qd = 0.0f; - joint.q_ref = joint.q; - } - } - - integration::IntegrationService* RobotSystem::getIntegrator() { return _integrator.get(); } - const integration::IntegrationService* RobotSystem::getIntegrator() const { return _integrator.get(); } - void RobotSystem::setStandardIntegrator(integration::eIntegrationMethod m) { - _curIntMethod = m; _integrator->setIntegrationMethod(m); - } - - integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() { return _AD_integrator.get(); } - const integration::DifferentiableIntegrator* RobotSystem::getADIntegrator() const { return _AD_integrator.get(); } - void RobotSystem::setADIntegrator(integration::eAutoDiffIntegrationMethod m) { - _curIntMethod_AD = m; _AD_integrator->setIntegrationMethod(m); - } - - std::shared_ptr RobotSystem::runtimeIntegratorState() { - return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); - } - - std::shared_ptr RobotSystem::runtimeIntegratorState() const { - return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); - } - - // --- ROBOT KINEMATICS AND JOINT STATE METHODS --- - - std::string RobotSystem::findRootLink() const { - std::unordered_set children; - for (const auto& joint : _robot.joints) { children.insert(joint.child); } - for (const auto& link : _robot.links) { - if (children.find(link.name) == children.end()) { - return link.name; - } - } - return _robot.links.empty() ? "" : _robot.links.front().name; // fallback - } - - // Method to update the pose of each robot link based on current joint angles using forward kinematics - void RobotSystem::computeRobotKinematics(std::vector& world) { - if (!_hasRobot) { - world.clear(); - return; - }; - - world.resize(_robot.links.size()); - for (auto& T : world) { T = Mat4::Identity(); } - - // Find root link - const std::string rootName = findRootLink(); - auto itRoot = _linkIndex.find(rootName); - if (itRoot == _linkIndex.end()) { - LOG_WARN_ONCE("RobotSystem::updateRobotKinematics: root link '%s' not found in link index", rootName.c_str()); - return; - } - - // Set root link pose - int rootIdx = itRoot->second; - world[rootIdx] = _robotRootPose; - - // parent -> children joints - std::unordered_map> children; - children.reserve(_robot.joints.size()); - for (const auto& j : _robot.joints) children[j.parent].push_back(&j); - - std::stack st; - st.push(rootName); - - // Traverse the kinematic tree using DFS - while (!st.empty()) { - std::string parentName = st.top(); - st.pop(); - - // Skip if parent link not found - auto itP = _linkIndex.find(parentName); - if (itP == _linkIndex.end()) { continue; } - int pIdx = itP->second; - - const Mat4& T_parent = world[pIdx]; - - - // Find children joints - auto it = children.find(parentName); - if (it == children.end()) continue; - - // For each child joint - for (const RobotJoint* jp : it->second) { - const RobotJoint& j = *jp; - auto itC = _linkIndex.find(j.child); - if (itC == _linkIndex.end()) { continue; } - int cIdx = itC->second; - - // Joint origin transform - Mat4 T_joint = Mat4::Identity(); - T_joint.block<3, 1>(0, 3) = j.origin_xyz; - - // Joint origin rotation - Mat4 R_joint = Mat4::Identity(); - R_joint.block<3, 3>(0, 0) = j.origin_q.toRotationMatrix(); - - // Compute child link pose in world frame - Mat4 T_child = T_parent * T_joint * R_joint; - - Vec3 axis = j.axis.norm() > 1e-8 ? j.axis.normalized() : Vec3(0, 0, 1); // default axis if zero - - // Apply joint rotation for revolute joints - if (j.type == eJointType::REVOLUTE) { - Mat4 R_q = Mat4::Identity(); - R_q.block<3, 3>(0, 0) = Eigen::AngleAxisd(j.q, axis).toRotationMatrix(); - T_child = T_child * R_q; - } - else if (j.type == eJointType::PRISMATIC) { - Mat4 T_q = Mat4::Identity(); - T_q.block<3, 1>(0, 3) = axis * j.q; // translate along joint axis by q - T_child = T_child * T_q; - } - - // FIXED joints: no motion - world[cIdx] = T_child; - st.push(j.child); - } - } - } - - // --- 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; } - // Find joint child matching childLink - for (const auto& joint : _robot.joints) { - if (joint.child == childLink) { - outAngle = joint.q; - return true; - } - } - return false; - } - - // Method to set the angle of a specific robot joint - bool RobotSystem::trySetJointAngleRad(const std::string& childLink, double angleRad) { - if (!_hasRobot) { return false; } - // Find joint child matching childLink - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - joint.q = clampJointAngle(joint, angleRad); // clamp to joint limits - return true; - } - } - return false; - } - - // Method to get the angular velocity of a specific robot joint - bool RobotSystem::tryGetJointOmegaRad(const std::string& childLink, double& outOmega) const { - if (!_hasRobot) { return false; } - // Find joint child matching childLink - for (const auto& joint : _robot.joints) { - if (joint.child == childLink) { - outOmega = joint.qd; - return true; - } - } - return false; - } - - // Method to set the angular velocity of a specific robot joint - bool RobotSystem::trySetJointOmegaRad(const std::string& childLink, double omegaRad) { - if (!_hasRobot) { return false; } - // Find joint child matching childLink - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - joint.qd = omegaRad; - return true; - } - } - return false; - } - - // Method to directly inject an angular velocity into the state vector for a specific robot joint (bypassing any clamping or limits) - bool RobotSystem::injectJointOmegaRad(const std::string& childLink, double omega) { - if (!_hasRobot) return false; - const size_t n = _robot.joints.size(); - // Find joint child matching childLink - for (size_t i = 0; i < n; ++i) { - if (_robot.joints[i].child == childLink) { - // Modify actual state vector - mathlib::VecX x = packState(); - x[i + n] = omega; // velocity slot - unpackState(x); - return true; - } - } - 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; } - for (const auto& joint : _robot.joints) { - if (joint.child == childLink) { - outTargetRad = joint.q_ref; - return true; - } - } - return false; - } - - // Method to set the target angle (reference) of a specific robot joint in radians - bool RobotSystem::trySetJointTargetRad(const std::string& childLink, double targetRad) { - if (!_hasRobot) { return false; } - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - if (joint.limits.continuous) { joint.q_ref = wrapRad(targetRad); } - else { joint.q_ref = clampJointAngle(joint, targetRad); } // clamp to joint - return true; - } - } - return false; - } - - // Method to get the maximum angular velocity of a specific robot joint in radians - bool RobotSystem::tryGetJointOmegaMaxRad(const std::string& childLink, double& maxOmegaRad) const { - if (!_hasRobot) { return false; } - for (const auto& joint : _robot.joints) { - if (joint.child == childLink) { - maxOmegaRad = joint.limits.maxqd; - return true; - } - } - return false; - } - - // Method to set the maximum angular velocity of a specific robot joint in radians - bool RobotSystem::trySetJointOmegaMaxRad(const std::string& childLink, double maxOmegaRad) { - if (!_hasRobot) { return false; } - if (maxOmegaRad <= 0.0f) { return false; } - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - joint.limits.maxqd = maxOmegaRad; - return true; - } - } - return false; - } - - // Method to increment the target angle (reference) of a specific robot joint in radians - bool RobotSystem::tryAddJointTargetRad(const std::string& childLink, double deltaRad) { - if (!_hasRobot) { return false; } - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - double t = joint.q_ref + deltaRad; - if (joint.limits.continuous) { t = wrapRad(t); } - else { t = std::clamp(t, joint.limits.minAngle, joint.limits.maxAngle); } - joint.q_ref = t; // clamp to joint limits - return true; - } - } - return false; - } - - // Method to set the reference angular velocity of a specific robot joint in radians - bool RobotSystem::trySetJointOmegaRefRad(const std::string& childLink, double omegaRefRad) { - if (!_hasRobot) { return false; } - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - joint.qd_ref = omegaRefRad; - return true; - } - } - return false; - } - - // Method to set the reference angular acceleration of a specific robot joint in radians - bool RobotSystem::trySetJointAlphaRefRad(const std::string& childLink, double alphaRefRad) { - if (!_hasRobot) { return false; } - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - joint.qdd_ref = alphaRefRad; - return true; - } - } - return false; - } - - // Method to set the max Omega reference of a specific robot joint in radians - bool RobotSystem::trySetJointOmegaRefMaxRad(const std::string& childLink, double maxOmegaRad) { - if (!_hasRobot) { return false; } - if (maxOmegaRad <= 0.0f) { return false; } - for (auto& joint : _robot.joints) { - if (joint.child == childLink) { - joint.limits.omegaRefMaxRad_s = maxOmegaRad; - return true; - } - } - return false; - } - - // Method to zero the reference derivatives (velocity and acceleration) of a specific robot joint - bool RobotSystem::tryZeroJointRefDerivatives() { - if (!_hasRobot) { return false; } - for (auto& joint : _robot.joints) { - joint.qd_ref = 0.0f; - joint.qdd_ref = 0.0f; - } - return true; - } - - // Method to check if a specific robot joint is at its target angle within a tolerance (radians) - bool RobotSystem::isJointAtTargetRad(const std::string& childLink, double tolRad) const { - if (!_hasRobot) { return false; } - if (tolRad < 0.0f) { tolRad = -tolRad; } - - for (const auto& joint : _robot.joints) { - if (joint.child == childLink) { - double err = joint.q_ref - joint.q; - if (joint.limits.continuous) { err = wrapToPi(err); } - err = std::abs(err); - return err <= tolRad; - } - } - return false; - } - - // Method to check if a specific robot joint is at its target angle within a tolerance (degrees) - bool RobotSystem::isJointAtTargetDeg(const std::string& childLink, double tolDeg) const { - return isJointAtTargetRad(childLink, radians(tolDeg)); - } - - // Method to check if a specific robot joint is near a target angle within a tolerance (radians) - bool RobotSystem::isJointNearAngleRad(const std::string& childLink, double targetRad, double tolRad) const { - if (!_hasRobot) { return false; } - tolRad = std::abs(tolRad); - - for (const auto& joint : _robot.joints) { - if (joint.child == childLink) { - double err = targetRad - joint.q; - if (joint.limits.continuous) { err = wrapToPi(err); } - return std::abs(err) <= tolRad; - } - } - return false; - } - - // Method to check if a specific robot joint is near a target angle within a tolerance (degrees) - bool RobotSystem::isJointNearAngleDeg(const std::string& childLink, double targetDeg, double tolDeg) const { - return isJointNearAngleRad(childLink, radians(targetDeg), radians(tolDeg)); - } - - // --- ROBOT LINK AND ROOT POSE METHODS --- - - // Method to set the rotation angle of a specific robot link angle in degrees - bool RobotSystem::setRobotLinkRotation(const std::string& childLinkName, double angleDeg) { - for (auto& j : _robot.joints) { - if (j.child == childLinkName) { - j.q = radians(angleDeg); - computeRobotKinematics(_worldTransforms); - return true; - } - } - return false; - } - - Mat4 RobotSystem::setRobotRoot(const Vec3& pos, const Quat& rot) { - Mat4 T = Mat4::Identity(); - T.block<3, 1>(0, 3) = pos; - Mat4 R = Mat4::Identity(); - R.block<3, 3>(0, 0) = rot.toRotationMatrix(); - return T * R; - } - - // Method to set the robot root pose in world coordinates - void RobotSystem::setRobotRootPose(const Vec3& pos, const Quat& rot) { - _robotRootPose = setRobotRoot(pos, rot); - } - - // Method to set the robot root home pose in world coordinates - void RobotSystem::setRobotRootHome(const Vec3& pos, const Quat& rot) { - _robotRootHome = setRobotRoot(pos, rot);; - _robotRootPose = _robotRootHome; - } - - // Method to set the default pose of the robot using joint angles in degrees - bool RobotSystem::setDefaultPoseDeg() { - if (!_hasRobot) { return false; } - _robotQHome = _robot.makeJointVector(); - _robotHomeValid = true; - 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; - for (const auto& j : _robot.joints) { - if (j.name.find("hip_pitch") != std::string::npos) { drive += -j.qd; } - } - return drive; - } - - // Method to integrate the base translation of the robot based on leg joint angles (for legged robots) - void RobotSystem::integrateBaseTranslation(double dt) { - double hipL = 0.0; - double hipR = 0.0; - - tryGetJointAngleRad("left_hip_pitch_link", hipL); - tryGetJointAngleRad("right_hip_pitch_link", hipR); - - // Positive when left leg is in stance - const double gaitPhase = hipR - hipL; - - // Tunable gain: rad -> N - const double driveGain = 180.0; - double F_forward = -driveGain * gaitPhase; - _lastBaseForwardForce = F_forward; - - Vec3 dampingForce = -_baseLinearDamping * _baseVel; - Vec3 F_world(F_forward, 0.0, 0.0); - F_world += dampingForce; - _baseAcc = F_world / _baseMass; - - _baseVel += _baseAcc * dt; - _basePos += _baseVel * dt; - - LOG_INFO_ONCE("hipL=%.3f hipR=%.3f gaitPhase=%.3f", hipL, hipR, hipR - hipL); - LOG_INFO_ONCE("baseVel = (%.3f, %.3f, %.3f)", _baseVel.x(), _baseVel.y(), _baseVel.z()); - } - - // Method to update the robot root pose based on the integrated base translation (for legged robots) - void RobotSystem::updateBaseRootPose() { - Mat4 T = Mat4::Identity(); - T.block<3, 1>(0, 3) = Vec3(_basePos.x(), _basePos.y(), _basePos.z()); - - Mat4 R = Mat4::Identity(); - R.block<3, 3>(0, 0) = Eigen::AngleAxisd(_baseYaw, Vec3(0, 1, 0)).toRotationMatrix(); - - _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) { _robot.torqueMode = mode; } - - // Method to claim the current active log buffer for exporting logged data (returns pointer to buffer active before swap) - std::unique_ptr 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 - - auto out = std::make_unique(); // create a new buffer to return to caller - out->swap(_logBuffers[prev]); // swap contents of previous active buffer with new buffer - - return out; - } - - // 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 diff --git a/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp b/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp deleted file mode 100644 index ce546e2b..00000000 --- a/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// DSFE_Core TrajectoryManager.cpp -#include "pch.h" -#include "Robots/TrajectoryManager.h" -#include "Robots/RobotSystem.h" - -#include -#include -#include - -namespace control { - // Clear trajectory for a specific robot link - void TrajectoryManager::clear(const std::string& link) { _active.erase(link); } - - // Clears all active trajectories - void TrajectoryManager::clearAll() { _active.clear(); } - - // Evaluate the trajectory for a specific robot link at time t, returning the desired state in out - bool TrajectoryManager::tryEval(const std::string& link, double t, control::TrajState& out) const { - auto it = _active.find(link); - if (it == _active.end()) { return false; } - const auto r = it->second->eval(t); - out.q = r.q; - out.qd = r.qd; - out.qdd = r.qdd; - return true; - } - - // Check if a trajectory is active for a specific robot link - bool TrajectoryManager::hasActive(const std::string& link) const { - auto it = _active.find(link); - return (it != _active.end() && it->second); - } - - - // Set a trajectory for a specific robot link - void TrajectoryManager::set(const std::string& link, std::unique_ptr traj) { - if (!traj) { - _active.erase(link); - return; - } - _active.insert_or_assign(link, std::move(traj)); - } - - // Apply active trajectories to the robot at time t - void TrajectoryManager::apply(robots::RobotSystem& robot, double t) { - for (auto it = _active.begin(); it != _active.end();) { - const std::string& link = it->first; - auto& traj = it->second; - - // Evaluate trajectory at time t - const auto ref = traj->eval(t); - - // Apply trajectory reference to robot joint - const bool ok1 = robot.trySetJointTargetRad(link, (float)ref.q); - if (!ok1) { D_ERROR("Bad link key '%s' (no joint.child match)", link.c_str()); } - - const bool ok2 = robot.trySetJointOmegaRefRad(link, (float)ref.qd); - if (!ok2) { D_ERROR("Bad link key '%s' (no joint.child match)", link.c_str()); } - - const bool ok3 = robot.trySetJointAlphaRefRad(link, (float)ref.qdd); - if (!ok3) { D_ERROR("Bad link key '%s' (no joint.child match)", link.c_str()); } - - // Remove finished trajectories - if (traj->finished(t)) { - D_WARN("Trajectory finished immediately: link='%s' t=%.6f", link.c_str(), t); - it = _active.erase(it); - } - else { - ++it; - } - - } - } -} // namespace control \ No newline at end of file From ae8bff66694f0140e07ebd03599391f3dd5d47d2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 15:38:19 +0100 Subject: [PATCH 003/114] chore: Updated header comments on Analysis files --- DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h | 4 ++++ DSFE_App/DSFE_Core/include/Analysis/Telemetry.h | 13 ++++++++----- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h b/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h index 793d4406..1e501e6c 100644 --- a/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h +++ b/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h @@ -1,3 +1,7 @@ +/* + * File: Analysis/MetricLogger.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include diff --git a/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h b/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h index dd80ed2b..3346ead7 100644 --- a/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h +++ b/DSFE_App/DSFE_Core/include/Analysis/Telemetry.h @@ -1,4 +1,7 @@ -// DSFE_Core Telemetry.h +/* + * File: Analysis/Telemetry.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -11,7 +14,7 @@ #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" -namespace robots { class RobotSystem; } +namespace systems { class RigidBodySystem; } namespace control { class TrajectoryManager; } namespace diagnostics { @@ -134,13 +137,13 @@ namespace diagnostics { } // Update method to be called each simulation step - void update(double simTime, const robots::RobotSystem& robotSys, const control::TrajectoryManager* trajOpt = nullptr, eTelemetryLevel level = eTelemetryLevel::NONE) { + void update(double simTime, const systems::RigidBodySystem& sys, const control::TrajectoryManager* trajOpt = nullptr, eTelemetryLevel level = eTelemetryLevel::NONE) { if (level == eTelemetryLevel::NONE) { return; } // Record samples at the specified frequency while (simTime >= _next_t) { // Record telemetry data - record(_next_t, robotSys, trajOpt, level); + record(_next_t, sys, trajOpt, level); _next_t += _T; } } @@ -150,7 +153,7 @@ namespace diagnostics { private: // Record telemetry data at time t - void record(double t, const robots::RobotSystem& robotSys, const control::TrajectoryManager* trajOpt, eTelemetryLevel level); + void record(double t, const systems::RigidBodySystem& sys, const control::TrajectoryManager* trajOpt, eTelemetryLevel level); // Sampling parameters double _fs = 60.0; From 2d898c93ffc93e77464b6ca91827bd884417dff0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 15:40:41 +0100 Subject: [PATCH 004/114] refator: Updated files to use `RigidBody` rather than `Robot` for new naming format * Need to test build, but GUI requires changes first --- DSFE_App/DSFE_Core/CMakeLists.txt | 20 +- DSFE_App/DSFE_Core/include/DSL/Command.h | 32 + .../DSFE_Core/include/DSL/CommandContext.h | 112 ++ .../DSFE_Core/include/DSL/CommandFactory.h | 41 + .../DSFE_Core/include/DSL/Commands/LoadCmd.h | 56 + .../include/DSL/Commands/ParallelGroupCmd.h | 42 + .../include/DSL/Commands/RotateByCmd.h | 47 + .../include/DSL/Commands/RotateJointByCmd.h | 59 ++ .../include/DSL/Commands/RotateJointToCmd.h | 54 + .../include/DSL/Commands/RotateToCmd.h | 46 + .../DSFE_Core/include/DSL/Commands/SaveCmd.h | 58 ++ .../include/DSL/Commands/SelectCmd.h | 41 + .../DSFE_Core/include/DSL/Commands/SetCmd.h | 68 ++ .../include/DSL/Commands/SetOmegaCmd.h | 42 + .../DSFE_Core/include/DSL/Commands/SpinCmd.h | 46 + .../DSFE_Core/include/DSL/Commands/StartCmd.h | 43 + .../DSFE_Core/include/DSL/Commands/StopCmd.h | 42 + .../include/DSL/Commands/TrajClearCmd.h | 42 + .../include/DSL/Commands/TrajSetCmd.h | 46 + .../DSFE_Core/include/DSL/Commands/WaitCmd.h | 45 + DSFE_App/DSFE_Core/include/DSL/ICommand.h | 45 + .../DSFE_Core/include/DSL/IStoredProgram.h | 84 ++ DSFE_App/DSFE_Core/include/DSL/MainContext.h | 25 + DSFE_App/DSFE_Core/include/DSL/Parser.h | 47 + DSFE_App/DSFE_Core/include/DSL/ProgramData.h | 103 ++ .../DSFE_Core/include/DSL/RegisterCommand.h | 12 + DSFE_App/DSFE_Core/include/DSL/RunWrapper.h | 22 + DSFE_App/DSFE_Core/include/DSL/SimFwd.h | 19 + .../DSFE_Core/include/DSL/StoredProgram.h | 101 ++ DSFE_App/DSFE_Core/include/DSL/Token.h | 34 + DSFE_App/DSFE_Core/include/DSL/Utils.h | 71 ++ .../DSFE_Core/include/EngineLib/LogMacros.h | 5 +- .../DSFE_Core/include/Physics/DynamicsTypes.h | 194 ++++ .../DSFE_Core/include/Physics/PhysicsState.h | 5 +- .../include/Physics/RigidBodyDynamics.h | 172 +++ .../include/Physics/RigidBodyDynamics.inl | 647 ++++++++++++ .../include/Physics/RigidBodyKinematics.h | 52 + .../include/Physics/RigidBodyKinematics.inl | 108 ++ .../include/Physics/SpatialDynamics.h | 98 ++ .../include/Physics/SpatialDynamics.inl | 322 ++++++ .../include/Platform/SimulationState.h | 7 +- .../DSFE_Core/include/Scene/SimulationCore.h | 45 +- .../include/Systems/RigidBodyLoader.h | 15 + .../include/Systems/RigidBodyMetrics.h | 45 + .../include/Systems/RigidBodyModel.h | 213 ++++ .../include/Systems/RigidBodySnapshot.h | 84 ++ .../include/Systems/RigidBodySystem.h | 328 ++++++ .../include/Systems/RigidBodySystemStep.inl | 255 +++++ .../DSFE_Core/include/Systems/SpatialModel.h | 36 + .../include/Systems/SpatialModelCast.inl | 25 + .../include/Systems/TrajectoryManager.h | 39 + DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp | 19 +- .../DSFE_Core/src/Interpreter/Command.cpp | 29 +- .../src/Interpreter/CommandContext.cpp | 44 +- .../src/Interpreter/CommandFactory.cpp | 7 +- .../src/Interpreter/Commands/LoadCmd.cpp | 31 +- .../Interpreter/Commands/ParallelGroupCmd.cpp | 11 +- .../src/Interpreter/Commands/RotateByCmd.cpp | 9 +- .../Interpreter/Commands/RotateJointByCmd.cpp | 23 +- .../Interpreter/Commands/RotateJointToCmd.cpp | 23 +- .../src/Interpreter/Commands/RotateToCmd.cpp | 9 +- .../src/Interpreter/Commands/SelectCmd.cpp | 7 +- .../src/Interpreter/Commands/SetCmd.cpp | 24 +- .../src/Interpreter/Commands/SetOmegaCmd.cpp | 23 +- .../src/Interpreter/Commands/SpinCmd.cpp | 11 +- .../src/Interpreter/Commands/StartCmd.cpp | 7 +- .../src/Interpreter/Commands/StopCmd.cpp | 7 +- .../src/Interpreter/Commands/TrajClearCmd.cpp | 18 +- .../src/Interpreter/Commands/TrajSetCmd.cpp | 27 +- .../src/Interpreter/Commands/WaitCmd.cpp | 9 +- DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp | 21 +- .../src/Interpreter/RegisterCommand.cpp | 7 +- .../DSFE_Core/src/Interpreter/RunWrapper.cpp | 11 +- .../src/Interpreter/StoredProgram.cpp | 30 +- .../DSFE_Core/src/Interpreter/UIContext.cpp | 23 +- DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp | 23 +- .../src/Numerics/IntegrationService.cpp | 6 +- .../src/Physics/RigidBodyDynamics.cpp | 13 + .../src/Physics/RigidBodyKinematics.cpp | 14 + .../DSFE_Core/src/Platform/DataManager.cpp | 5 +- DSFE_App/DSFE_Core/src/Platform/Logger.cpp | 6 +- DSFE_App/DSFE_Core/src/Platform/Paths.cpp | 5 +- .../DSFE_Core/src/Platform/StudyRunner.cpp | 15 +- .../DSFE_Core/src/Scene/SimulationCore.cpp | 171 +-- .../src/Systems/RigidBodySnapshot.cpp | 46 + .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 986 ++++++++++++++++++ .../DSFE_Core/src/Systems/SystemLoader.cpp | 503 +++++++++ .../src/Systems/TrajectoryManager.cpp | 77 ++ 88 files changed, 6211 insertions(+), 329 deletions(-) create mode 100644 DSFE_App/DSFE_Core/include/DSL/Command.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/CommandContext.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/CommandFactory.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/LoadCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/ParallelGroupCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointByCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointToCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/RotateToCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/SaveCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/SetOmegaCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/StartCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/StopCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/TrajClearCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/TrajSetCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/ICommand.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/MainContext.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Parser.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/ProgramData.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/RegisterCommand.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/RunWrapper.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/SimFwd.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/StoredProgram.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Token.h create mode 100644 DSFE_App/DSFE_Core/include/DSL/Utils.h create mode 100644 DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h create mode 100644 DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h create mode 100644 DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl create mode 100644 DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h create mode 100644 DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl create mode 100644 DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h create mode 100644 DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl create mode 100644 DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h create mode 100644 DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h create mode 100644 DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h create mode 100644 DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h create mode 100644 DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h create mode 100644 DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl create mode 100644 DSFE_App/DSFE_Core/include/Systems/SpatialModel.h create mode 100644 DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl create mode 100644 DSFE_App/DSFE_Core/include/Systems/TrajectoryManager.h create mode 100644 DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp create mode 100644 DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp create mode 100644 DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp create mode 100644 DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp create mode 100644 DSFE_App/DSFE_Core/src/Systems/SystemLoader.cpp create mode 100644 DSFE_App/DSFE_Core/src/Systems/TrajectoryManager.cpp diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 947efe43..05a5bcae 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -36,13 +36,16 @@ set(SINGLE_BODY_SYS_SRC src/SingleBodySystem/Body.cpp ) -set(MULTI_BODY_SYS_SRC - src/Robots/RobotDynamics.cpp - src/Robots/RobotKinematics.cpp - src/Robots/RobotLoader.cpp - src/Robots/RobotSystem.cpp - src/Robots/TrajectoryManager.cpp - src/Robots/RobotSimSnapshot.cpp +set(SYSTEMS_SRC + src/Systems/SystemLoader.cpp + src/Systems/RigidBodySystem.cpp + src/Systems/TrajectoryManager.cpp + src/Systems/RigidBodySimSnapshot.cpp +) + +set(PHYSICS_SRC + src/Physics/RigidBodyDynamics.cpp + src/Physics/RigidBodyKinematics.cpp ) set(PLATFORM_SRC @@ -86,7 +89,8 @@ set(SIM_CORE_SRC src/Scene/SimulationCore.cpp) target_sources(DSFE_Core PRIVATE ${DEP_CORE_SRC} ${SINGLE_BODY_SYS_SRC} - ${MULTI_BODY_SYS_SRC} + ${SYSTEMS_SRC} + ${PHYSICS_SRC} ${PLATFORM_SRC} ${DSL_SRC} ${SIM_CORE_SRC} diff --git a/DSFE_App/DSFE_Core/include/DSL/Command.h b/DSFE_App/DSFE_Core/include/DSL/Command.h new file mode 100644 index 00000000..b3d87171 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Command.h @@ -0,0 +1,32 @@ +/* + * File: DSL/Command.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once +#include "EngineCore.h" +#include "DSL/ICommand.h" +#include "DSL/MainContext.h" + +namespace commands { + // Class representing a generic command + class DSFE_API Command : public ICommand { + public: + // Set the command context + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + + program_data::CmdResult update(CommandContext& cntx, double dt) override; + + void execute() override; + + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + + dsl::IStoredProgram* getProgram() const override { return _program; } + void setProgram(dsl::IStoredProgram* program) override { _program = program; } + + protected: + dsl::IStoredProgram* _program = nullptr; + CommandContext* _cntx = nullptr; + }; +} // namespace commands diff --git a/DSFE_App/DSFE_Core/include/DSL/CommandContext.h b/DSFE_App/DSFE_Core/include/DSL/CommandContext.h new file mode 100644 index 00000000..6e7c6616 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/CommandContext.h @@ -0,0 +1,112 @@ +/* + * File: DSL/CommandContext.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSL/SimFwd.h" +#include "DSL/Utils.h" + +#include "Platform/Logger.h" + +namespace commands { + struct DSFE_API ActiveRigidRot { + mathlib::Vec3 axisUnit{ 0.0, 0.0, 0.0 }; + mathlib::Quat qStart{ 1.0, 0.0, 0.0, 0.0 }; + mathlib::Quat qTarget{ 1.0, 0.0, 0.0, 0.0 }; + double maxOmega = 0.0; // rad/s + double epsAngle = 0.5 * constants::PI / 180; // rad + bool active = false; + }; + + struct DSFE_API ActiveJointRot { + std::string link; + double start = 0.0; + double target = 0.0; // rad + double maxOmega = 0.0; // rad/s + double epsAngle = 0.5 * constants::PI / 180; // rad + bool wrapShortest = true; + bool active = false; + }; + + // Class representing the command context + class DSFE_API CommandContext { + public: + CommandContext(core::ISimulationCore* core); + + // --- INITIALISATION METHODS --- + utils::OpResult startSim(); + utils::OpResult setFixedDt(double dt); + utils::OpResult loadSingleBody(const std::string& bodyName); + utils::OpResult loadMultibody(const std::string& bodyName); + + // --- GLOBAL STATE METHODS --- + + // Sets the angular units for rotation commands + void setAngularUnits(utils::AngularUnits units); + // Gets the current angular units + utils::AngularUnits getAngularUnits() const; + + // Sets the maximum absolute angular velocity (omega) clamp + void setOmegaClamp(double maxAbsOmega); + // Gets the current omega clamp value + double getOmegaClamp() const; + + // Stops all angular velocity for the body + utils::OpResult stopAllOmega(); // stops all angular velocity + + utils::OpResult setJointOmega(const std::string& childLink, double omegaDegPerSec); // deg/s + utils::OpResult stopJointOmega(const std::string& childLink); + + // --- HELPER METHODS --- + core::ISimulationCore* Core() const; + systems::RobotSystem& RigidBody() const; + + + // --- ROTATION COMMAND METHODS --- + + 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 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); + + //utils::OpResult beginRigidRotateTo(scene::Object* obj, mathlib::Vec3 axisUnit, double maxOmegaDegPerSec, double angleDeg); + utils::OpResult beginJointRotateTo(const std::string& link, double maxOmegaDegPerSec, double angleDeg); + + // --- READ-ONLY ACCESSORS --- + bool hasLink(std::size_t linkIndex) const; + + private: + core::ISimulationCore* _core = nullptr; + + utils::AngularUnits _angularUnits = utils::AngularUnits::DegPerSec; + double _omegaClamp = 0.0; // Default: no clamp + + ActiveRigidRot _rig; + ActiveJointRot _jnt; + + double NormaliseOmega(double omega) const; + double convertOmegaToInternal(double omega) const; + + mathlib::Vec3 normaliseDirection(const mathlib::Vec3& dir) const; + + double getJointAngleRad(const std::string& link) const; + + std::unordered_map _jointAngles; + + double _currentAngle = 0.0; // current angle for rotation commands + std::string _currentLinkName; // current link name for joint commands + + mathlib::Vec3 angularVelocityPrev = mathlib::Vec3::Zero(); + mathlib::Vec3 linearVelocityPrev = mathlib::Vec3::Zero(); + + double _dtheta = 0.0; // angle displacement + double _dt = 0.0; // time interval + double _omega = 0.0; // angular velocity + }; +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/CommandFactory.h b/DSFE_App/DSFE_Core/include/DSL/CommandFactory.h new file mode 100644 index 00000000..b332a1e3 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/CommandFactory.h @@ -0,0 +1,41 @@ +/* + * File: DSL/CommandFactory.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSL/ICommand.h" +#include +#include + +namespace commands { + // Type alias for command creator function + using Creator = std::unique_ptr(*)(const std::string&, const std::vector&); + + // CommandFactory class for registering and creating commands + class DSFE_API CommandFactory { + public: + // Get the singleton instance of CommandFactory + static CommandFactory& Instance(); + + // Public API + bool registerCommand(const std::string name, Creator creator); + // Create a command by name + ICommand* create(const std::string_view& name, const std::string& id, const std::vector& args) const; + // Check if a command is registered + bool hasCommand(const std::string_view& name) const; + // Get a list of registered command names + std::vector commandNames() const; + + // Delete copy constructor and assignment operator to prevent copies + CommandFactory(const CommandFactory&) = delete; + CommandFactory& operator=(const CommandFactory&) = delete; + + private: + //singleton instance + CommandFactory() = default; + + std::unordered_map _registry; + }; +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/LoadCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/LoadCmd.h new file mode 100644 index 00000000..e3c01b51 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/LoadCmd.h @@ -0,0 +1,56 @@ +/* + * File: DSL/Commands/LoadCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + // Enum for load target type + enum class LoadTargetType { + RigidBody + }; + + // Struct for load target + struct DSFE_API LoadTarget { + LoadTargetType type = LoadTargetType::RigidBody; + std::string path; + }; + + // Class representing the LOAD command + class DSFE_API LoadCmd final : public Command { + public: + // Constructor + LoadCmd(const std::string& id, const std::vector& tokens); + + // Get the command name + std::string_view getName() const { return "load"; } + + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + + // Get current result + CmdResult currentResult() const override { return getResult(); } + + // Execute the command + void execute() override; + + private: + LoadTarget _target{}; + CmdResult _result = { CmdState::NotStarted, {}, "" }; + std::string _path; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + // --- Free Function to Create LoadCmd --- + std::unique_ptr CreateLoadCmd(const std::string& id, const std::vector& tokens); +} // namespace commands diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/ParallelGroupCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/ParallelGroupCmd.h new file mode 100644 index 00000000..ea6baae8 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/ParallelGroupCmd.h @@ -0,0 +1,42 @@ +/* + * File: DSL/Commands/ParallelGroupCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include + +#include "DSL/Command.h" + +namespace commands { + class DSFE_API ParallelGroupCmd final : public Command { + public: + // Any: succeed if any command succeeds; All: succeed only if all commands succeed + enum class Policy { Any, All }; + // Constructor + ParallelGroupCmd(Policy policy, std::vector> cmds, double timeout = 0.0); + + // Delete copy constructor and assignment operator + ParallelGroupCmd(const ParallelGroupCmd&) = delete; + ParallelGroupCmd& operator=(const ParallelGroupCmd&) = delete; + // Default move constructor and assignment operator + ParallelGroupCmd(ParallelGroupCmd&&) noexcept = default; + ParallelGroupCmd& operator=(ParallelGroupCmd&&) noexcept = default; + + CmdResult update(CommandContext& cntx, double dt) override; + CmdResult currentResult() const override { return _result; } + + private: + void execute() override; + + Policy _policy; + std::vector> _cmds; + bool _started = false; + double _elapsed = 0.0; + double _timeoutSec = 0.0; + + CmdResult _result = { CmdState::NotStarted, {}, "" }; + }; +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h new file mode 100644 index 00000000..7f155d5e --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h @@ -0,0 +1,47 @@ +/* + * File: DSL/Commands/RotateByCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + + // Class representing the ROTATE command + class DSFE_API RotateByCmd final : public Command { + public: + // Constructor + RotateByCmd(utils::AxisMask axis, double maxOmegaDeg, double deltaDeg); + + std::string_view getName() const { return "rotateBy"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + program_data::CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + CmdResult update(CommandContext& cntx, double dt) override; + + utils::AxisMask _axes{}; + double _deltaDeg; + double _omegaDeg; + double _totalRotated = 0.0; + bool _started = false; + + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + // Free function to create a RotateCmd + std::unique_ptr CreateRotateByCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointByCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointByCmd.h new file mode 100644 index 00000000..73cbf795 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointByCmd.h @@ -0,0 +1,59 @@ +/* + * File: DSL/Commands/RotateJointByCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class DSFE_API RotateJointByCmd final : public Command { + public: + // Constructor + RotateJointByCmd(std::string link, double omegaDeg, double deltaDeg); + + std::string_view getName() const { return "rotateJointBy"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + CmdResult update(CommandContext& cntx, double dt) override; + + std::string _link; + double _deltaDeg = 0.0; + double _omegaDeg = 0.0; + double _totalRotated = 0.0; + + bool _started = false; + double _elapsed = 0.0; + double _timeoutSec = 10.0; + + double _deltaRad = 0.0; + double _maxOmegaRad = 0.0; + + double _targetRad = 0.0; + double _thetaStartRad = 0.0; + + double _settleT = 0.0; + double _noProgressT = 0.0; + double _bestAbsErr = 0.0; + + + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + // Free function to create a RotateCmd + std::unique_ptr CreateRotateJointByCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointToCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointToCmd.h new file mode 100644 index 00000000..3685a966 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointToCmd.h @@ -0,0 +1,54 @@ +/* + * File: DSL/Commands/RotateJointToCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + // Class representing the ROTATE command + class DSFE_API RotateJointToCmd final : public Command { + public: + // Constructor + RotateJointToCmd(std::string link, double maxOmegaDeg, double angleDeg); + + std::string_view getName() const { return "rotateJointTo"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + CmdResult update(CommandContext& cntx, double dt) override; + std::string _link; + double _angleDeg; // angle relative to the start position + double _maxOmegaDeg; + + bool _started = false; + double _elapsed = 0.0; + double _timeoutSec = 10.0; + + double _targetRad = 0.0; + double _maxOmegaRad = 0.0; + + double _settleT = 0.0; + double _noProgressT = 0.0; + double _bestAbsErr = 0.0; + + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + // Free function to create a RotateCmd + std::unique_ptr CreateRotateJointToCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/RotateToCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateToCmd.h new file mode 100644 index 00000000..aceedcbb --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateToCmd.h @@ -0,0 +1,46 @@ +/* + * File: DSL/Commands/RotateToCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + + // Class representing the ROTATE command + class DSFE_API RotateToCmd final : public Command { + public: + // Constructor + RotateToCmd(utils::AxisMask axes, double maxOmegaDeg, double angleDeg); + + std::string_view getName() const { return "rotateTo"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + CmdResult update(CommandContext& cntx, double dt) override; + + utils::AxisMask _axes; + double _angleDeg = 0.0; + double _maxOmegaDeg = 0.0; + bool _started = false; + + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + // Free function to create a RotateCmd + std::unique_ptr CreateRotateToCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SaveCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SaveCmd.h new file mode 100644 index 00000000..5ec0e7c1 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SaveCmd.h @@ -0,0 +1,58 @@ +/* + * File: DSL/Commands/SaveCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include +#include + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" + +namespace commands { + // Types of saves that can be performed by the SaveCmd + enum class eSaveType { + SimData, + Plots + }; + + struct ENGINE_API SaveCmdArgs { + eSaveType type; + std::string filename; + bool isIntegratorName; + }; + + class ENGINE_API SaveCmd : public Command { + public: + // Constructor + SaveCmd(const std::string& id, const std::vector& tokens); + + std::string_view getName() const { return "save"; } + void setContext(UIContext& cntx) { _cntx = &cntx; } + + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + + CommandContext* _cntx = nullptr; + SaveCmdArgs _target; + std::string _filename = ""; // Filename to save to + bool _integratorName = false; // Whether to include integrator name in the filename + bool _started = false; + + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateSaveCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h new file mode 100644 index 00000000..49099b94 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h @@ -0,0 +1,41 @@ +/* + * File: DSL/Commands/SelectCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include +#include + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" + +namespace commands { + class DSFE_API SelectCmd final : public Command { + public: + // Constructor + SelectCmd(); + + std::string_view getName() const { return "select"; } + void setContext(CommandContext& cntx) { _cntx = &cntx; } + + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + + CommandContext* _cntx = nullptr; + program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateSelectCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h new file mode 100644 index 00000000..76d3fb8f --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h @@ -0,0 +1,68 @@ +/* + * File: DSL/Commands/SetCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include +#include +#include + +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +#include "Platform/Logger.h" + +namespace commands { + + enum class SetTargetType { + IntegratorMethod, + Omega, + FixedDt, + Gravity + }; + + struct DSFE_API SetTarget { + SetTargetType type = SetTargetType::IntegratorMethod; + IntegratorMethod method = IntegratorMethod::RK4; // Default method + mathlib::Vec3 omega{ 0.0, 0.0, 0.0 }; + double fixedDt = 0.0; + double gravity = 0.0; + }; + + // Class representing the SET command + class DSFE_API SetCmd final : public Command { + public: + SetCmd(const std::string& id, const std::string& tokens); + std::string_view getName() const { return "set"; } + void setContext(CommandContext& cntx) { _cntx = &cntx; } + + // Getters and Setters for Result + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + // Get current method + IntegratorMethod getCurrentMethod() const { return _method; } + + // Execute the command + void execute() override; + + private: + SetTarget _target{}; + std::string _id; + std::string _tokens; + IntegratorMethod _method = IntegratorMethod::RK4; + CommandContext* _cntx = nullptr; + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + // --- Free Function to Create SetCmd --- + std::unique_ptr CreateSetCmd(const std::string& id, const std::vector& tokens); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SetOmegaCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SetOmegaCmd.h new file mode 100644 index 00000000..76f8d159 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SetOmegaCmd.h @@ -0,0 +1,42 @@ +/* + * File: DSL/Commands/SetOmegaCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class SetOmegaCmd final : public Command { + public: + SetOmegaCmd(std::string link, const double omega); + ~SetOmegaCmd() override = default; + + std::string_view getName() const { return "setomega"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + CmdResult update(CommandContext& cntx, double dt) override; + void execute() override; + + std::string _link; + double _omega; + bool _started = false; + CmdResult _result{ CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateSetOmegaCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h new file mode 100644 index 00000000..214c6936 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h @@ -0,0 +1,46 @@ +/* + * File: DSL/Commands/SpinCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "Interpreter/Command.h" +#include "Interpreter/CommandContext.h" + +namespace commands { + + // Class representing the ROTATE command + class DSFE_API SpinCmd final : public Command { + public: + // Constructor + SpinCmd(utils::AxisMask axes, double omegaDeg, double duration); + + std::string_view getName() const { return "SpinCmd"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + CmdResult update(CommandContext& cntx, double dt) override; + + utils::AxisMask _axes; + double _omegaDeg = 0.0; + double _duration = 0.0; + double _remainingTime = 0.0; + bool _started = false; + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message); + void markCompleted(); + bool hasStarted() const; + }; + + // Free function to create a RotateCmd + std::unique_ptr CreateSpinCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/StartCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/StartCmd.h new file mode 100644 index 00000000..91c72d38 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/StartCmd.h @@ -0,0 +1,43 @@ +/* + * File: DSL/Commands/StartCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include +#include + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class DSFE_API StartCmd final : public Command { + public: + // Constructor + StartCmd(); + + std::string_view getName() const { return "start"; } + void setContext(CommandContext& cntx) { _cntx = &cntx; } + + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + + CommandContext* _cntx = nullptr; + bool _started = false; + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateStartCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/StopCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/StopCmd.h new file mode 100644 index 00000000..36e44a56 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/StopCmd.h @@ -0,0 +1,42 @@ +/* + * File: DSL/Commands/StopCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include +#include + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class DSFE_API StopCmd final : public Command { + public: + // Constructor + StopCmd(); + + std::string_view getName() const { return "stop"; } + void setContext(CommandContext& cntx) { _cntx = &cntx; } + + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + + bool _started = false; + CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateStopCmd(const std::string& id, const std::vector& args); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/TrajClearCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajClearCmd.h new file mode 100644 index 00000000..36042656 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajClearCmd.h @@ -0,0 +1,42 @@ +/* + * File: DSL/Commands/TrajClearCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class TrajClearCmd final : public Command { + public: + explicit TrajClearCmd() = default; + ~TrajClearCmd() override = default; + + std::string_view getName() const { return "trajClear"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + CmdResult update(CommandContext& cntx, double dt) override; + void execute() override; + + core::ISimulationCore* _core = nullptr; + bool _done = false; + bool _started = false; + CmdResult _result{ CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateTrajClearCmd(const std::string& id, const std::vector& args); + +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/TrajSetCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajSetCmd.h new file mode 100644 index 00000000..b211b7ee --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajSetCmd.h @@ -0,0 +1,46 @@ +/* + * File: DSL/Commands/TrajSetCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class TrajSetCmd final : public Command { + public: + TrajSetCmd(std::string link, std::string type, std::vector params); + ~TrajSetCmd() override = default; + + std::string_view getName() const { return "trajSet"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + CmdResult update(CommandContext& cntx, double dt) override; + void execute() override; + + std::string _link; + std::string _type; + std::vector _params; + bool _done = false; + bool _started = false; + CmdResult _result{ CmdState::NotStarted, {}, "" }; + + static std::string upperCopy(std::string s); + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateTrajSetCmd(const std::string& id, const std::vector& args); + +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h new file mode 100644 index 00000000..a3962db8 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h @@ -0,0 +1,45 @@ +/* + * File: DSL/Commands/WaitCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include +#include + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class DSFE_API WaitCmd final : public Command { + public: + // Constructor + WaitCmd(double t); + + std::string_view getName() const { return "stop"; } + void setContext(CommandContext& cntx) { _cntx = &cntx; } + + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + void execute() override; + CmdResult update(CommandContext& cntx, double dt) override; + + utils::AxisMask _axes{}; + double _remainingTime = 0.0; + bool _started = false; + program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + + std::unique_ptr CreateWaitCmd(const std::string& id, const std::vector& args); +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/ICommand.h b/DSFE_App/DSFE_Core/include/DSL/ICommand.h new file mode 100644 index 00000000..e8a1301a --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/ICommand.h @@ -0,0 +1,45 @@ +/* + * File: DSL/ICommand.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSL/IStoredProgram.h" +#include +#include + +namespace commands { + // Forward declaration of ICommand for use in IStoredProgram + class CommandContext; + + // ICommand interface + class DSFE_API ICommand { + public: + // Virtual destructor + virtual ~ICommand() = default; + + // Context setters + virtual void setContext(CommandContext& cntx) = 0; + + // Update command + virtual program_data::CmdResult update(CommandContext& cntx, double dt) = 0; + + // Get current result + virtual program_data::CmdResult currentResult() const = 0; + + // Execute command + virtual void execute() = 0; + + virtual dsl::IStoredProgram* getProgram() const = 0; + virtual void setProgram(dsl::IStoredProgram* program) = 0; + + // Mark the command as failed with a message + virtual void markFailed(const std::string& message) = 0; + // Mark the command as completed + virtual void markCompleted() = 0; + // Check if the command has started + virtual bool hasStarted() const = 0; + }; + +} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h new file mode 100644 index 00000000..d2c3ed9f --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h @@ -0,0 +1,84 @@ +/* + * File: DSL/IStoredProgram.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSLProgramData.h" +#include "DSL/Utils.h" +#include +#include + +// Forward declarations +namespace commands { class ICommand; } +namespace scene { class Object; } + +namespace dsl { + // IStoredProgram interface + class DSFE_API IStoredProgram { + public: + // Virtual destructor + virtual ~IStoredProgram() = default; + + // Add a command to the program + virtual void add(std::unique_ptr cmd) = 0; + virtual void add(commands::ICommand* cmd) = 0; + + // Reset program to initial state + virtual void reset() = 0; + + // Clear all stored instructions + virtual void clear() = 0; + + // Start program execution + virtual void start() = 0; + // Start simulation + virtual void startSim() = 0; + + // Stop program execution + virtual void stop() = 0; + // Stop simulation + virtual void stopSim() = 0; + + // Puase program execution + virtual void pause() = 0; + // Wait for simulation to run for dt seconds + virtual void waitSim(double dt) = 0; + + // Step the program by dt + virtual void step(double dt) = 0; + // Get current program status + virtual ProgramStatus status() const = 0; + + // State checkers + virtual bool isEmpty() const = 0; + virtual bool isRunning() const = 0; + virtual bool isPaused() const = 0; + virtual bool isStopped() const = 0; + virtual bool isCompleted() const = 0; + virtual bool isFaulted() const = 0; + + // Update the command state + virtual CmdResult updateState() = 0; + + // Set & Get Current line number + virtual void setCurrentLineNumber(int lineNumber) = 0; + virtual int getCurrentLineNumber() const = 0; + + // Set & Get Integrator Method + virtual void setIntegratorMethod(IntegratorMethod method) = 0; + virtual IntegratorMethod getIntegratorMethod() const = 0; + + // Set & Get Omega + virtual void setOmega(mathlib::Vec3 omega, utils::AngularUnits units) = 0; + + // Set & Get Fixed Dt + virtual void setFixedDt(double dt) = 0; + virtual double getFixedDt() const = 0; + + // Set & Get Gravity + virtual void setGravity(double g) = 0; + virtual double getGravity() const = 0; + }; +} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/MainContext.h b/DSFE_App/DSFE_Core/include/DSL/MainContext.h new file mode 100644 index 00000000..a2ff018b --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/MainContext.h @@ -0,0 +1,25 @@ +/* + * File: DSL/MainContext.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSL/SimFwd.h" +#include "DSL/CommandContext.h" + +namespace commands { + // Main context to combine multiple command contexts for different subsystems (currently only command context) + class DSFE_API MainContext { + public: + MainContext(core::ISimulationCore* core) + : _motion(core) {} + + // Accessors + commands::CommandContext& motion() { return _motion; } + const commands::CommandContext& motion() const { return _motion; } + + private: + commands::CommandContext _motion; + }; +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Parser.h b/DSFE_App/DSFE_Core/include/DSL/Parser.h new file mode 100644 index 00000000..23067122 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Parser.h @@ -0,0 +1,47 @@ +/* + * File: DSL/Parser.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSL/ProgramData.h" +#include "DSL/IStoredProgram.h" +#include "DSL/Token.h" +#include +#include +#include + +#include "Platform/Logger.h" + +namespace dsl { + // Class representing a parsed command + class DSFE_API Parser { + public: + Parser(IStoredProgram* program); + void parse(std::string code); + + private: + IStoredProgram* _program = nullptr; + program_data::ProgramData _programData; + Command _currentCmd; + + std::vector _tokens; + std::vector lines; + size_t pos = 0; + + // --- Helper Functions --- + + // Helpers for parsing + static bool requiresIdentifier(std::string_view cmdName); + static bool matchIdentifier(const std::string& s); + + // Tokenise and classify code into commands + void tokeniseAndClassifyCode(const std::string& code); + + // --- Command and Program Builders --- + + void buildProgram(); + void buildCommand(Command& cmd); + }; +} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/ProgramData.h b/DSFE_App/DSFE_Core/include/DSL/ProgramData.h new file mode 100644 index 00000000..dfbafb33 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/ProgramData.h @@ -0,0 +1,103 @@ +/* + * File: DSL/ProgramData.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once +#include "EngineCore.h" +#include +#include +#include +#include + +namespace dsl { + // Struct representing source location + struct DSFE_API SrcLocation { + std::string filename; // Name of the source file + int line = 0; // Line number in the source file + int column = 0; // Column number in the source file + }; + + // Struct representing a single instruction + struct DSFE_API Command { + std::string rawLine; // The original line of code + std::string cmdName; // The command name + std::string identifier; // The command identifier + std::vector tokens; // The command arguments/tokens + int lineNumber = 0; // Line number in the source code + + // For parallel blocks + bool isParallelBlock = false; + double timeoutSec = 0.0; + std::vector inner; + }; + + // Struct representing program data + struct DSFE_API ProgramData { + std::vector cmd; // Vector storing the instructions + }; + + // Numerical integrator methods + enum class IntegratorMethod { + Euler, + Midpoint, + Heun, + Ralston, + RK4, + RK45, + ImplicitEuler, + ImplicitMidpoint, + GLRK2, + GLRK3, + AD_ImplicitEuler, + AD_ImplicitMidpoint, + AD_GLRK2, + AD_GLRK3 + }; + + // Enum representing the state of the program + enum ProgramState { + Empty, + Running, + Paused, + Stopped, + Completed, + Faulted + }; + + // Struct for program status + struct DSFE_API ProgramStatus{ + ProgramState state = ProgramState::Empty; + size_t pc = 0; + }; + + // Command states + enum CmdState { + NotStarted, + Executing, + Executed, + Failed + }; + + // Command signals (not used yet, but will be) + enum CmdSignalType { + CmdSignal_None, + CmdSignal_Start, + CmdSignal_Stop, + CmdSignal_Pause, + CmdSignal_Resume, + CmdSignal_Jump + }; + + // Command signal data struct + struct DSFE_API CmdSignalData { + CmdSignalType signal = CmdSignal_None; + size_t jumpTarget = 0; // for jump signals + }; + + // Command result struct + struct DSFE_API CmdResult { + CmdState state = CmdState::NotStarted; + CmdSignalData signalData; + std::string message; + }; +} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/RegisterCommand.h b/DSFE_App/DSFE_Core/include/DSL/RegisterCommand.h new file mode 100644 index 00000000..c5c469fb --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/RegisterCommand.h @@ -0,0 +1,12 @@ +/* + * File: DSL/RegisterCommand.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "DSL/CommandFactory.h" + +namespace commands { + // Free function to register all commands + void RegisterAllCommands(CommandFactory& factory); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/RunWrapper.h b/DSFE_App/DSFE_Core/include/DSL/RunWrapper.h new file mode 100644 index 00000000..3cb4d8dd --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/RunWrapper.h @@ -0,0 +1,22 @@ +/* + * File: DSL/RunWrapper.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSL/Parser.h" +#include "DSL/IStoredProgram.h" + +namespace dsl { + // Class that wraps the parsing and storing of a program + class DSFE_API RunWrapper { + public: + RunWrapper(Parser* parser, IStoredProgram* program); + // Parse and store the program from a code string + void runProgram(const std::string& code); + private: + Parser* _parser; + IStoredProgram* _program; + }; +} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/SimFwd.h b/DSFE_App/DSFE_Core/include/DSL/SimFwd.h new file mode 100644 index 00000000..91e19228 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/SimFwd.h @@ -0,0 +1,19 @@ +/* + * File: DSL/SimFwd.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" + +#include +#include +#include +#include +#include + +#include "Platform/Logger.h" + +// Forward declarations for the main classes used in the interpreter +namespace core { struct ISimulationCore; } +namespace systems { class RigidBodySystem; } diff --git a/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h b/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h new file mode 100644 index 00000000..cdca22af --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h @@ -0,0 +1,101 @@ +/* + * File: DSL/StoredProgram.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once +#include "EngineCore.h" +#include "DSL/IStoredProgram.h" +#include "DSL/ICommand.h" +#include "DSL/MainContext.h" +#include +#include + +namespace dsl { + // Class representing a stored program in the interpreter. + class DSFE_API StoredProgram : public IStoredProgram { + public: + // Constructor + StoredProgram(core::ISimulationCore* core); + ~StoredProgram() override; + + // Delete copy constructor and assignment operator to prevent copies + StoredProgram(const StoredProgram&) = delete; + StoredProgram& operator=(const StoredProgram&) = delete; + + // Delete move constructor and assignment operator to prevent moves + StoredProgram(StoredProgram&&) = delete; + StoredProgram& operator=(StoredProgram&&) = delete; + + void add(std::unique_ptr cmd) override; + void add(commands::ICommand* cmd) override; + + void reset() override; + + void clear() override; + + void start() override; + void startSim() override; + + void stop() override; + void stopSim() override; + + void pause() override; + void waitSim(double dt) override; + + // Step the program by dt + void step(double dt) override; + + // Get current program status + ProgramStatus status() const override; + + // State checkers + bool isEmpty() const override { return _commands.empty(); } + bool isRunning() const override { return _state == ProgramState::Running; } + bool isPaused() const override { return _state == ProgramState::Paused; } + bool isStopped() const override { return _state == ProgramState::Stopped; } + bool isCompleted() const override { return _state == ProgramState::Completed; } + bool isFaulted() const override { return _state == ProgramState::Faulted; } + + // Set & Get Current line number + void setCurrentLineNumber(int lineNumber) override { _currentLineNumber = lineNumber; } + int getCurrentLineNumber() const override { return _currentLineNumber; } + + // Set & Get Integrator Method + void setIntegratorMethod(IntegratorMethod method) override; + IntegratorMethod getIntegratorMethod() const override; + + // Set & Get Omega + void setOmega(mathlib::Vec3 omega, utils::AngularUnits units) override; + + // Set & Get Fixed Dt + void setFixedDt(double dt) override; + double getFixedDt() const override; + + // Set & Get Gravity + void setGravity(double gravity) override; + double getGravity() const override; + + private: + core::ISimulationCore* _core = nullptr; + commands::MainContext _cntx; + + // Bool for tracking if the program has reached the end + bool atEnd() const; + // Bool for tracking if there are commands left to execute + bool commandsLeft() const; + + ProgramState _state = ProgramState::Stopped; + bool _stopRequested = false; + + CmdResult updateState() override; + int _currentLineNumber = 0; + int PC = 0; // Program Counter + + std::vector> _commands; + + IntegratorMethod _integratorMethod = IntegratorMethod::RK4; // Default integrator method + double _gravity = 0.0; + double _dt = 0.0; + mathlib::Vec3 _rgb = mathlib::Vec3{ 1.0f, 0.0f, 0.0f }; + }; +} // namespace interpreter diff --git a/DSFE_App/DSFE_Core/include/DSL/Token.h b/DSFE_App/DSFE_Core/include/DSL/Token.h new file mode 100644 index 00000000..ba6d5d21 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Token.h @@ -0,0 +1,34 @@ +/* + * File: DSL/Token.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +namespace dsl { + enum class TokenType { + Unknown, + Comment, + Identifier, + Number, + LParen, + RParen, + LBrace, + RBrace, + Comma, + EndOfLine, + EndOfFile, + String + }; + + // Struct representing a token in the parser + struct DSFE_API Token { + TokenType type = TokenType::Unknown; + std::string value; + double numberValue = 0.0; + int lineNumber = 0; + int columnNumber = 0; + }; +} // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/Utils.h b/DSFE_App/DSFE_Core/include/DSL/Utils.h new file mode 100644 index 00000000..335a2be9 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Utils.h @@ -0,0 +1,71 @@ +/* + * File: DSL/Utils.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "DSL/SimFwd.h" +#include + +#include "Platform/Logger.h" + +namespace utils { + // Struct for operation result + struct DSFE_API OpResult { + bool ok = true; + std::string message; + bool done = false; + + static OpResult Success(bool done=false) { return { true, {}, done}; } + static OpResult Failure(const std::string& msg) { return OpResult{ false, msg, false}; } + }; + + // Struct for axis mask + struct DSFE_API AxisMask { + bool x = false; + bool y = false; + bool z = false; + + bool any() const { return x || y || z; } + }; + + enum class AngularUnits { + DegPerSec, + RadPerSec + }; + + // --- String Utilities --- + std::vector split(const std::string_view s, const std::string_view delimiters); + std::string_view trim(std::string_view str); + std::string toLower(std::string_view str); + std::string toUpper(std::string_view str); + std::string stripBraces(std::string s); + void ignoreCaseCompare(std::string& str); + bool startsWith(const std::string& str, const std::string& prefix); + bool endsWith(const std::string& str, const std::string& suffix); + bool contains(const std::string& str, const std::string& substr); + + // --- Type Checking Utilities --- + bool isInteger(const std::string_view s); + bool isFloat(const std::string_view s); + bool isDouble(const std::string_view s); + bool isBoolean(const std::string_view s); + + // --- Conversion Utilities --- + std::optional toBoolean(const std::string_view s); + + // --- Command Utilities --- + double parseDouble(const std::string_view s); + float parseFloat(const std::string s); + mathlib::Vec3 parseVec3(const std::string& str); + AxisMask parseAxisMask(const std::string& s); + //bool tryParseObjID(const std::string& s, scene::ObjectID& out); + + // --- Unit Conversion Utilities --- + double degToRad(double degrees); + mathlib::Vec3 degToRad(mathlib::Vec3& degrees); + + double radToDeg(double radians); + mathlib::Vec3 radToDeg(mathlib::Vec3& radians); +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/EngineLib/LogMacros.h b/DSFE_App/DSFE_Core/include/EngineLib/LogMacros.h index 306f2bb7..8b67d78a 100644 --- a/DSFE_App/DSFE_Core/include/EngineLib/LogMacros.h +++ b/DSFE_App/DSFE_Core/include/EngineLib/LogMacros.h @@ -1,4 +1,7 @@ -// DSFE_Core LogMacros.h +/* + * File: EngineLib/LogMacros.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once // ============================================ // Macros for logging information, warnings, and errors with automatic file and function context. diff --git a/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h new file mode 100644 index 00000000..cfb09f58 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h @@ -0,0 +1,194 @@ +/* + * File: Physics/DynamicsTypes.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include +#include + +#include "Systems/RigidBodyMetrics.h" + +namespace physics { + // Scratch buffers for dense dynamics + template + struct DenseDynamicsScratch { + mathlib::MatX_T M; // mass matrix + mathlib::VecX_T rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) + mathlib::VecX_T h; // Coriolis and centrifugal bias vector + mathlib::VecX_T tau; // control torque vector + mathlib::VecX_T I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) + + std::vector> T_world; + std::vector> jointWorldPoses; + + size_t jointCap = 0; + size_t linkCap = 0; + + // Resizes the scratch buffers + void resize(size_t nJoints, size_t nLinks) { + // Do not resize if the current capacities are sufficients + if (jointCap == nJoints + && linkCap == nLinks) { + return; + } + + // Dense buffers + M.resize(nJoints, nJoints); + rhs.resize(nJoints); + h.resize(nJoints); + tau.resize(nJoints); + I_eff_controller.resize(nJoints); + T_world.resize(nLinks); + jointWorldPoses.resize(nJoints); + + jointCap = nJoints; + linkCap = nLinks; + } + + // Sets all buffers to zero or identity + // * (ONLY FOR DEBUGGING PURPOSES, CALL clear() FOR PRODUCTION USE) + void zero() { + // Dense buffers + M.setZero(); + rhs.setZero(); + h.setZero(); + tau.setZero(); + I_eff_controller.setZero(); + for (auto& T : T_world) T.setIdentity(); + for (auto& T : jointWorldPoses) T.setIdentity(); + } + + void clear() { + // Dense buffers + M.resize(0, 0); + rhs.resize(0); + h.resize(0); + tau.resize(0); + I_eff_controller.resize(0); + T_world.clear(); + jointWorldPoses.clear(); + jointCap = 0; + linkCap = 0; + } + }; + + // Scratch buffers for spatial dynamics computations + template + struct SpatialDynamicsScratch { + std::vector> Xup; // spatial transformation from parent to current link + std::vector> IA; // articulated body inertia + std::vector> Ia; // articulated body inertia in the link frame + + std::vector> v; // spatial velocity + std::vector> c; // spatial bias acceleration + std::vector> a; // spatial acceleration + std::vector> pA; // articulated bias force + std::vector> U; // articulated body force + std::vector> f_ext; // spatial force + + mathlib::VecX_T u; // joint force contribution + mathlib::VecX_T d; // joint inertia contribution + + std::vector>> dXup_dq; // derivative of spatial transformation w.r.t. joint angles + + std::vector>> dv_dq; // derivative of spatial velocity w.r.t. joint angles + std::vector>> dv_dqd; // derivative of spatial velocity w.r.t. joint velocities + std::vector>> dc_dq; // derivative of spatial bias acceleration w.r.t. joint angles + std::vector>> dc_dqd; // derivative of spatial bias acceleration w.r.t. joint velocities + + size_t jointCap = 0; + + // Resizes the scratch buffers + void resize(size_t nJoints) { + // Do not resize if the current capacities are sufficient + if (jointCap == nJoints) { return; } + + // Spatial buffers + Xup.resize(nJoints); + IA.resize(nJoints); + Ia.resize(nJoints); + v.resize(nJoints); + c.resize(nJoints); + a.resize(nJoints); + pA.resize(nJoints); + U.resize(nJoints); + f_ext.resize(nJoints); + + u.resize(nJoints); + d.resize(nJoints); + + dXup_dq.resize(nJoints); + dv_dq.resize(nJoints); + dv_dqd.resize(nJoints); + dc_dq.resize(nJoints); + dc_dqd.resize(nJoints); + + jointCap = nJoints; + } + + void clear() { + Xup.clear(); + IA.clear(); + Ia.clear(); + v.clear(); + c.clear(); + a.clear(); + pA.clear(); + U.clear(); + f_ext.clear(); + + u.resize(0); + d.resize(0); + + dXup_dq.clear(); + dv_dq.clear(); + dv_dqd.clear(); + dc_dq.clear(); + dc_dqd.clear(); + + jointCap = 0; + } + }; + + // Output structure for dynamics computations + template + struct DynamicsResult { + mathlib::VecX_T dxdt; + mathlib::VecX_T qdd; + + systems::RigidBodyMetrics metrics; + + void resize(size_t n) { + dxdt.resize(2 * n); + qdd.resize(n); + metrics.resize(n); + } + }; + + // Central scratch structure that contains all buffers needed for dynamics computations, both dense and spatial + template + struct DynamicsScratch { + DenseDynamicsScratch dense; + SpatialDynamicsScratch spatial; + // TODO add kinematics scratch + + // Gravity scratch buffer + mathlib::VecX_T g; + + // Resizes all scratch buffers using the given number of joints and links + void resize(size_t nJoints, size_t nLinks) { + dense.resize(nJoints, nLinks); + spatial.resize(nJoints); + g.resize(nJoints); + } + + // Clears all scratch buffers + void clear() { + dense.clear(); + spatial.clear(); + g.resize(0); + } + }; +} // namespace physics \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/PhysicsState.h b/DSFE_App/DSFE_Core/include/Physics/PhysicsState.h index 967b9125..526dee4a 100644 --- a/DSFE_App/DSFE_Core/include/Physics/PhysicsState.h +++ b/DSFE_App/DSFE_Core/include/Physics/PhysicsState.h @@ -1,4 +1,7 @@ -// DSFE_Core PhysicsState.h +/* + * File: Physics/PhysicsState.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #pragma warning(disable : 4251) diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h new file mode 100644 index 00000000..3d44d7e7 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h @@ -0,0 +1,172 @@ +/* + * File: Physics/RigidBodyDynamics.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include + +#include "Physics/DynamicsTypes.h" +#include "Systems/RigidBodyMetrics.h" + +#include "Physics/SpatialDynamics.h" +#include "Systems/SpatialModel.h" +#include "Physics/RigidBodyKinematics.h" +#include "Systems/RigidBodySimSnapshot.h" + +#include "Systems/TrajectoryManager.h" + +#include +#include + +#include "EngineLib/LogMacros.h" + +// Forward declarations +namespace control { class TrajectoryManager; } +namespace integration { class IntegrationService; enum class eIntegrationMethod; } + +namespace systems { + // Forward declarations + struct RigidBodyLink; + struct RigidBodyJoint; + enum class eTorqueMode; +} // namespace systems + +namespace physics { + // Dynamics class responsible for computing inertia, mass matrix, gravity torque, control torques, and state derivatives + class DSFE_API RigidBodyDynamics { + public: + // Constructor + RigidBodyDynamics(); + + // Computes the inertia tensor of a body link + template + mathlib::Mat3_T computeLinkInertiaTensor(const Link& link) const; + + // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint + template + Scalar computeJointInertiaContribution( + const systems::RigidBodyJoint& joint, + const systems::RigidBodyLink& link, + const mathlib::Pose_T& jointWorldPose, + const mathlib::Pose_T& linkWorldPose + ) const; + + // Computes the full mass matrix M(q) based on the current state and body configuration + template + void computeMassMatrix( + const systems::RigidBodyConstModel& body, + const std::vector>& T_world, + const std::vector>& jointWorldPoses, + mathlib::MatX_T& M_out + ) const; + + // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and body configuration + template + mathlib::VecX_T computeCoriolisVector( + const systems::RigidBodyConstModel& body, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const std::vector>& T_world, + const mathlib::MatX_T& M + ) const; + + // Computes the gravity torque for a joint based on the current state and body configuration + template + mathlib::VecX_T computeGravityTorque( + const systems::RigidBodyConstModel& body, + const std::vector>& T_world, + const std::vector>& jointWorldPoses + ) const; + + // Computes the analytical Jacobian matrix J(q) for the body based on the current state and body configuration + template + void analyticalJacobian( + const systems::RigidBodyConstModel& body, + const mathlib::VecX_T& x, + mathlib::MatX_T& J_out, + DenseDynamicsScratch& scratch + ); + + // Computes the Coriolis and centrifugal torque for a joint based on the current state and body configuration + template + mathlib::VecX_T derivative_dense( + Scalar t, + const mathlib::VecX_T& x, + const systems::RigidBodySnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ); + + template + mathlib::VecX_T derivative_spatial( + const systems::SpatialModel& model, + Scalar t, + const mathlib::VecX_T& x, + const systems::RigidBodySnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ); + + template + void jacobian_spatial( + const systems::SpatialModel& model, + const mathlib::VecX_T& x, + const systems::RigidBodySnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DynamicsScratch& scratch + ); + + // Computes the derivative of the state vector with control gains based on the current state and body configurations + template + mathlib::VecX_T derivative_with_gains( + Scalar t, + const mathlib::VecX_T& x, + const systems::RigidBodySnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + DynamicsScratch& scratch, + DynamicsResult& out + ); + + // Computes the Jacobian matrix with control gains based on the current state and body configuration + template + void jacobian_with_gains( + const mathlib::VecX_T& x, + const systems::RigidBodySnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DenseDynamicsScratch& scratch + ); + + // Set the gravity strength for the body system + void setGravity(double gravity) { _gravity = gravity; } + const double getGravity() const { return _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 + std::unique_ptr _kinematics = nullptr; + + static bool isControlledJoint(eJointType t) { + return + t == eJointType::REVOLUTE || + t == eJointType::PRISMATIC; + } + + double _dt = 1.0 / 180.0; // default timestep for dynamics updates + + double _gravity{ 0.0 }; + bool _baseIsFree = false; + double _lastBaseForwardForce{ 0.0 }; + }; +} // namespace physics + +#include "Systems/RigidBodyDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl new file mode 100644 index 00000000..45a74558 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -0,0 +1,647 @@ +/* + * File: Physics/RigidBodyDynamics.inl + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +namespace physics { + // Computes the inertia tensor of a rigidbody link + template + mathlib::Mat3_T RigidBodyDynamics::computeLinkInertiaTensor(const Link& link) const { + const systems::Inertia& I = link.inertial.inertia; + + // Construct the inertia tensor matrix + mathlib::Mat3_T M = mathlib::Mat3_T::Zero(); + M << + I.ixx, I.ixy, I.ixz, + I.ixy, I.iyy, I.iyz, + I.ixz, I.iyz, I.izz; + + return M; // [kg*m^2], (3x3) inertia tensor in link frame + } + + // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint + template + Scalar RigidBodyDynamics::computeJointInertiaContribution( + const systems::RigidBodyJoint& joint, + const systems::RigidBodyLink& link, + const mathlib::Pose_T& jointWorldPose, + const mathlib::Pose_T& linkWorldPose + ) const { + const Scalar m = (Scalar)link.inertial.mass; + + // Rotation from link frame to world frame + mathlib::Mat3_T R_joint = jointWorldPose.template block<3, 3>(0, 0); + // Joint axis in world frame + mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_joint * joint.axis); + + // Position of joint in world frame + mathlib::Vec3_T joint_pos_world = jointWorldPose.template block<3, 1>(0, 3); + + // Rotation from link frame to world frame + mathlib::Mat3_T R_link = linkWorldPose.template block<3, 3>(0, 0); + mathlib::Vec3_T link_pos_world = linkWorldPose.template block<3, 1>(0, 3); + mathlib::Vec3_T com_world = R_link * Vec3_T(link.inertial.com_xyz) + link_pos_world; + + // Translational contribution (parallel axis theorem) + mathlib::Vec3_T r = com_world - joint_pos_world; + + // Translational contribution to inertia about the joint axis + Scalar I_trans = m * (axis_world.cross(r)).squaredNorm(); + + // Rotational contribution + mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); + mathlib::Mat3_T I_world = R_link * I_local * R_link.transpose(); + + // Rotational contribution to inertia about the joint axis + Scalar I_rot = axis_world.transpose() * I_world * axis_world; + Scalar I_eff_i = I_trans + I_rot; + + return Eigen::numext::maxi(I_eff_i, Scalar(1e-6)); // [kg*m^2], I_eff contribution of this joint + floor to avoid singularities + } + + // Computes the full mass matrix M(q) based on the current state and body configuration + template + void RigidBodyDynamics::computeMassMatrix( + const RigidBodyConstModel& body, + const std::vector>& T_world, + const std::vector>& jointWorldPoses, + mathlib::MatX_T& M_out + ) const { + const size_t n = body.joints.size(); + M_out.resize(n, n); + M_out.setZero(); + + // Compute its contribution to the mass matrix for each link + for (size_t k = 0; k < body.links.size(); ++k) { + const RigidBodyLink& link = body.links[k]; + const Scalar m = link.inertial.mass; + + if (m <= Scalar(0)) { continue; } + + const mathlib::Mat3_T R = T_world[k].template block<3, 3>(0, 0); // Rotation from link frame to world frame + const mathlib::Vec3_T p = T_world[k].template block<3, 1>(0, 3); // Center of mass of the link in world frame + const mathlib::Vec3_T com = R * link.inertial.com_xyz + p; // Center of mass in world frame + + mathlib::Mat3_T I_local = computeLinkInertiaTensor(link); // inertia tensor in link frame + mathlib::Mat3_T I_world = R * I_local * R.transpose(); // inertia tensor in world frame + + // Compute Jacobian columns for each joint and accumulate mass matrix contributions + for (size_t i = 0; i < n; ++i) { + const RigidBodyJoint& j_i = body.joints[i]; + if (j_i.type == eJointType::FIXED) { continue; } + if (!body.jointAffectsLink(i, k)) { continue; } // skip if joint i does not affect link k + + const mathlib::Pose_T& T_joint_i = jointWorldPoses[i]; // pose of joint i in world frame + + // Rotation from joint i frame to world frame + const mathlib::Mat3_T R_i = T_joint_i.template block<3, 3>(0, 0); // rotation from joint i frame to world frame + const mathlib::Vec3_T p_i = T_joint_i.template block<3, 1>(0, 3); // joint position in world frame + const mathlib::Vec3_T z_i = mathlib::safeNormalised(R_i * j_i.axis); // joint axis in world frame + + mathlib::Vec3_T J_vi = z_i.cross(com - p_i); // linear velocity Jacobian column for joint i + mathlib::Vec3_T J_wi = z_i; // angular velocity Jacobian column for joint i + + // Computes the contribution to the mass matrix from this link for joints i and j + for (size_t j = 0; j < n; ++j) { + const RigidBodyJoint& j_j = body.joints[j]; + if (j_j.type == eJointType::FIXED) { continue; } + + if (!body.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k + + const mathlib::Pose_T& T_joint_j = jointWorldPoses[j]; // pose of joint i in world frame + + const mathlib::Mat3_T R_j = T_joint_j.template block<3, 3>(0, 0); + const mathlib::Vec3_T p_j = T_joint_j.template block<3, 1>(0, 3); + const mathlib::Vec3_T z_j = mathlib::safeNormalised(R_j * j_j.axis); + + mathlib::Vec3_T J_vj = z_j.cross(com - p_j); // linear velocity Jacobian column for joint j + mathlib::Vec3_T J_wj = z_j; // angular velocity Jacobian column for joint j + + M_out(i, j) += m * J_vi.dot(J_vj) + J_wi.transpose() * I_world * J_wj; // [kg*m^2] + } + } + } + } + + // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and body configuration + template + mathlib::VecX_T RigidBodyDynamics::computeCoriolisVector( + const RigidBodyConstModel& body, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const std::vector>& T_world, + const mathlib::MatX_T& M + ) const { + const size_t n = body.joints.size(); + const Scalar eps = Scalar(1e-6); // small value to prevent division by zero + mathlib::VecX_T q_eps = q; + + std::vector> dM_dq(n, mathlib::MatX_T::Zero(n, n)); // partial derivatives of M with respect to each joint angle + mathlib::VecX_T x_eps(2 * n); // state vector for kinematics + + std::vector> T_world_eps; // forward kinematics for perturbed configurations + T_world_eps.resize(body.links.size()); + + std::vector> jointWorldPoses_eps; + jointWorldPoses_eps.resize(n); + + mathlib::MatX_T M_plus(n, n); + + // Finite difference approximation of dM/dq for each joint + for (size_t k = 0; k < n; ++k) { + q_eps = q; // reset to original configuration for each joint perturbation + q_eps[k] += eps; // perturb joint k by a small amount + + // Construct the state vector for the perturbed configuration + for (size_t i = 0; i < n; ++i) { + x_eps[i] = q_eps[i]; + x_eps[n + i] = qd[i]; + } + + // Compute forward kinematics for the perturbed state + _kinematics->computeForwardKinematics_fromState(body, x_eps, T_world_eps); + jointWorldPoses_eps = _kinematics->calcJointWorldPoses(T_world_eps, body); + computeMassMatrix(body, T_world_eps, jointWorldPoses_eps, M_plus); // mass matrix for the perturbed configuration + + if (!M_plus.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } + if (!M_plus.isApprox(M_plus, Scalar(1e-8))) { throw std::runtime_error("Mass matrix lost symmetry"); } + + dM_dq[k] = (M_plus - M) / eps; // [kg*m^2/rad], partial derivative of mass matrix with + } + + // Compute Coriolis and centrifugal bias vector h using Christoffel symbols of the first kind + VecX_T h = VecX_T::Zero(n); + for (size_t i = 0; i < n; ++i) { + for (size_t j = 0; j < n; ++j) { + for (size_t k = 0; k < n; ++k) { + Scalar C_ijk = 0.5 * (dM_dq[k](i, j) + dM_dq[j](i, k) - dM_dq[i](j, k)) * qd[k]; // Christoffel symbol of the first kind for indices (i, j, k) + h(i) += C_ijk * qd[j] * qd[k]; // contribution to Coriolis and centrifugal bias for joint i from joints j and k + } + } + } + return h; // [Nm], Coriolis and centrifugal bias vector for the body at configuration q and velocity qd + } + + // Computes the gravity torque for a joint based on the current state and body configuration + template + mathlib::VecX_T RigidBodyDynamics::computeGravityTorque( + const RigidBodyConstModel& body, + const std::vector>& T_world, + const std::vector>& jointWorldPoses + ) const { + const size_t n = body.joints.size(); + mathlib::VecX_T tau_G = mathlib::VecX_T::Zero(n); + Scalar g{ _gravity }; // [m/s^2], gravity acceleration magnitude + + // For each joint, sum the gravity contributions from all links + for (size_t i = 0; i < n; ++i) { + const RigidBodyJoint& j = body.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + + Scalar tau_g_i = Scalar(0); // [Nm], gravity torque contribution for joint i + + const mathlib::Pose_T& T_joint = jointWorldPoses[i]; // pose of joint i in world frame + + const mathlib::Mat3_T R_i = T_joint.template block<3, 3>(0, 0); + const mathlib::Vec3_T p_i = T_joint.template block<3, 1>(0, 3); + const mathlib::Vec3_T axis_world = mathlib::safeNormalised(R_i * body.joints[i].axis); + + // For each link, compute the gravitational force and its torque contribution about joint i + for (size_t k = 0; k < body.links.size(); ++k) { + const RigidBodyLink& link = body.links[k]; + const Scalar m = (Scalar)link.inertial.mass; + if (m <= Scalar(0)) { continue; } + + if (!body.jointAffectsLink(i, k)) { continue; } + + // Link's center of mass in world frame + const mathlib::Mat3_T R_k = T_world[k].template block<3, 3>(0, 0); + const mathlib::Vec3_T com_world = R_k * link.inertial.com_xyz + T_world[k].template block<3, 1>(0, 3); + + // Gravitational force on the link + mathlib::Vec3_T g_world; + g_world = mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame + const mathlib::Vec3_T F_g = m * g_world; // [N], gravitational force on the link in world frame + const mathlib::Vec3_T r = com_world - p_i; // [m] + + // Torque = r × F_g projected onto joint axis + tau_g_i += axis_world.dot(r.cross(F_g)); + } + tau_G[i] = tau_g_i; + } + return tau_G; // [Nm], gravity torques for each joint + } + + // Computes the analytical Jacobian matrix J(q) for the body based on the current state and body configuration + template + void RigidBodyDynamics::analyticalJacobian( + const RigidBodyConstModel& body, + const mathlib::VecX_T& x, + mathlib::MatX_T& J_out, + DenseDynamicsScratch& scratch + ) { + const size_t n = body.joints.size(); + + Eigen::Map> q_local(x.data(), n); + Eigen::Map> qd_local(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(body, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, body); + computeMassMatrix(body, scratch.T_world, scratch.jointWorldPoses, scratch.M); + + J_out.setZero(2 * n, 2 * n); // [rad/rad] position part, [rad/s / rad/s] velocity part + J_out.block(0, n, n, n).setIdentity(); + + mathlib::MatX_T dTau_dq = mathlib::MatX::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const RigidBodyJoint& joint = body.joints[i]; + if (joint.type == eJointType::FIXED) { continue; } + + const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency + const Scalar z = static_cast(joint.zeta_target); // damping ratio + const Scalar eps = static_cast(1e-6); + + const Scalar I_eff = mathlib::LSE_smoothMax(scratch.M(i, i), eps); + + const Scalar k_p = I_eff * wn * wn; + const Scalar k_d = Scalar(2) * z * I_eff * wn; + + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + dTau_dq(i, i) = -k_p; + + Scalar qd_i = qd_local[i]; + Scalar tanh_term = mathlib::tanh(qd_i / eps_f); + Scalar stiff_friction_slope = c * (Scalar(1) - tanh_term * tanh_term) / eps_f; + dTau_dv(i, i) = -k_d - b + stiff_friction_slope; + } + + auto solver = scratch.M.ldlt(); + mathlib::MatX_T da_dq = solver.solve(dTau_dq); + mathlib::MatX_T da_dv = solver.solve(dTau_dv); + + J_out.block(n, 0, n, n) = da_dq; + J_out.block(n, n, n, n) = da_dv; + } + + // Computes the Coriolis and centrifugal torque for a joint based on the current state and body configuration + template + mathlib::VecX_T RigidBodyDynamics::derivative_dense( + Scalar t, + const mathlib::VecX_T& x, + const RigidBodySimSnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = snap.model->joints.size(); + mathlib::VecX_T dx(2 * n); + + // Map the input state vector to joint angles and velocities + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + + computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); // [kg*m^2], full mass matrix for the body at configuration q + scratch.dense.h = mathlib::VecX_T::Zero(n); // Temp test to isolate potential issues + //scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); // [Nm], full Coriolis and centrifugal torque vector + + if (!scratch.dense.M.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } + + scratch.g.setZero(); + if (snap.torqueMode != eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const RigidBodyJoint& joint = snap.model->joints[i]; + + // Fixed joints + if (joint.type == eJointType::FIXED) { + out.metrics.q[i] = q[i]; + out.metrics.qd[i] = qd[i]; + out.metrics.qdd[i] = 0.0; + continue; + } + + const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency + const Scalar z = static_cast(joint.zeta_target); // damping ratio + + const Scalar err = snap.q_ref[i] - q[i]; // [rad], position error + const Scalar err_d = snap.qd_ref[i] - qd[i]; // [rad/s], velocity error + + const Scalar eps = static_cast(1e-6); + + const Scalar I_eff = mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps); // [kg*m^2], effective inertia for joint i with floor to prevent singularities + const Scalar k_p = I_eff * wn * wn; // [Nm/rad], proportional gain + const Scalar k_d = Scalar(2.0) * z * I_eff * wn; // [Nm/(rad/s)], derivative gain + + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i + tau_i += scratch.g[i]; // Gravity compensation + tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias + // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation + // tau_i += tau_f; + + scratch.dense.tau[i] = tau_i; + + out.metrics.q[i] = mathlib::real(q[i]); + out.metrics.qd[i] = mathlib::real(qd[i]); + + out.metrics.err[i] = mathlib::real(err); + out.metrics.errd[i] = mathlib::real(err_d); + + out.metrics.I_eff[i] = mathlib::real(I_eff); + out.metrics.tau[i] = mathlib::real(tau_i); + } + + // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g + + // Solve for Accelerations + Eigen::LDLT> solver(scratch.dense.M); + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M"); + } + + out.qdd = solver.solve(scratch.dense.rhs); // [rad/s^2], joint accelerations computed from dynamics + + if (!out.qdd.allFinite()) { + LOG_ERROR("Non-finite joint accelerations computed. Check for singularities or numerical issues in the mass matrix."); + throw std::runtime_error("Non-finite joint accelerations computed from dynamics"); + } + + out.metrics.qdd = out.qdd; + + // Fill derivatives + dx.head(n) = qd; + dx.tail(n) = out.qdd; + + return dx; + } + + template + mathlib::VecX_T RigidBodyDynamics::derivative_spatial( + const systems::SpatialModel& model, + Scalar t, + const mathlib::VecX_T& x, + const RigidBodySimSnapshot_T& snap, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = model.joints.size(); + mathlib::VecX_T dx(2 * n); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + // Quick guard: check for non-finite states and bail with zero derivative + for (size_t i = 0; i < n; ++i) { + double q_r = mathlib::real(q[i]); + double qd_r = mathlib::real(qd[i]); + if (!std::isfinite(q_r) || !std::isfinite(qd_r)) { + LOG_ERROR("Non-finite state detected in derivative_spatial: q[%zu]=%g qd[%zu]=%g", i, q_r, i, qd_r); + // Return zero derivative to avoid propagating NaNs + dx.setZero(); + out.qdd.setZero(); + return dx; + } + } + + SpatialDynamics::computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(n); + mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(n); + mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, qd_zero, qdd_zero, scratch); + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& joint = model.joints[i]; + if (!isControlledJoint(joint.type)) { + scratch.dense.tau[i] = Scalar(0); + continue; + } + + const Scalar wn = static_cast(snap.model->joints[i].wn_target); + const Scalar z = static_cast(snap.model->joints[i].zeta_target); + + const Scalar err = snap.q_ref[i] - q[i]; + const Scalar err_d = snap.qd_ref[i] - qd[i]; + + const Scalar eps = static_cast(1e-6); + + const Scalar I_eff = mathlib::LSE_smoothMax(M(i, i), eps); + const Scalar k_p = I_eff * wn * wn; + const Scalar k_d = Scalar(2) * z * I_eff * wn; + + const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-3); + + Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; + Scalar tau_f = c * mathlib::tanh(qd[i] / Scalar(0.1)) + b * qd[i]; // simple friction model with viscous and Coulomb friction + tau_i += tau_f; + + const Scalar Q_max = static_cast(snap.model->joints[i].limits.maxEffort); + LOG_INFO_ONCE("Max effort for joint %zu: %g Nm", i, mathlib::real(Q_max)); + + tau_i = Q_max * mathlib::tanh(tau_i / Q_max); // saturate control torque to max effort using smooth tanh saturation + + scratch.dense.tau[i] = tau_i; + + out.metrics.q[i] = mathlib::real(q[i]); + out.metrics.qd[i] = mathlib::real(qd[i]); + + out.metrics.err[i] = mathlib::real(err); + out.metrics.errd[i] = mathlib::real(err_d); + + out.metrics.I_eff[i] = mathlib::real(I_eff); + out.metrics.tau[i] = mathlib::real(tau_i); + } + + out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); + out.metrics.qdd = out.qdd; + dx.head(n) = qd; + dx.tail(n) = out.qdd; + return dx; + } + + template + void RigidBodyDynamics::jacobian_spatial( + const systems::SpatialModel& model, + const mathlib::VecX_T& x, + const RigidBodySimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + + F_out.setZero(2 * n, 2 * n); + F_out.block(0, n, n, n).setIdentity(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + SpatialDynamics::computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + scratch.dense.M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); + + mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& joint = model.joints[i]; + if (!isControlledJoint(joint.type)) { continue; } + dTau_dq(i, i) = -kp[i]; + const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + dTau_dv(i, i) = -kd[i] - b; + } + + Eigen::LDLT> solver(scratch.dense.M); // compute the Cholesky decomposition of the mass matrix for efficient solving + + mathlib::MatX_T dqdd_dtau_q = solver.solve(dTau_dq); // compute the partial derivative of qdd with respect to q + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); + } + + mathlib::MatX_T dqdd_dtau_v = solver.solve(dTau_dv); // compute the partial derivative of qdd with respect to qd + + if (solver.info() != Eigen::Success) { + LOG_ERROR("LDLT decomposition failed for mass matrix M in jacobian_spatial. Matrix may be singular or ill-conditioned."); + throw std::runtime_error("LDLT decomposition failed for mass matrix M in jacobian_spatial"); + } + + F_out.block(n, 0, n, n) = dqdd_dtau_q; // fill the Jacobian block for qdd with respect to q + F_out.block(n, n, n, n) = dqdd_dtau_v; // fill the Jacobian block for qdd with respect to qd) + } + + // Computes the derivative of the state vector with control gains based on the current state and body configurations + template + mathlib::VecX_T RigidBodyDynamics::derivative_with_gains( + Scalar t, + const mathlib::VecX_T& x, + const RigidBodySimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + DynamicsScratch& scratch, + DynamicsResult& out + ) { + const size_t n = snap.model->joints.size(); + mathlib::VecX_T dx(2 * n); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.dense.T_world); + scratch.dense.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.dense.T_world, *snap.model); + + computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); + scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); + + scratch.g.setZero(); + if (snap.torqueMode != eTorqueMode::NONE) { + scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); + } + + scratch.dense.tau.setZero(); + for (size_t i = 0; i < n; ++i) { + const RigidBodyJoint& joint = snap.model->joints[i]; + if (joint.type == eJointType::FIXED) continue; + + const Scalar eps = static_cast < Scalar>(1e-6); + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; + tau_i += scratch.g[i] + scratch.dense.h[i]; + tau_i -= b * qd[i]; + tau_i -= c * mathlib::tanh(qd[i] / eps_f); + + scratch.dense.tau[i] = tau_i; + } + + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; + out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); + out.metrics.qdd = out.qdd; + + dx.head(n) = qd; + dx.tail(n) = out.qdd; + return dx; + } + + // Computes the Jacobian matrix with control gains based on the current state and body configuration + template + void RigidBodyDynamics::jacobian_with_gains( + const mathlib::VecX_T& x, + const RigidBodySimSnapshot_T& snap, + const mathlib::VecX_T& kp, + const mathlib::VecX_T& kd, + mathlib::MatX_T& F_out, + DenseDynamicsScratch& scratch + ) { + const size_t n = snap.model->joints.size(); + + F_out.setZero(2 * n, 2 * n); + F_out.block(0, n, n, n).setIdentity(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + // Compute a local mass matrix for this exact stage evaluation frame + _kinematics->computeForwardKinematics_fromState(*snap.model, x, scratch.T_world); + scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, *snap.model); + computeMassMatrix(*snap.model, scratch.T_world, scratch.jointWorldPoses, scratch.M); + + mathlib::MatX_T dTau_dq = mathlib::MatX_T::Zero(n, n); + mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); + + for (size_t i = 0; i < n; ++i) { + const RigidBodyJoint& joint = snap.model->joints[i]; + if (joint.type == eJointType::FIXED) continue; + + dTau_dq(i, i) = -kp[i]; + + const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient + const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient + const Scalar eps_f = static_cast(1e-2); + + Scalar tanh_term = mathlib::tanh(qd[i] / eps_f); + Scalar stiff_friction = -c * (Scalar(1.0) - tanh_term * tanh_term) / eps_f; + dTau_dv(i, i) = -kd[i] - b + stiff_friction; + } + + auto solver = scratch.M.ldlt(); + F_out.block(n, 0, n, n) = solver.solve(dTau_dq); + F_out.block(n, n, n, n) = solver.solve(dTau_dv); + } +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h new file mode 100644 index 00000000..d2150642 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h @@ -0,0 +1,52 @@ +// DSFE_Core RigidBodyKinematics.h +#pragma once + +#include "EngineCore.h" +#include +#include +#include "Systems/RigidBodySimSnapshot.h" + +#include "EngineLib/LogMacros.h" + +// Forward declarations +namespace systems { + struct RigidBodyLink; + struct RigidBodyJoint; +} + +namespace physics { + // Kinematics class responsible for computing forward kinematics and related transformations + class DSFE_API RigidBodyKinematics { + public: + // Constructor + RigidBodyKinematics(); + + // Computes the forward kinematics for the body based on the current state and body configuration + template + void computeForwardKinematics_fromState( + const systems::RigidBodyConstModel& body, + const mathlib::VecX_T& x, + std::vector>& T_world_out + ) const; + + // Computes the joint world poses for all joints based on the current state and body configuration + template + std::vector> calcJointWorldPoses( + const std::vector>& T_world, + const systems::RigidBodyConstModel& body + ); + + // Computes the forward kinematics for a single joint motion based on the joint axis and angle + template + mathlib::Pose_T jointMotionTransform( + const mathlib::Vec3_T& axis_joint, + Scalar q + ) const; + + // Converts roll-pitch-yaw angles (in radians) to a quaternion representation + template + mathlib::Quat_T rpyRadToQuat(const mathlib::Vec3_T& rpyRad); + }; +} // namespace physics + +#include "Physics/RigidBodyKinematics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl new file mode 100644 index 00000000..1b3b2a59 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl @@ -0,0 +1,108 @@ +/* + * File: Physics/RigidBodyKinematics.inl + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +namespace physics { + // Computes the forward kinematics for the body based on the current state and body configuration + template + void RigidBodyKinematics::computeForwardKinematics_fromState( + const systems::RigidBodyConstModel& body, + const mathlib::VecX_T& x, + std::vector>& T_world_out + ) const { + const auto& joints = body.joints; + const auto& links = body.links; + const size_t n = joints.size(); + + T_world_out.resize(body.links.size()); + + if (T_world_out.empty()) { + LOG_ERROR("T_world_out is empty"); + return; + } + + mathlib::Pose_T T = mathlib::Pose_T::Identity(); // world -> base + T_world_out[0] = T; // base link + + // Compute the transform to the next link using each joint + for (size_t i = 0; i < n; ++i) { + const auto& joint = joints[i]; + const Scalar q = x[i]; // joint angle from state vector + + mathlib::Pose_T T_origin = mathlib::Pose_T::Identity(); // transform from parent link to joint frame (fixed) + mathlib::Quat_T q_origin = joint.origin_q.template cast(); // convert quaternion to correct scalar type + + T_origin.template block<3, 3>(0, 0) = q_origin.toRotationMatrix(); // rotation from parent link frame to joint frame, derived from rpy in JSON + T_origin.template block<3, 1>(0, 3) = joint.origin_xyz.template cast(); // translation from parent link to joint frame + + // Compute joint motion transform based on joint axis and angle + Pose_T T_motion = mathlib::Pose_T::Identity(); + if (joint.type == eJointType::REVOLUTE) { + T_motion = jointMotionTransform(joint.axis.template cast(), q); // rotation about joint axis + } + else if (joint.type == eJointType::PRISMATIC) { + T_motion.template block<3, 1>(0, 3) = mathlib::safeNormalised(joint.axis) * q; // translation along joint axis + } + + // compose transforms + T = T * T_origin * T_motion; // parent -> joint -> motion -> child + + int childIdx = body.linkIndex(joint.child); + if (childIdx < 0 || childIdx >= T_world_out.size()) { + LOG_ERROR("Invalid child link index for joint {}: {}", joint.name.c_str(), childIdx); + continue; + } + + T_world_out[childIdx] = T; // world -> child link + } + } + + // Computes the joint world poses for all joints based on the current state and body configuration + template + std::vector> RigidBodyKinematics::calcJointWorldPoses( + const std::vector>& T_world, + const systems::RigidBodyConstModel& body + ) { + std::vector> jointWorldPoses(body.joints.size()); + + for (size_t i = 0; i < body.joints.size(); ++i) { + const RigidBodyJoint& joints = body.joints[i]; + int childIdx = body.linkIndex(joints.child); + + if (childIdx < 0 || childIdx >= T_world.size()) { + LOG_ERROR("Invalid child link index for joint {}: {}", joints.name.c_str(), childIdx); + continue; + } + + jointWorldPoses[i] = T_world[childIdx]; + } + return jointWorldPoses; + } + + // Computes the forward kinematics for a single joint motion based on the joint axis and angle + template + mathlib::Pose_T RigidBodyKinematics::jointMotionTransform( + const mathlib::Vec3_T& axis_joint, + Scalar q + ) const { + mathlib::Pose_T T = mathlib::Pose_T::Identity(); // homogeneous transformation matrix (4x4) + T.template block<3, 3>(0, 0) = mathlib::AngleAxis(q, mathlib::safeNormalised(axis_joint)); // set upper-left 3x3 block to rotation matrix + return T; // (4x4) homogeneous transformation + } + + // Converts roll-pitch-yaw angles (in radians) to a quaternion representation + template + mathlib::Quat_T RigidBodyKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { + const Scalar roll = rpyRad.x(); + const Scalar pitch = rpyRad.y(); + const Scalar yaw = rpyRad.z(); + + const Quat_T qx(Eigen::AngleAxis(roll, mathlib::Vec3_T(Scalar(1), Scalar(0), Scalar(0)))); + const Quat_T qy(Eigen::AngleAxis(pitch, mathlib::Vec3_T(Scalar(0), Scalar(1), Scalar(0)))); + const Quat_T qz(Eigen::AngleAxis(yaw, mathlib::Vec3_T(Scalar(0), Scalar(0), Scalar(1)))); + + return (qz * qy * qx).normalized(); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h new file mode 100644 index 00000000..939ca0aa --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h @@ -0,0 +1,98 @@ +/* + * File: Physics/SpatialDynamics.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "Systems/SpatialModel.h" +#include "Physics/DynamicsTypes.h" + +namespace physics { + class DSFE_API SpatialDynamics { + public: + template + static void computeSpatialKinematicsAndBias( + const systems::SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + std::vector>& Xup_out, + std::vector>& v_out, + std::vector>& c_out + ); + + template + static void computeAccelerations_RNEA( + const SpatialModel& model, + const mathlib::VecX_T& qdd, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& g, + std::vector>& a_out + ); + + template + static void computeBackwardForces_RNEA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out + ); + + template + static mathlib::VecX_T RNEA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& qdd, + DynamicsScratch& scratch + ); + + template + static mathlib::MatX_T CRBA( + const SpatialModel& model, + const std::vector>& Xup, + DynamicsScratch& scratch + ); + + template + static void computeArticulatedBodies_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& c, + const mathlib::VecX_T& tau, + std::vector>& IA_out, + std::vector>& pA_out, + std::vector>& Ia_out, + mathlib::VecX_T& u_out, + mathlib::VecX_T& d_out, + std::vector>& U_out + ); + + template + static void computeAccelerations_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& u_out, + const mathlib::VecX_T& d_out, + const std::vector>& U, + const SpatialVec_T& a0, + std::vector>& a_out, + mathlib::VecX_T& qdd_out + ); + + template + static mathlib::VecX_T ABA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& tau, + DynamicsScratch& scratch + ); + }; +} // namespace physics + +#include "Systems/SpatialDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl new file mode 100644 index 00000000..73d44adc --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -0,0 +1,322 @@ +/* + * File: Physics/SpatialDynamics.inl + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +namespace physics { + template + void SpatialDynamics::computeSpatialKinematicsAndBias( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + std::vector>& Xup_out, + std::vector>& v_out, + std::vector>& c_out + ) { + const size_t n = model.joints.size(); + v_out.resize(n); + Xup_out.resize(n); + c_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + // Joint Transform XJ + mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); + + if (j.type == eJointType::REVOLUTE) { + mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.angular()); + mathlib::Mat3_T R = mathlib::AngleAxis(q[i], axis); + mathlib::Vec3_T r = mathlib::Vec3_T::Zero(); + XJ = mathlib::spatialTransform(R, r); + } + else if (j.type == eJointType::PRISMATIC) { + mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.linear()); + mathlib::Vec3_T r = q[i] * axis; + mathlib::Mat3_T R = mathlib::Mat3_T::Identity(); + XJ = mathlib::spatialTransform(R, r); + } + + Xup_out[i] = XJ * j.Xtree; // Combined Transform + mathlib::SpatialVec_T vJ = j.S * qd[i]; // Joint Velocity + + // Root Link + if (j.parent < 0) { v_out[i] = vJ; } + else { v_out[i] = Xup_out[i] * v_out[j.parent] + vJ; } + + c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term + + using corScalar = typename std::decay_t; + static_assert(std::is_same_v, "c_out scalar type does not match model scalar type"); + } + } + + template + void SpatialDynamics::computeAccelerations_RNEA( + const SpatialModel& model, + const mathlib::VecX_T& qdd, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& g, + std::vector>& a_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + + mathlib::SpatialVec_T a0; // base acceleration (gravity) + a0.v << + g.template segment<3>(0), + g.template segment<3>(3); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T aJ = j.S * qdd[i]; // Joint Acceleration + + // Root Link + if (j.parent < 0) { + a_out[i] = Xup[i] * a0 + aJ + c[i]; + continue; + } + + a_out[i] = Xup[i] * a_out[j.parent] + aJ + c[i]; + } + } + + template + void SpatialDynamics::computeBackwardForces_RNEA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& a, + mathlib::VecX_T& tau_out + ) { + const size_t n = model.joints.size(); + tau_out.resize(n); + + std::vector> f(n); + + // Forward Force Computation + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + mathlib::SpatialVec_T I_v = j.inertia * v[i]; + mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); + f[i].v = j.inertia * a[i].v + coriolis.v; + } + + // Backward Recursion Computation + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + tau_out[i] = j.S.dot(f[i]); + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + if (j.parent >= 0) { f[j.parent] += XupT * f[i]; } + } + } + + template + mathlib::VecX_T SpatialDynamics::RNEA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& qdd, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + + // TODO Remove these temp scratches AFTER debugging + std::vector> v(n); + std::vector> Xup(n); + std::vector> c(n); + std::vector> a(n); + mathlib::VecX_T tau; + + // Compute spatial velocities and transforms + computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); + // Compute spatial accelerations + computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); + // Compute inverse dynamics (joint torques) + computeBackwardForces_RNEA(model, Xup, v, a, tau); + + return tau; + } + + template + mathlib::MatX_T SpatialDynamics::CRBA( + const SpatialModel& model, + const std::vector>& Xup, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + scratch.dense.M.setZero(n, n); + std::vector> Ic(n); // spatial inertia for each link + + // Initialise spatial inertia for each link based on the robot model + for (size_t i = 0; i < n; ++i) { Ic[i] = model.joints[i].inertia; } + + // Upward pass: propagate spatial inertia from child links to parent joints + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + int p = j.parent; + if (p >= 0) { + mathlib::MatX_T XupT = Xup[i].transpose(); + Ic[p] += XupT * Ic[i] * Xup[i]; + } + } + + // Downward pass: compute mass matrix contributions for each joint + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + if (j.type == eJointType::FIXED) { continue; } + mathlib::SpatialVec_T F = Ic[i] * j.S; + scratch.dense.M(i, i) = j.S.dot(F); + + int jIdx = (int)i; + while (model.joints[jIdx].parent >= 0) { + int p = model.joints[jIdx].parent; + mathlib::SpatialMat_T XupT = Xup[jIdx].transpose(); // TODO Make Eigen-copatible operator overloads for spatial transforms to avoid the errors from this transpose operation in a matrix multiplication context + F = XupT * F; + scratch.dense.M(i, p) = model.joints[p].S.dot(F); + scratch.dense.M(p, i) = scratch.dense.M(i, p); + jIdx = p; + } + } + return scratch.dense.M; // [kg*m^2], mass matrix computed using the Composite Rigid Body Algorithm (CRBA) + } + + template + void SpatialDynamics::computeArticulatedBodies_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& v, + const std::vector>& c, + const mathlib::VecX_T& tau, + std::vector>& IA_out, + std::vector>& pA_out, + std::vector>& Ia_out, + mathlib::VecX_T& u_out, + mathlib::VecX_T& d_out, + std::vector>& U_out + ) { + const size_t n = model.joints.size(); + + // Resize scratch buffers + IA_out.resize(n); + pA_out.resize(n); + Ia_out.resize(n); + U_out.resize(n); + u_out.resize(n); + d_out.resize(n); + + // Upward pass: compute articulated body inertias and bias forces + for (int i = (int)n - 1; i >= 0; --i) { + const SpatialJoint& j = model.joints[i]; + + if (j.type == eJointType::FIXED) { + Ia_out[i] = IA_out[i]; + if (j.parent >= 0) { + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; + pA_out[j.parent] += XupT * pA_out[i]; + } + continue; + } + + U_out[i] = IA_out[i] * j.S; + d_out[i] = dot(j.S, U_out[i]); + if (d_out[i] < Scalar(1e-12)) { + d_out[i] = Scalar(1e-12); + } + + u_out[i] = tau[i] - dot(j.S, pA_out[i]); + Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; + + // pA = pA + Ia * c + U * (u/d) + pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); + + if (j.parent >= 0) { + mathlib::SpatialMat_T XupT = Xup[i].transpose(); + IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; + pA_out[j.parent] += XupT * pA_out[i]; + } + } + } + + template + void SpatialDynamics::computeAccelerations_ABA( + const SpatialModel& model, + const std::vector>& Xup, + const std::vector>& c, + const mathlib::VecX_T& u_out, + const mathlib::VecX_T& d_out, + const std::vector>& U, + const SpatialVec_T& a0, + std::vector>& a_out, + mathlib::VecX_T& qdd_out + ) { + const size_t n = model.joints.size(); + a_out.resize(n); + qdd_out.resize(n); + + for (size_t i = 0; i < n; ++i) { + const SpatialJoint& j = model.joints[i]; + + if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } + else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } + + if (j.type == eJointType::FIXED) { + qdd_out[i] = Scalar(0); + continue; + } + + qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; + a_out[i] += j.S * qdd_out[i]; + } + } + + template + mathlib::VecX_T SpatialDynamics::ABA( + const SpatialModel& model, + const mathlib::VecX_T& q, + const mathlib::VecX_T& qd, + const mathlib::VecX_T& tau, + DynamicsScratch& scratch + ) { + const size_t n = model.joints.size(); + mathlib::VecX_T qdd = mathlib::VecX_T::Zero(n); + + mathlib::SpatialVec_T a0; // base acceleration (gravity) + a0.v << + scratch.g.template segment<3>(0), + scratch.g.template segment<3>(3); + + computeSpatialKinematicsAndBias( + model, q, qd, + scratch.spatial.Xup, + scratch.spatial.v, + scratch.spatial.c + ); + + for (size_t i = 0; i < n; ++i) { + scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia + scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); + } + + // Compute articulated body inertias and bias forces + computeArticulatedBodies_ABA( + model, scratch.spatial.Xup, + scratch.spatial.v, scratch.spatial.c, tau, + scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U + ); + + // Compute joint accelerations using the articulated body algorithm + computeAccelerations_ABA( + model, scratch.spatial.Xup, scratch.spatial.c, + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, + a0, scratch.spatial.a, qdd + ); + + return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) + } +} // namespace physics \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h index 446bbee0..3f19eaba 100644 --- a/DSFE_App/DSFE_Core/include/Platform/SimulationState.h +++ b/DSFE_App/DSFE_Core/include/Platform/SimulationState.h @@ -1,8 +1,11 @@ -// DSFE_Core SimulationState.h +/* + * File: Platform/SimulationState.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include +#include // Types of selections in the simulation enum class SelectionType { diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 759b0ce1..fa363942 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -1,4 +1,7 @@ -// DSFE_Core SimulationCore.h +/* + * File: Scene/SimulationCore.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -21,7 +24,7 @@ // Forward Declarations namespace control { class TrajectoryManager; } -namespace robots { class RobotSystem; } +namespace systems { class RigidBodySystem; } namespace single_body_system { class SingleBodySystem; } namespace interpreter { class IStoredProgram; } @@ -36,12 +39,12 @@ namespace core { SimulationCore(); ~SimulationCore(); - SimulationCore(robots::RobotSystem& robot, control::TrajectoryManager& traj); + SimulationCore(systems::RigidBodySystem& sys, control::TrajectoryManager& traj); // Buffer queue for exporting sim outputs void startExportThread(); void stopExportThread(); - void enqueueExportBuffer(std::unique_ptr buf); + void enqueueExportBuffer(std::unique_ptr buf); void flushExports(); // Simulation control @@ -77,7 +80,7 @@ namespace core { void setRunTag(const std::string& tag) override { _runTag = tag; } // Subsystems access - robots::RobotSystem& robotSystem() override; + systems::RigidBodySystem& rigidBoySystem() override; single_body_system::SingleBodySystem& singleBodySystem() override; control::TrajectoryManager& trajectoryManager() override; @@ -86,10 +89,10 @@ namespace core { void loadSingleBody(const std::string& name) override; void loadSingleBodyInternal(const std::string& name); // Internal method that assumes ownership - // Robot management - bool hasRobot() const override; - void loadRobot(const std::string& name) override; - void loadRobotInternal(const std::string& name); // Internal method that assumes ownership + // RigidBody management + bool hasRigidBody() const override; + void loadRigidBody(const std::string& name) override; + void loadRigidBodyInternal(const std::string& name); // Internal method that assumes ownership // Run a script to completion synchronously with a specific integrator bool runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) override; @@ -99,18 +102,18 @@ namespace core { size_t telemetrySampleCount() const override; // Setters for subsystems and scene objects - void setRobotSystem(robots::RobotSystem* robot); + void setRigidBodySystem(systems::RigidBodySystem* sys); void setSingleBodySystem(single_body_system::SingleBodySystem* singleBody); void setTrajectoryManager(control::TrajectoryManager* traj); - void setJointLogBuffer(robots::JointLogBuffer* buffer); - void setTrajRefBuffer(robots::TrajRefBuffer* buffer); + void setJointLogBuffer(systems::JointLogBuffer* buffer); + void setTrajRefBuffer(systems::TrajRefBuffer* buffer); // Helpers void tick(double frame_dt) override; void stepFixed(double frame_dt); // Export logged telemetry data to HDF5 files - void exportLogsToHDF5(const robots::JointLogBuffer& buf); + void exportLogsToHDF5(const systems::JointLogBuffer& buf); void exportRefsToHDF5(); // Increment simulation time by dt (used in the simulation loop) @@ -135,8 +138,8 @@ namespace core { void setActiveProgram(interpreter::IStoredProgram* p) override; interpreter::IStoredProgram* activeProgram() const override; - bool robotPresentationDirty() const override { return _robotPresentationDirty; } - void clearRobotPresentationDirty() override { _robotPresentationDirty = false; } + bool rigidBodyPresentationDirty() const override { return _rigidBodyPresentationDirty; } + void clearRigidBodyPresentationDirty() override { _rigidBodyPresentationDirty = false; } private: // Export thread management @@ -146,18 +149,18 @@ namespace core { std::thread _expThread; std::mutex _expMutex; std::condition_variable _expCondVar; - std::queue> _expQ; + std::queue> _expQ; std::atomic _expThreadRunning{ false }; // Owning storage (used only in owning mode) // std::unique_ptr>> _objectsOwned; - std::unique_ptr _robotOwned; + std::unique_ptr _rigidBodyOwned; std::unique_ptr _singleBodyOwned; std::unique_ptr _trajOwned; // Non-owning access (always used by logic) // std::vector>* _objects = nullptr; - robots::RobotSystem* _robot = nullptr; + systems::RigidBodySystem* _rigidBody = nullptr; single_body_system::SingleBodySystem* _singleBody = nullptr; control::TrajectoryManager* _traj = nullptr; @@ -182,13 +185,13 @@ namespace core { // Active Script Program interpreter::IStoredProgram* _activeProgram = nullptr; - bool _robotPresentationDirty = false; + bool _rigidBodyPresentationDirty = false; bool _singleBodyPresentationDirty = false; // 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 + systems::JointLogBuffer _jointLogBuffer; // Buffer for logging joint data each step + systems::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 diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h new file mode 100644 index 00000000..29347f69 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h @@ -0,0 +1,15 @@ +/* + * File: Systems/RigidBodyLoader.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "Systems/RigidBodyModel.h" + +namespace systems { + class DSFE_API RigidBodyLoader { + public: + static RigidBodyLoader loadFromJSON(const std::string& filepath); + }; +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h new file mode 100644 index 00000000..e0841f7d --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h @@ -0,0 +1,45 @@ +// DSFE_Core RobotMetrics.h +#pragma once + +#include "EngineCore.h" + + +namespace systems { + // Per-joint metrics + template + struct RigidBodyMetrics { + // State + mathlib::VecX_T q; + mathlib::VecX_T qd; + mathlib::VecX_T qdd; + + mathlib::VecX_T err; + mathlib::VecX_T errd; + + // Dynamics + mathlib::VecX_T I_eff; + mathlib::VecX_T tau; + + // Constraints / realism + mathlib::VecX_T tau_barrier; + mathlib::VecX_T tau_sat; + + // Energy, Work, & Power + mathlib::VecX_T KE; + mathlib::VecX_T PE; + mathlib::VecX_T E_total; + mathlib::VecX_T W_actuator; + + // Stability flags + std::vector sat_flag; + + void resize(size_t n) { + q.resize(n); qd.resize(n); qdd.resize(n); + err.resize(n); errd.resize(n); + I_eff.resize(n); tau.resize(n); + tau_barrier.resize(n); tau_sat.resize(n); + KE.resize(n); PE.resize(n); E_total.resize(n); W_actuator.resize(n); + sat_flag.resize(n, 0); + } + }; +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h new file mode 100644 index 00000000..d9b52e50 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h @@ -0,0 +1,213 @@ +/* + * File: Systems/RigidBodyModel.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once +#include "EngineCore.h" + +#include +#include +#include +#include +#include "Platform/Logger.h" +#include "EngineLib/LogMacros.h" + +namespace systems { + // --- RigidBody Kinematic Models --- + enum class eKinematicsModel { + URDF, + DH + }; + // --- URDF Joint Types --- + enum class eJointType { + FIXED = 0, + REVOLUTE = 1, + PRISMATIC = 2, + FREE = 3 + }; + // --- 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) + }; + + // --- RigidBody Model Links --- + + // Inertia tensor struct, representing the inertia of a link about its center of mass, expressed in the link's local frame + struct Inertia { double ixx = 0, ixy = 0, ixz = 0, iyy = 0, iyz = 0, izz = 0; }; + + // Inertial properties of a link + struct Inertial { + double mass = 0.0; + mathlib::Vec3 com_xyz{ 0.0,0.0,0.0 }; + Inertia inertia{}; + }; + + // Collision shape struct, supporting basic shapes (box, cylinder) and mesh (not implemented yet) + struct CollisionShape { + std::string type; + + // Collision Geometry + mathlib::Vec3 origin_xyz{ 0.0, 0.0, 0.0 }; + mathlib::Vec3 origin_rpy{ 0.0, 0.0, 0.0 }; + + // Collision Geometry Parameters + mathlib::Vec3 size{ 0.0, 0.0, 0.0 }; // cylinder -> size = [radius, length, 0], box -> size = [x, y, z] + std::string meshFile; // for mesh collision shapes, not implemented yet + mathlib::Vec4 material{ 1.0, 0.0, 0.2, 1.0 }; + float metallic = 0.5f; + float roughness = 0.5f; + }; + + // Per-mesh entry with individual material properties + struct VisualMeshEntry { + std::string meshFile; + mathlib::Vec4 material{ 1.0, 0.0, 0.2, 1.0 }; + float metallic = 0.5f; + float roughness = 0.5f; + bool hasMaterial = false; // true if material was explicitly specified + }; + + // Visual struct, representing the visual geometry of a link + struct Visual { + // Visual Geometry + mathlib::Vec3 origin_xyz{ 0.0, 0.0, 0.0 }; + mathlib::Vec3 origin_rpy{ 0.0, 0.0, 0.0 }; + + // Visual Geometry Parameters + std::vector meshFiles; // for multiple visual meshes per link (legacy, string-only) + std::vector meshEntries; // for multiple visual meshes with per-mesh material + }; + + // Link struct, representing a single link in the rigid body + struct RigidBodyLink { + std::string name; + + // Geometries (for rendering) + Visual visual{}; + std::vector collisions; + Inertial inertial{}; + }; + + // --- RigidBody Model Joints --- + + // Joint limits struct, representing the physical limits of a joint + struct JointLimit { + bool continuous = false; + double minAngle = 0.0; + double maxAngle = 0.0; + double maxqd = PI_d; + double maxEffort = 0.0; // max torque/force + // Soft limits + double omegaRefMaxRad_s = 0.0; + }; + + // Joint dynamics parameters, representing the damping and friction properties of a joint + struct JointDynamics { + double damping = 0.0; + double friction = 0.0; + }; + + // Joint struct, representing a single joint in the RigidBody model + struct RigidBodyJoint { + // Joint name and parent-child link names + std::string name = ""; + std::string parent = ""; + std::string child = ""; + + // URDF joint type + eJointType type = eJointType::REVOLUTE; + + // Parent joint axis and pivot (for visualization of the joint frame) + mathlib::Vec3 axisParent{ 0.0, 0.0, 1.0 }; + mathlib::Vec3 pivotParent{ 0.0, 0.0, 0.0 }; + + // URDF joint frame (parent → joint) + mathlib::Vec3 origin_xyz{ 0.0, 0.0, 0.0 }; // translation from parent link frame to joint frame, expressed in parent link frame + mathlib::Vec3 origin_rpy{ 0.0, 0.0, 0.0 }; // roll, pitch, yaw in radians + mathlib::Quat origin_q{ 1,0,0,0 }; // Rotation matrix from link frame to base frame, derived from rpy_deg in JSON + + // Axis expressed IN JOINT FRAME + mathlib::Vec3 axis{ 0.0, 0.0, 1.0 }; + + // --- Limits --- + JointLimit limits; + JointDynamics dynamics; + + // --- State --- + double q = 0.0; // rad + double qd = 0.0; // rad/s + double torque = 0.0; // Nm or N + double eta = 0.0f; // Integral state + + // --- Control --- + double q_ref = 0.0; // rad + double qd_ref = 0.0; // rad/s + double qdd_ref = 0.0; // rad/s^2 + + // --- Control Parameters --- + double wn_target = 0.0; // rad/s + double zeta_target = 0.0; // damping ratio + + // --- Precomputed transforms --- + mathlib::Mat4 jointToChildRest = mathlib::Mat4::Identity(); + mathlib::Mat4 parentToJoint = mathlib::Mat4::Identity(); + }; + + // --- RigidBody Model --- + + // RigidBodyModel struct, representing the entire rigid-body model + struct RigidBodyModel { + std::string name = "unnamed_body"; + float scale = 1.0f; + + // Links and joints + 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; + + // Visualization options + eVisualFrame visualFrame = eVisualFrame::JOINT; + std::unordered_map materials; + mathlib::Mat4 baseFrame = mathlib::Mat4::Identity(); // transform from world frame to rigidbody base frame, can be set in JSON + + bool baseFrameIsEngineAligned = false; + + // Create an Eigen vector of joint angles + VecX makeJointVector() const { + 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].q; } + return q; + } + + // Set joint angles from an Eigen vector + void setJointVector(const VecX& q) { + const int n = static_cast(joints.size()); + if (q.size() != n) { + LOG_ERROR("Joint vector size mismatch: expected %d, got %d", n, q.size()); + D_ERROR("Joint vector size mismatch: expected %d, got %d", n, q.size()); + return; + } + for (int i = 0; i < n; ++i) { + double a = q(i); + joints[i].q = a; + } + } + }; +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h new file mode 100644 index 00000000..4d1eb8d1 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h @@ -0,0 +1,84 @@ +/* + * File: Systems/RigidBodySnapshot.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "Systems/RigidBodyModel.h" +#include + +namespace systems { + // Immutable system data needed by solver threads + struct DSFE_API RigidBodyConstModel { + std::string name; + bool baseFrameIsAligned = false; + double scale = 1.0; + mathlib::Mat4 baseFrame = mathlib::Mat4::Identity(); + + bool jointAffectsLink(size_t jIdx, size_t lIdx) const; + + std::vector links; + std::vector joints; + + std::unordered_map linkNameToIndex; + int linkIndex(const std::string& linkName) const; + }; + + // Runtime snapshot for one integration/derivative step + template + struct RigidBodySnapshot_T { + + const RigidBodyConstModel* model = nullptr; + + mathlib::VecX_T q; // joint angles + mathlib::VecX_T qd; // joint velocities + + mathlib::VecX_T q_ref; // reference joint angles + mathlib::VecX_T qd_ref; // reference joint velocities + mathlib::VecX_T qdd_ref; // reference joint accelerations + + mathlib::Mat4_T root_pose = mathlib::Mat4_T::Identity(); + + bool baseIsFree = false; + + Scalar lastBaseForwardForce = Scalar(0); + Scalar gravity = Scalar(0); + + eTorqueMode torqueMode = eTorqueMode::CONTROLLED; + + Scalar dt = Scalar(0); + Scalar simTime = Scalar(0); + }; + using RigidBodySnapshot = RigidBodySnapshot_T; + + template + inline RigidBdoySnapshot_T castSnapshot( + const RigidBodySnapshot_T& src + ) { + RigidBodySnapshot_T dst; + + dst.model = src.model; + + dst.q = src.q.template cast(); + dst.qd = src.qd.template cast(); + + dst.q_ref = src.q_ref.template cast(); + dst.qd_ref = src.qd_ref.template cast(); + dst.qdd_ref = src.qdd_ref.template cast(); + + dst.root_pose = src.root_pose.template cast(); + + dst.baseIsFree = src.baseIsFree; + + dst.lastBaseForwardForce = ToScalar(src.lastBaseForwardForce); + dst.gravity = ToScalar(src.gravity); + + dst.torqueMode = src.torqueMode; + + dst.dt = ToScalar(src.dt); + dst.simTime = ToScalar(src.simTime); + + return dst; + } +} // namespace rigidbodys \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h new file mode 100644 index 00000000..91f78ff9 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -0,0 +1,328 @@ +/* + * File: Systems/RigidBodySystem.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include "EngineCore.h" +#include "Systems/RigidBodyModel.h" + +#include "Physics/SpatialModel.h" +#include "System/RigidBodySimSnapshot.h" +#include "Physics/DynamicsTypes.h" + +#include +#include "Physics/RigidBodyKinematics.h" +#include "Physics/RigidBodyDynamics.h" +#include "Physics/SpatialDynamics.h" + +#include "Analysis/MetricLogger.h" +#include "Numerics/IntegrationService.h" + +// Forward declarations +namespace control { class TrajectoryManager; } + +namespace systems { + // Forward declarations + enum class eTorqueMode; + + // Joint state structure + struct DSFE_API JointState { + double theta; + double omega; + }; + + // Metrics structure + enum class eRole { + Simulation, + Baseline + }; + + // Step Result struct + template + struct RigidBodyStepResult_T { + integration::StepOut_T stepOut; + RigidBodySnapshot_T snap; + DynamicsResult dynamics; + mathlib::VecX_T tau_rnea; + }; + + inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) + + class DSFE_API RigidBodySystem { + public: + RigidBodySystem(); + ~RigidBodySystem(); + + // --- Utility Methods --- + + static double clampJointAngle(const RigidBodyJoint& joint, double angleRad); + template + static T clampJointAngle_T(const RigidBodyJoint& joint, T angleRad); + + // ---- Accessors --- + + const systems::RigidBodyModel& model() const; + const std::vector& worldTransforms() const { return _worldTransforms; } + + const std::vector& links() const { return _body.links; } + std::vector& links() { return _body.links; } + const std::vector& joints() const { return _body.joints; } + std::vector& joints() { return _body.joints; } + + std::size_t linkCount() const { return _body.links.size(); } + std::size_t jointCount() const { return _body.joints.size(); } + + std::string findRootLink() const; + + bool hasLinkName(const std::string& linkName) const { return _linkIndex.find(linkName) != _linkIndex.end(); } + const std::string& rigidbodyName() const { return _body.name; } + bool hasRigidBody() const { return _hasRigidBody; } + + void setGravity(double g); + const double getGravity() const { return _gravity; } + + void setNaturalFrequency(double wn) { _wn = wn; } + double getNaturalFrequency() const { return _wn; } + void resetNaturalFrequencyToTarget() { for (auto& joint : _body.joints) { joint.wn_target = _wn; } } + + void setDampingRatio(double zeta) { _zeta = zeta; } + double getDampingRatio() const { return _zeta; } + void resetDampingRatioToTarget() { + for (auto& joint : _body.joints) { joint.zeta_target = _zeta; } + } + + // Get pointer to this Systemsystem + const Systemsystem& getRigidBody() const { return *this; } + + // ---- Joint State Methods --- + + void computeRigidBodyKinematics(std::vector& world); + + bool tryGetJointAngleRad(const std::string& childLink, double& outAngle) const; + bool trySetJointAngleRad(const std::string& childLink, double angleRad); + + bool tryGetJointOmegaRad(const std::string& childLink, double& outOmega) const; + bool trySetJointOmegaRad(const std::string& childLink, double omegaRad); + bool injectJointOmegaRad(const std::string& childLink, double omega); + + bool tryGetJointTargetRad(const std::string& childLink, double& outTargetRad) const; + bool trySetJointTargetRad(const std::string& childLink, double targetRad); + + bool tryGetJointOmegaMaxRad(const std::string& childLink, double& maxOmegaRad) const; + bool trySetJointOmegaMaxRad(const std::string& childLink, double maxOmegaRad); + + bool tryAddJointTargetRad(const std::string& childLink, double deltaRad); + + bool isJointAtTargetRad(const std::string& childLink, double tolRad) const; + bool isJointAtTargetDeg(const std::string& childLink, double tolDeg) const; + + bool isJointNearAngleRad(const std::string& childLink, double targetRad, double tolRad) const; + bool isJointNearAngleDeg(const std::string& childLink, double targetDeg, double tolDeg) const; + + bool trySetJointOmegaRefRad(const std::string& childLink, double omegaRefRad); + bool trySetJointAlphaRefRad(const std::string& childLink, double alphaRefRad); + + bool trySetJointOmegaRefMaxRad(const std::string& childLink, double omegaRefMaxRad); + + bool tryZeroJointRefDerivatives(); + + // --- SIMULATION STEP METHOD --- + + template + RigidBodySnapshot_T takeSnapshot(T simTime) const; + template + void step_AD(double dt, double simTime); + + void step(double dt, double simTime); + void updateTrajectoryInputs(control::TrajectoryManager& traj, double t); + + // --- RIGIDBODY LOADING AND RESET METHODS --- + + void loadRigidBody(const std::string& name); + void resetRigidBody(); + void stopAll(); + + // --- RIGIDBODY LINK AND ROOT POSE METHODS --- + + bool setLinkRotation(const std::string& childLinkName, double angleDeg); + mathlib::Mat4 setRoot(const mathlib::Vec3& pos, const mathlib::Quat& rot); + void setRootPose(const mathlib::Vec3& pos, const mathlib::Quat& rot); + void setRootHome(const mathlib::Vec3& pos, const mathlib::Quat& rot); + + bool setDefaultPoseDeg(); + void setCurrentJointIndex(int index) { _currentJointIndex = index; } + + // --- GET AND SET INTEGRATION METHOD --- + + integration::eIntegrationMethod getIntegrationMethod() const { return _curIntMethod; } + std::string getIntegratorName() const { return _integrator->IntegratorName(_curIntMethod); } + void setStandardIntegrator(integration::eIntegrationMethod m); + + integration::eAutoDiffIntegrationMethod AD_IntegrationMethod() const { return _curIntMethod_AD; } + std::string AD_integratorName() const { return _AD_integrator->IntegratorName(_curIntMethod_AD); } + void setADIntegrator(integration::eAutoDiffIntegrationMethod m); + + integration::IntegrationService* getIntegrator(); + const integration::IntegrationService* getIntegrator() const; + + integration::DifferentiableIntegrator* getADIntegrator(); + const integration::DifferentiableIntegrator* getADIntegrator() const; + + bool autoDiffEnabled() const { return _useAutoDiff; } + void enableAutoDiff(bool enable) { _useAutoDiff = enable; } + + std::shared_ptr runtimeIntegratorState(); + std::shared_ptr runtimeIntegratorState() const; + + void setRefBuffer(systems::TrajRefBuffer* buf) { _refBuffer = buf; } + void setLogBuffer(systems::JointLogBuffer* buf) { _logBuffer = buf; } + void setRole(eRole role) { _role = role; } + + // Setter and getter the torque mode for the rigidbody system + void setTorqueMode(eTorqueMode mode); + eTorqueMode getTorqueMode() const { return _body.torqueMode; } + + // Swap for the current log buffer, returning a ptr to new active buffer + std::unique_ptr 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 buildLinkIndex(); + void buildSpatialModel(); + + template + SystemstepResult_T step_impl( + const mathlib::VecX_T& x, + Scalar dt, Scalar t, IntegratorT& integrator, + DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + ); + + template + void postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& scratch, const RigidBodyStepResult_T& result); + + std::unique_ptr _kinematics; + std::unique_ptr _dynamics; + + std::unique_ptr _integrator; + integration::eIntegrationMethod _curIntMethod{}; + + std::unique_ptr _AD_integrator; + integration::eAutoDiffIntegrationMethod _curIntMethod_AD{}; + + eRole _role = eRole::Simulation; + + double _wn = 0.0; // configurable natural frequency for PD control (rad/s) + double _zeta = 0.0; // configurable damping ratio for PD control (unitless) + + bool _useAutoDiff = false; + + // Compute the forward drive (velocity) of the rigidbody's root link based on the current state and rigidbody configuration + double computeForwardDrive() const; + // Integrate the floating base translation based on the current state and rigidbody configuration + void integrateBaseTranslation(double dt); + // Integrate the floating base rotation (yaw-only for now) based on the current state and rigidbody configuration + void updateBaseRootPose(); + + // State packing and unpacking + mathlib::VecX packState() const; + void unpackState(const mathlib::VecX& x); + + template + void unpackState(const mathlib::VecX_T& x); + + // State packing and unpacking using a DualNumber vector. + mathlib::VecX_T> packState_AD() const; + void unpackState_AD(const mathlib::VecX_T>& x); + + // Reference state packing and unpacking + mathlib::VecX packRefState() const; + void unpackRefState(const mathlib::VecX& xr); + + // Enforce joint limits after integration + void enforceJointLimits(Joint& j); + + // Simulation time + double _simTime = 0.0; + + // RigidBody model, and rigidbody mode + RigidBodyModel _body; + eTorqueMode _torqueMode = _body.torqueMode; + + SpatialModel _spatialModel; + RigidBodyConstModel _constModel; + + DynamicsScratch _dynScratch; + DynamicsResult _dynResult; + + DynamicsScratch> _dynScratch_AD; + DynamicsResult> _dynResult_AD; + + // World to rigidbody base transform (meters) + std::vector _worldTransforms; + + // Flags and precomputed data + bool _hasBody = false; + mathlib::Mat4 _root_pose = Mat4::Identity(); + mathlib::Mat4 _root_home = Mat4::Identity(); + mathlib::VecX _q_home = mathlib::VecX(); // home/reset joint angles (radians) + bool _home_valid = false; // is home position valid + + // Index maps for quick lookup of links and joints by name + std::unordered_map _link_idx; + std::unordered_map _joint_idx; + // List of joint indices that correspond to the rigidbody's degrees of freedom (excluding fixed joints) + std::vector _dofJointIndices; + + std::string _loadedName; + int _currentJointIndex = -1; + + // Reference state + mathlib::VecX _xRef; + bool _refInit = false; + bool _isReference = false; + + // precomputed clamp lookup tables + mutable std::vector _clampTheta; + mutable std::vector _clampOmega; + + // Gravity acceleration (m/s^2) + double _gravity = 0.0; + + // FLoating base state + bool _baseIsFree = false; + + // Linear + mathlib::Vec3 _basePos{ 0,0,0 }; + mathlib::Vec3 _baseVel{ 0,0,0 }; + mathlib::Vec3 _baseAcc{ 0,0,0 }; + + // Angular (yaw-only for now, extend later) + double _baseYaw = 0.0; + double _baseYawRate = 0.0; + double _baseYawAcc = 0.0; + + // Tunables + double _baseMass = 62.0; // kg (H1 ~60–65) + double _baseLinearDamping = 6.0; // Ns/m + double _baseYawDamping = 2.0; // Nms/rad + double _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 Systemsystem) + systems::JointLogBuffer* _logBuffer = nullptr; + systems::TrajRefBuffer* _refBuffer = nullptr; + + }; +} // namespace rigidbody +#include "SystemsystemStep.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl new file mode 100644 index 00000000..e924dde9 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -0,0 +1,255 @@ +/* + * File: Systems/RigidBodySystemStep.inl + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +namespace systems { + template + T RigidBodySystem::clampJointAngle_T(const Joint& joint, T angleRad) { + if (joint.limits.continuous) { return mathlib::wrapRad(angleRad); } + else { return std::clamp(angleRad, T(joint.limits.minAngle), T(joint.limits.maxAngle)); } + } + + // Method to take a snapshot of the current rigidbody state + template + RigidBodySnapshot_T RigidBodySystem::takeSnapshot(T simTime) const { + SystemsimSnapshot_T snap; + snap.model = &_constModel; + const size_t n = (size_t)_body.joints.size(); + + snap.q.resize(n); + snap.qd.resize(n); + + snap.q_ref.resize(n); + snap.qd_ref.resize(n); + snap.qdd_ref.resize(n); + + for (size_t i = 0; i < n; ++i) { + const auto& j = _body.joints[i]; + + snap.q[i] = j.q; + snap.qd[i] = j.qd; + + snap.q_ref[i] = j.q_ref; + snap.qd_ref[i] = j.qd_ref; + snap.qdd_ref[i] = j.qdd_ref; + } + + snap.root_pose = _root_pose.template cast(); + snap.baseIsFree = _baseIsFree; + snap.lastBaseForwardForce = T(_lastBaseForwardForce); + snap.gravity = T(_gravity); + + snap.torqueMode = _body.torqueMode; + + snap.dt = T(_dynamics->dt()); + snap.simTime = simTime; + + return snap; + } + + template + RigidBodyStepResult_T RigidBodySystem::step_impl( + const mathlib::VecX_T& x, + Scalar dt, Scalar t, IntegratorT& integrator, + DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + ) { + SystemstepResult_T result; + result.snap = takeSnapshot(t); + auto& snap = result.snap; + const size_t n = snap.model->joints.size(); + + Eigen::Map> q(x.data(), n); + Eigen::Map> qd(x.data() + n, n); + + mathlib::VecX_T qdd(n); + for (size_t i = 0; i < n; ++i) { qdd[i] = _body.joints[i].qdd_ref; } + + std::vector> T_start(snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*snap.model, x, T_start); + std::vector> jointWorldPoses_start = _kinematics->calcJointWorldPoses(T_start, *snap.model); + + SpatialModel spatialModel = _spatialModel.template cast(); + auto& dynScratch = dynamicScratch; + auto& dynResult = dynamicResult; + + SpatialDynamics::computeSpatialKinematicsAndBias( + spatialModel, + q, qd, + dynScratch.spatial.Xup, + dynScratch.spatial.v, dynScratch.spatial.c + ); + + // CRBA only for controller inertia scaling + mathlib::MatX_T M_start = SpatialDynamics::CRBA( + spatialModel, + dynScratch.spatial.Xup, + dynScratch + ); + + // Cache frozen joint gains for this step + mathlib::VecX_T kp_frozen(n), kd_frozen(n); + for (size_t i = 0; i < n; ++i) { + const auto& joint = snap.model->joints[i]; + //LOG_INFO("Snap model joint name = %s", joint.name.c_str()); + if (joint.type == eJointType::FIXED) { continue; } + + dynScratch.dense.I_eff_controller[i] = mathlib::max(M_start(i, i), Scalar(1e-6)); + const Scalar I_eff = dynScratch.dense.I_eff_controller[i]; + + kp_frozen[i] = I_eff * joint.wn_target * joint.wn_target; + kd_frozen[i] = Scalar(2) * joint.zeta_target * I_eff * joint.wn_target; + } + + // Compute RNEA torques for feedforward control + mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( + spatialModel, + q, qd, qdd, + dynScratch + ); // [Nm] + result.tau_rnea = tau_rnea; + //LOG_INFO_ONCE("tau_rnea size = %d", (double)tau_rnea.size()); + + // Define the derivative function for integration, capturing necessary variables by reference + auto f_deriv = [&, kp_frozen, kd_frozen](auto t, const auto& xIn) { + return _dynamics->derivative_spatial( + spatialModel, + t, xIn, + snap, + dynScratch, dynResult + ); + }; + // Define the Jacobian function for integration, capturing necessary variables by reference + auto f_J = [&, kp_frozen, kd_frozen](const mathlib::VecX_T& xIn, mathlib::MatX_T& J_out) { + _dynamics->jacobian_spatial( + spatialModel, + xIn, snap, + kp_frozen, kd_frozen, + J_out, dynScratch + ); + }; + + if (!x.allFinite()) { LOG_ERROR("[step_impl] input state already non-finite"); } + + if constexpr (std::is_same_v, integration::IntegrationService>) { + mathlib::VecX x_real = x.template cast(); + auto step = integrator.step(_curIntMethod, x_real, static_cast(t), static_cast(dt), f_deriv, f_J); + result.stepOut.x_next = step.x_next.template cast(); + result.stepOut.dt_taken = step.dt_taken; + result.stepOut.dt_sug = step.dt_sug; + } + else if constexpr (std::is_same_v, integration::DifferentiableIntegrator>) { + result.stepOut = integrator.step(_curIntMethod_AD, x, t, dt, f_deriv); + } + + for (int i = 0; i < result.stepOut.x_next.size(); ++i) { + const auto v = mathlib::real(result.stepOut.x_next[i]); + if (std::isnan(v) || std::isinf(v)) { LOG_ERROR("Non-finite x_next[%d] = %f", i, (double)v); } // TODO add Scalar isnan and isinf checks to mathlib and use those instead (need to handle both float and double cases) + } + + result.dynamics = dynResult; + return result; + } + + template + void RigidBodySystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const SystemstepResult_T& result) { + const size_t n = result.snap.model->joints.size(); + + Eigen::Map q_next(x.data(), n); + Eigen::Map qd_next(x.data() + n, n); + + // Enforce joint limits + /*for (auto& j : _body.joints) { enforceJointLimits(j); }*/ + + // Recompute kinematics and dynamics at the new state for logging and control purposes + std::vector T_world(result.snap.model->links.size()); + _kinematics->computeForwardKinematics_fromState(*result.snap.model, x, T_world); + + // Alternative would be just + + // Compute mass matrix at the new state + mathlib::MatX M = dynScratch.dense.M.unaryExpr([](const auto& v) { return mathlib::real(v); }); + + // Extract real parts of relevant variables for logging and control + mathlib::VecX q_real = result.snap.q.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX qd_real = result.snap.qd.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX q_ref_real = result.snap.q_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX qd_ref_real = result.snap.qd_ref.unaryExpr([](const auto& v) { return mathlib::real(v); }); + mathlib::VecX tau_rnea_real = result.tau_rnea.unaryExpr([](const auto& v) { return mathlib::real(v); }); + + // Compute system kinetic energy: E_kin = 0.5 * qd^T * M(q) * qd + double sys_KE = 0.5 * qd_real.transpose() * M * qd_real; // [J], kinetic energy of the rigidbody at configuration q and velocity qd + + // Compute system potential energy at configuration q (relative to gravity) + double sys_PE = 0.0; + double g = _dynamics->getGravity(); + + for (size_t k = 0; k < _body.links.size(); ++k) { + const Link& link = _body.links[k]; + const double m = link.inertial.mass; + if (m <= 0.0) { continue; } + Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); + sys_PE += m * g * com_world.z(); + } + + const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system + + // Log metrics to buffer if logging is enabled + systems::JointLogBuffer* buf = nullptr; + if (_useInternalLogging) { int idx = _activeLogBufIdx.load(std::memory_order_acquire); buf = &_logBuffers[idx]; } + else { buf = _logBuffer; } + + if (buf) { + auto dynResult = result.dynamics; + for (size_t i = 0; i < n; ++i) { + const Joint& j = _body.joints[i]; + + const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); + const double err = q_ref_real[i] - q_real[i]; + const double err_d = qd_ref_real[i] - qd_real[i]; + JointLogBuffer::JointLogEntry e{}; + + e.sim_time = _simTime; + e.dt_taken = mathlib::real(result.stepOut.dt_taken); + e.dt_sug = mathlib::real(result.stepOut.dt_sug); + e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = mathlib::real(dynResult.metrics.qdd[i]); + e.err = err; e.err_d = err_d; + e.I_eff = I_eff; + e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = 0.0; + e.tau_sat = mathlib::real(dynResult.metrics.tau_sat[i]); + e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; + e.clamp_theta = mathlib::real(_clampTheta[i]); e.clamp_omega = mathlib::real(_clampOmega[i]); + e.sat_flag = mathlib::real(dynResult.metrics.sat_flag[i]); e.joint_index = (int)i; + buf->push_entry(e); + } + } + } + + template + void RigidBodySystem::step_AD(double dt, double simTime) { + if (!hasRigidBody()) { return; } + using Dual = mathlib::DualNumber_T; + _simTime = simTime; + const size_t n = _body.joints.size(); + mathlib::VecX_T x = packState_AD(); + + assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit + for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } + + auto result = step_impl(x, Dual(dt), Dual(simTime), *_AD_integrator, _dynScratch_AD, _dynResult_AD); + unpackState_AD(result.stepOut.x_next); + _dynamics->setDt(result.stepOut.dt_taken); + + auto x_real = result.stepOut.x_next.unaryExpr([](const auto& v) { return mathlib::real(v); }); + postStepUpdate(x_real, _dynScratch_AD, result); + + // Update base pose if free-floating + if (_baseIsFree) { + integrateBaseTranslation(dt); + updateBaseRootPose(); + } + // Update kinematics + computeRigidBodyKinematics(_worldTransforms); + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h new file mode 100644 index 00000000..7e3e17ea --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h @@ -0,0 +1,36 @@ +/* + * File: Systems/SpatialModel.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +#include +#include +#include "Systems/RigidBodyModel.h" + +namespace systems { + // Spatial joint struct + template + struct SpatialJoint { + int parent = -1; + + eJointType type = eJointType::FIXED; + + mathlib::SpatialMat_T Xtree; + mathlib::SpatialMat_T inertia; + mathlib::SpatialVec_T S; + + std::string name; + }; + + // Spatial model struct + template + struct SpatialModel { + std::vector> joints; + std::unordered_map linkNameToIndex; + + template + SpatialModel cast() const; + }; +} // namespace systems +#include "SpatialModelCast.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl b/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl new file mode 100644 index 00000000..eded7fc9 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl @@ -0,0 +1,25 @@ +/* + * File: Systems/SpatialModelCast.inl + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once + +namespace systems { + template + template + SpatialModel SpatialModel::cast() const { + SpatialModel out; + out.joints.resize(joints.size()); + for (size_t i = 0; i < joints.size(); ++i) { + const auto& j = joints[i]; + auto& out_j = out.joints[i]; + out_j.parent = j.parent; + out_j.type = j.type; + out_j.Xtree = j.Xtree.template cast(); + out_j.inertia = j.inertia.template cast(); + out_j.S = j.S.template cast(); + out_j.name = j.name; + } + return out; + } +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/TrajectoryManager.h b/DSFE_App/DSFE_Core/include/Systems/TrajectoryManager.h new file mode 100644 index 00000000..a3bdcf5b --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/TrajectoryManager.h @@ -0,0 +1,39 @@ +/* + * File: Systems/TrajectoryManager.h + * Created by: Joss Salton, 26-07-2026 + */ +#pragma once +#include "EngineCore.h" +#include +#include + +namespace systems { class RigidBodySystem; } + +namespace control { + class DSFE_API TrajectoryManager { + public: + TrajectoryManager() = default; + ~TrajectoryManager() = default; + // non-copyable + TrajectoryManager(const TrajectoryManager&) = delete; + TrajectoryManager& operator=(const TrajectoryManager&) = delete; + // movable is fine + TrajectoryManager(TrajectoryManager&&) noexcept = default; + TrajectoryManager& operator=(TrajectoryManager&&) noexcept = default; + + void clear(const std::string& link); + void clearAll(); + + bool empty() const { return _active.empty(); } + bool tryEval(const std::string& link, double t, control::TrajState& out) const; + bool hasActive(const std::string& link) const; + + void set(const std::string& link, std::unique_ptr traj); + void apply(systems::RigidBodySystem& sys, double t); + + std::size_t activeCount() const { return _active.size(); } + + private: + std::unordered_map> _active; + }; +} // namespace control \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp b/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp index 7824a98f..54a332a2 100644 --- a/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp +++ b/DSFE_App/DSFE_Core/src/Analysis/Telemetry.cpp @@ -1,21 +1,24 @@ +/* + * File: Analysis/Telemetry.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -// File: Telemetry.cpp -// GitHub: SaltyJoss + #include "Analysis/Telemetry.h" -#include "Robots/RobotSystem.h" -#include "Robots/TrajectoryManager.h" +#include "Systems/RigidBodySystem.h" +#include "Systems/TrajectoryManager.h" namespace diagnostics { // Record telemetry data at time t - void TelemetryRecorder::record(double t, const robots::RobotSystem& robotSys, const control::TrajectoryManager* trajOpt, eTelemetryLevel /*level*/) { - const int n = (int)robotSys.getRobot().joints().size(); + void TelemetryRecorder::record(double t, const systems::RigidBodySystem& robotSys, const control::TrajectoryManager* trajOpt, eTelemetryLevel /*level*/) { + const int n = (int)robotSys.getRigidBody().joints().size(); // Begin write TelemetrySample& s = ring.beginWrite(); s.timeSec = t; // Resize joint vector (if needed) - const auto& joints = robotSys.getRobot().joints(); + const auto& joints = robotSys.getRigidBody().joints(); s.j.resize(joints.size()); // Accumulators for error statistics @@ -28,7 +31,7 @@ namespace diagnostics { // Collect telemetry for each joint for (int i = 0; i < n; ++i) { - const auto& j = robotSys.getRobot().joints()[i]; + const auto& j = robotSys.getRigidBody().joints()[i]; JointTelemetry jt; diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Command.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Command.cpp index 24892639..66f27a9f 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Command.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Command.cpp @@ -1,25 +1,20 @@ +/* + * File: DSL/Command.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -// File: Command.cpp -#include "Interpreter/Command.h" + +#include "DSL/Command.h" namespace commands { // Execute command - void Command::execute() { - // No base implementation - } - CmdResult Command::update(CommandContext& cntx, double dt) { - return CmdResult{ CmdState::NotStarted, {}, "" }; - } + void Command::execute() {} + // Update command with time step dt + CmdResult Command::update(CommandContext& cntx, double dt) { return CmdResult{ CmdState::NotStarted, {}, "" }; } // Mark the command as failed with a message - void Command::markFailed(const std::string& message) { - // Base implementation (if any) can go here - } + void Command::markFailed(const std::string& message) {} // Mark the command as completed - void Command::markCompleted() { - // Base implementation (if any) can go here - } + void Command::markCompleted() {} // Check if the command has started - bool Command::hasStarted() const { - return false; // Base implementation (if any) can go here - } + bool Command::hasStarted() const { return false; } } // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp b/DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp index 4c3ecda8..5676e936 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp @@ -1,9 +1,12 @@ -// DSFE_Core CommandContext.cpp +/* + * File: DSL/CommandContext.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/CommandContext.h" +#include "DSL/CommandContext.h" #include "Scene/SimulationCore.h" -#include "Robots/RobotSystem.h" +#include "Systems/RigidBodySystem.h" #include "EngineLib/LogMacros.h" @@ -44,12 +47,12 @@ namespace commands { return OpResult::Success(true); } - // Loads a robot by name and updates the context with the new robot system + // Loads a rigidBody by name and updates the context with the new rigidBody system OpResult CommandContext::loadMultibody(const std::string& bodyName) { if (!_core) { return OpResult::Failure("Simulation manager is null."); } - if (bodyName.empty()) return OpResult::Failure("Robot name is empty."); - _core->loadRobot(bodyName); // only load robot for now, as multibody is not finished - auto& rs = _core->robotSystem(); + if (bodyName.empty()) return OpResult::Failure("RigidBody name is empty."); + _core->loadRigidBody(bodyName); // only load rigidBody for now, as multibody is not finished + auto& rs = _core->rigidBodySystem(); return OpResult::Success(true); } @@ -62,7 +65,7 @@ namespace commands { utils::OpResult CommandContext::setJointOmega(const std::string& childLink, double omegaDegPerSec) { double omegaRadPerSec = degToRad(omegaDegPerSec); - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); rs.trySetJointOmegaRad(childLink, omegaRadPerSec); return OpResult::Success(true); } @@ -70,7 +73,7 @@ namespace commands { utils::OpResult CommandContext::stopJointOmega(const std::string& childLink) { return setJointOmega(childLink, 0.0); } core::ISimulationCore* CommandContext::Core() const { return _core; } - robots::RobotSystem& CommandContext::Robot() const { return _core->robotSystem(); } + systems::RigidBodySystem& CommandContext::RigidBody() const { return _core->rigidBodySystem(); } Vec3 CommandContext::normaliseDirection(const Vec3& dir) const { const double x = dir.x(); @@ -86,7 +89,7 @@ namespace commands { } double CommandContext::getJointAngleRad(const std::string& link) const { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); double a = 0.0f; if (rs.tryGetJointAngleRad(link, a)) { return (double)a; } else { LOG_WARN("Failed to get joint angle for link '%s'", link.c_str()); } @@ -96,7 +99,7 @@ namespace commands { // --- JOINT ANGLE METHODS --- utils::OpResult CommandContext::setJointTargetRad(const std::string& link, double thetaTargetRad) { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); if (!rs.trySetJointTargetRad(link, thetaTargetRad)) { return OpResult::Failure("Failed to set joint target -> Joint not found or target rejected."); } @@ -104,7 +107,7 @@ namespace commands { } utils::OpResult CommandContext::setJointTargetDeltaRad(const std::string& link, double deltaRad) { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); double refRad = 0.0f; if (!rs.tryGetJointTargetRad(link, refRad)) { return OpResult::Failure("Failed to get joint angle -> Joint not found."); @@ -114,7 +117,7 @@ namespace commands { } utils::OpResult CommandContext::setJointMaxOmegaRad(const std::string& link, double maxqd) { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); if (maxqd <= 0.0) { return OpResult::Failure("Max omega must be positive."); } if (!rs.trySetJointOmegaMaxRad(link, maxqd)) { return OpResult::Failure("Failed to set joint max omega -> Joint not found or invalid value."); @@ -124,7 +127,7 @@ namespace commands { // Sets the reference angular velocity for a joint (rad/s) utils::OpResult CommandContext::setJointOmegaRefRad(const std::string& link, double qd_ref) { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); if (!rs.trySetJointOmegaRefRad(link, qd_ref)) { return OpResult::Failure("Failed to set joint omega ref -> Joint not found or invalid value."); } @@ -133,7 +136,7 @@ namespace commands { // Sets the reference angular acceleration for a joint (rad/s^2) utils::OpResult CommandContext::setJointAlphaRefRad(const std::string& link, double qdd_ref) { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); if (!rs.trySetJointAlphaRefRad(link, qdd_ref)) { return OpResult::Failure("Failed to set joint alpha ref -> Joint not found or invalid value."); } @@ -143,7 +146,7 @@ namespace commands { utils::OpResult CommandContext::updateJointRotateTo(double /*dt*/) { if (!_jnt.active) { return OpResult::Success(true); } - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); const bool done = rs.isJointAtTargetRad(_jnt.link, _jnt.epsAngle); if (done) { _jnt.active = false; return OpResult::Success(true); } @@ -153,7 +156,7 @@ namespace commands { } utils::OpResult CommandContext::beginJointRotateTo(const std::string& link, double maxOmegaDegPerSec, double angleDeg) { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); if (link.empty()) return OpResult::Failure("beginJointRotateTo -> empty link."); const double current = getJointAngleRad(link); @@ -174,8 +177,7 @@ namespace commands { _jnt.wrapShortest = true; _jnt.epsAngle = degToRad(0.5); // 0.5 degrees tolerance - SIM_ROTATE("Begin joint rotate to link='%s' current=%.3f rad target=%.3f rad maxOmega=%.3f rad/s", - link.c_str(), current, target, maxOmega); + SIM_ROTATE("Begin joint rotate to link='%s' current=%.3f rad target=%.3f rad maxOmega=%.3f rad/s", link.c_str(), current, target, maxOmega); return OpResult::Success(false); } @@ -357,9 +359,9 @@ namespace commands { // --- READ-ONLY ACCESSORS --- - // Checks if the current context has a valid robot and if the specified link index is within bounds + // Checks if the current context has a valid rigidBody and if the specified link index is within bounds bool CommandContext::hasLink(std::size_t linkIndex) const { - auto& rs = _core->robotSystem(); + auto& rs = _core->rigidBodySystem(); return linkIndex < rs.links().size(); } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp b/DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp index ef7964fe..6c2b901f 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp @@ -1,7 +1,10 @@ -// DSFE_Core CommandFactory.cpp +/* + * File: DSL/CommandFactory.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/CommandFactory.h" +#include "DSL/CommandFactory.h" namespace commands { CommandFactory& CommandFactory::Instance() { diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp index 9d79d3d2..36feabf7 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp @@ -1,8 +1,11 @@ -// DSFE_Core LoadCmd.cpp +/* + * File: DSL/LoadCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/LoadCmd.h" -#include "Interpreter/Utils.h" +#include "DSL/Commands/LoadCmd.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" @@ -16,22 +19,14 @@ namespace commands { // constructor LoadCmd::LoadCmd(const std::string& id, const std::vector& tokens) { - if (id == "robot") { _target.type = LoadTargetType::MultiBody; } + if (id == "rigidbody") { _target.type = LoadTargetType::rigidBody ; } else { std::string errMsg = "Invalid load(,...) identifier -> " + id; markFailed(errMsg); D_FAIL(errMsg.c_str()); return; } - - if (tokens.empty()) { - std::string errMsg = "load() command requires a path argument."; - markFailed(errMsg); - D_FAIL(errMsg.c_str()); - return; - } - - if (!tokens.empty()) { _path = tokens[0]; _target.path = tokens[0]; } + _path = tokens[0]; _target.path = tokens[0]; } // Execute the command @@ -44,8 +39,7 @@ namespace commands { } switch (_target.type) { - case LoadTargetType::SingleBody: _cntx->loadSingleBody(_target.path); break; - case LoadTargetType::MultiBody: _cntx->loadMultibody(_target.path); break; + case LoadTargetType::RigidBody: _cntx->loadRigidBody(_target.path); break; default: { std::string errMsg = "Invalid load target type."; @@ -61,7 +55,12 @@ namespace commands { // Factory function to create a LoadCmd from arguments std::unique_ptr CreateLoadCmd(const std::string& id, const std::vector& tokens) { - if (tokens.empty()) return nullptr; + if (tokens.empty()) { + std::string errMsg = "load() command requires a path argument."; + D_FAIL(errMsg.c_str()); + return nullptr; + } + id = std::tolower(id); return std::make_unique(id, tokens); } } // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp index e039bb23..cddf4066 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp @@ -1,8 +1,11 @@ -// DSFE_Core ParallelGroupCmd.cpp +/* + * File: DSL/ParallelGroupCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/ParallelGroupCmd.h" -#include "Robots/RobotSystem.h" +#include "DSL/Commands/ParallelGroupCmd.h" +#include "Systems/RigidBodySystem.h" #include @@ -43,7 +46,7 @@ namespace commands { D_WARN("parallel timed out after %.3fs", _elapsed); // “soft finish” - cntx.Robot().stopAll(); + cntx.RigidBody().stopAll(); // treats timeout as Executed - ill keep for now, may explore different timeout policies later _result = { CmdState::Executed, {}, "parallel: timeout (soft-finish)" }; diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp index d907b40b..59a97550 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp @@ -1,8 +1,11 @@ -// DSFE_Core RotateToCmd.cpp +/* + * File: DSL/RotateByCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/RotateByCmd.h" -#include "Interpreter/Utils.h" +#include "DSL/Commands/RotateByCmd.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp index 047200c7..149b239e 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp @@ -1,9 +1,12 @@ -// DSFE_Core RotateJointByCmd.cpp +/* + * File: DSL/RotateJointByCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/RotateJointByCmd.h" -#include "Robots/RobotSystem.h" -#include "Interpreter/Utils.h" +#include "DSL/Commands/RotateJointByCmd.h" +#include "Systems/RigidBodySystem.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" @@ -23,8 +26,8 @@ namespace commands { } // Core update loop for rotateJointBy command - program_data::CmdResult RotateJointByCmd::update(CommandContext& cntx, double dt) { - auto& rs = cntx.Robot(); + CmdResult RotateJointByCmd::update(CommandContext& cntx, double dt) { + auto& body = cntx.RigidBody(); // Defensive dt - my research shows I need to avoid giant dt spikes causing weird timing/logic. double maxDt = 1.0 / 60.0; // 1/60s, 60Hz, or 16.67ms if (dt < 0.0) dt = 0.0; @@ -50,7 +53,7 @@ namespace commands { // Get starting angle double theta0 = 0.0f; - if (!rs.tryGetJointAngleRad(_link, theta0)) { + if (!body.tryGetJointAngleRad(_link, theta0)) { markFailed("rotateJointBy: joint not found (angle)."); D_FAIL("rotateJointBy: joint not found (angle) for '%s'", _link.c_str()); return CmdResult{ CmdState::Failed, {}, "rotateJointBy: joint not found (angle)." }; @@ -93,8 +96,8 @@ namespace commands { double theta = 0.0f; double omega = 0.0f; - const bool gotTheta = rs.tryGetJointAngleRad(_link, theta); - const bool gotOmega = rs.tryGetJointOmegaRad(_link, omega); + const bool gotTheta = body.tryGetJointAngleRad(_link, theta); + const bool gotOmega = body.tryGetJointOmegaRad(_link, omega); if (!gotTheta) { markFailed("rotateJointBy: joint not found (angle)."); @@ -115,7 +118,7 @@ namespace commands { _noProgressT += dt; } - const bool posOK = rs.isJointAtTargetRad(_link, tolPosRad); // consider at target if within position tolerance + const bool posOK = body.isJointAtTargetRad(_link, tolPosRad); // consider at target if within position tolerance const bool omegaOK = (absOm <= tolOmegaRad); // consider stopped if omega is small enough if (posOK && omegaOK) { diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp index 3719b0c2..a16508ba 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp @@ -1,9 +1,12 @@ -// DSFE_Core RotateJointToCmd.cpp +/* + * File: DSL/RotateJointToCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/RotateJointToCmd.h" -#include "Robots/RobotSystem.h" -#include "Interpreter/Utils.h" +#include "DSL/Commands/RotateJointToCmd.h" +#include "Systems/RigidBodySystem.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" @@ -24,8 +27,8 @@ namespace commands { } // Core update loop for rotateJointTo command - program_data::CmdResult RotateJointToCmd::update(CommandContext& cntx, double dt) { - auto& rs = cntx.Robot(); + CmdResult RotateJointToCmd::update(CommandContext& cntx, double dt) { + auto& body = cntx.RigidBody(); // Defensive dt - my research shows I need to avoid giant dt spikes causing weird timing/logic. double maxDt = 1.0 / 60.0; // 1/60s, 60Hz, or 16.67ms if (dt < 0.0) dt = 0.0; @@ -54,7 +57,7 @@ namespace commands { // Compute an informed timeout double theta0 = 0.0f; - if (!rs.tryGetJointAngleRad(_link, theta0)) { + if (!body.tryGetJointAngleRad(_link, theta0)) { markFailed("rotateJointTo: joint not found (angle)."); D_FAIL("rotateJointTo: joint not found (angle) for '%s'", _link.c_str()); return { CmdState::Failed, {}, "rotateJointTo: joint not found (angle)." }; @@ -99,8 +102,8 @@ namespace commands { double theta = 0.0f; double omega = 0.0f; - const bool gotTheta = rs.tryGetJointAngleRad(_link, theta); - const bool gotOmega = rs.tryGetJointOmegaRad(_link, omega); + const bool gotTheta = body.tryGetJointAngleRad(_link, theta); + const bool gotOmega = body.tryGetJointOmegaRad(_link, omega); if (!gotTheta) { markFailed("rotateJointTo: joint not found (angle)."); @@ -120,7 +123,7 @@ namespace commands { _noProgressT += dt; } - const bool posOK = rs.isJointAtTargetRad(_link, (double)tolPosRad); + const bool posOK = body.isJointAtTargetRad(_link, (double)tolPosRad); const bool omegaOK = absOm <= tolOmegaRad; if (posOK && omegaOK) { diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp index 7a30acbb..bc2c0f5d 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp @@ -1,8 +1,11 @@ -// DSFE_Core RotateToCmd.cpp +/* + * File: DSL/RotateToCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/RotateToCmd.h" -#include "Interpreter/Utils.h" +#include "DSL/Commands/RotateToCmd.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp index 10f446c0..60a012ab 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp @@ -1,7 +1,10 @@ -// DSFE_Core SelectCmd.cpp +/* + * File: DSL/SelectCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/SelectCmd.h" +#include "DSL/Commands/SelectCmd.h" #include "EngineLib/LogMacros.h" diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp index 523fa20e..502d6617 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp @@ -1,9 +1,12 @@ -// DSFE_Core SetCmd.cpp +/* + * File: DSL/SetCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/SetCmd.h" -#include "Interpreter/IStoredProgram.h" -#include "Interpreter/Utils.h" +#include "DSL/Commands/SetCmd.h" +#include "DSL/IStoredProgram.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" @@ -18,16 +21,23 @@ namespace commands { // Helper function to parse the integration method static IntegratorMethod parseMethod(const std::string& s) { + // Explicit variants if (s == "euler") return IntegratorMethod::Euler; if (s == "midpoint") return IntegratorMethod::Midpoint; if (s == "heun") return IntegratorMethod::Heun; if (s == "ralston") return IntegratorMethod::Ralston; if (s == "rk4") return IntegratorMethod::RK4; if (s == "rk45") return IntegratorMethod::RK45; + // Implicit variants if (s == "implicit_euler") return IntegratorMethod::ImplicitEuler; if (s == "implicit_midpoint") return IntegratorMethod::ImplicitMidpoint; if (s == "glrk2") return IntegratorMethod::GLRK2; if (s == "glrk3") return IntegratorMethod::GLRK3; + // Automatic Differentiation (AD) variants + if (s == "ad_implicit_euler") return IntegratorMethod::AD_ImplicitEuler; + if (s == "ad_implicit_midpoint") return IntegratorMethod::AD_ImplicitMidpoint; + if (s == "ad_glrk2") return IntegratorMethod::AD_GLRK2; + if (s == "ad_glrk3") return IntegratorMethod::AD_GLRK3; D_WARN("Integration Method not recognised -> %s ~ Defaulted to \"Fourth-Order Runge Kutta\"", s.c_str()); return IntegratorMethod::RK4; } @@ -40,7 +50,7 @@ namespace commands { if (startsWith(toLower(id), "integrator")) { std::string s = toLower(token); return SetTarget{ SetTargetType::IntegratorMethod, parseMethod(s) }; } if (startsWith(toLower(id), "dt")) { std::string s = token; return SetTarget{ SetTargetType::FixedDt, {}, {}, utils::parseDouble(s) }; } if (startsWith(toLower(id), "gravity")) { std::string s = token; return SetTarget{ SetTargetType::Gravity, {}, {}, {}, utils::parseDouble(s)}; } - if (startsWith(toLower(id), "omega")) { + if (startsWith(toLower(id), "velocity") || startsWith(toLower(id), "omega")) { std::string s = toLower(token); AxisMask m = utils::parseAxisMask(s); @@ -58,7 +68,8 @@ namespace commands { // Constructor SetCmd::SetCmd(const std::string& id, const std::string& tokens) - : _id(id), _tokens(tokens) { + : _id(toLower(id)), _tokens(tokens) + { _result = { CmdState::NotStarted, {}, "" }; } @@ -100,7 +111,6 @@ namespace commands { markCompleted(); return; } - markFailed("Unknown set target: " + _id); } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp index 218dab86..d0de2e15 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp @@ -1,10 +1,13 @@ -// DSFE_Core SetOmegaCmd.cpp +/* + * File: DSL/SetOmegaCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/SetOmegaCmd.h" -#include "Robots/RobotSystem.h" +#include "DSL/Commands/SetOmegaCmd.h" +#include "Systems/RigidBodySystem.h" +#include "DSL/Utils.h" -#include "Interpreter/Utils.h" #include "EngineLib/LogMacros.h" using namespace utils; @@ -26,16 +29,16 @@ namespace commands { } // Updates setOmega command - program_data::CmdResult SetOmegaCmd::update(CommandContext& cntx, double dt) { - auto& rs = cntx.Robot(); - if (!rs.hasLinkName(_link)) { - markFailed("setOmega: link '" + _link + "' not found in robot."); - return { CmdState::Failed, {}, "setOmega failed: link '" + _link + "' not found in robot." }; + CmdResult SetOmegaCmd::update(CommandContext& cntx, double dt) { + auto& body = cntx.RigidBody(); + if (!body.hasLinkName(_link)) { + markFailed("setOmega: link '" + _link + "' not found in body."); + return { CmdState::Failed, {}, "setOmega failed: link '" + _link + "' not found in body." }; } double omegaRad = degToRad(_omega); - if (!rs.injectJointOmegaRad(_link, omegaRad)) { + if (!body.injectJointOmegaRad(_link, omegaRad)) { markFailed("setOmega: failed to set joint omega for link='" + _link + "'."); return { CmdState::Failed, {}, "setOmega failed to set joint omega for link='" + _link + "'." }; } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp index e13a7b87..9de54931 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp @@ -1,8 +1,11 @@ -// DSFE_Core SpinCmd.cpp +/* + * File: DSL/SpinCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/SpinCmd.h" -#include "Interpreter/Utils.h" +#include "DSL/Commands/SpinCmd.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" @@ -22,7 +25,7 @@ namespace commands { } // Update the command - program_data::CmdResult SpinCmd::update(CommandContext& cntx, double dt) { + CmdResult SpinCmd::update(CommandContext& cntx, double dt) { if (!_started) { markFailed("spin() not started."); return CmdResult{ CmdState::Failed, {}, "spin() not started." }; diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp index a223a078..03ed997a 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp @@ -1,7 +1,10 @@ -// DSFE_Core StartCmd.cpp +/* + * File: DSL/StartCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/StartCmd.h" +#include "DSL/Commands/StartCmd.h" #include "EngineLib/LogMacros.h" diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp index 8c2dff9e..f1a42596 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp @@ -1,7 +1,10 @@ -// DSFE_Core StopCmd.cpp +/* + * File: DSL/StopCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/StopCmd.h" +#include "DSL/Commands/StopCmd.h" #include "EngineLib/LogMacros.h" diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp index 53d05047..dd78e317 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp @@ -1,14 +1,16 @@ -// DSFE_Core TrajClearCmd.cpp +/* + * File: DSL/TrajClearCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/TrajClearCmd.h" - -#include "Robots/TrajectoryManager.h" -#include "Robots/RobotSystem.h" +#include "DSL/Commands/TrajClearCmd.h" +#include "Systems/TrajectoryManager.h" +#include "Systems/RigidBodySystem.h" #include "Scene/SimulationCore.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" -#include "Interpreter/Utils.h" namespace commands { // --- Markers --- @@ -35,8 +37,8 @@ namespace commands { trajMgr.clearAll(); // clear all trajectories // Zero qd/qdd refs for all joints so nothing lingers - auto& rs = cntx.Robot(); - rs.tryZeroJointRefDerivatives(); // make sure this exists as a NO-ARG method (see step 4) + auto& body = cntx.RigidBody(); + body.tryZeroJointRefDerivatives(); // make sure this exists as a NO-ARG method (see step 4) SIM_SUCCESS("trajClear(): cleared all trajectories and zeroed ref derivatives"); _done = true; diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp index f3f0c79a..511e6246 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp @@ -1,10 +1,13 @@ -// DSFE_Core TrajSetCmd.cpp +/* + * File: DSL/TrajSetCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/TrajSetCmd.h" +#include "DSL/Commands/TrajSetCmd.h" #include "Scene/SimulationCore.h" -#include "Robots/RobotSystem.h" -#include "Robots/TrajectoryManager.h" +#include "Systems/RigidBodySystem.h" +#include "Systems/TrajectoryManager.h" #include "control/TrapezoidTrajectory.h" #include "control/SinusoidalTrajectory.h" @@ -14,7 +17,7 @@ #include #include -#include "Interpreter/Utils.h" +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" using namespace utils; @@ -54,17 +57,17 @@ namespace commands { } // Updates trajSet command - program_data::CmdResult TrajSetCmd::update(CommandContext& cntx, double dt) { + CmdResult TrajSetCmd::update(CommandContext& cntx, double dt) { auto* core = cntx.Core(); if (!core) { markFailed("trajSet: no SimulationManager in context."); return { CmdState::Failed, {}, "trajSet failed" }; } - auto& robot = cntx.Robot(); + auto& body = cntx.RigidBody(); // Validate joint exists and get current angle as q0. double q0 = 0.0f; - if (!robot.tryGetJointAngleRad(_link, q0)) { + if (!body.tryGetJointAngleRad(_link, q0)) { SIM_FAIL("trajSet: joint not found '%s'", _link.c_str()); markFailed("trajSet: joint not found."); return { CmdState::Failed, {}, "trajSet failed" }; @@ -75,7 +78,7 @@ namespace commands { // Get hardware max omega double wMax_hw = 0.0; - if (!robot.tryGetJointOmegaMaxRad(_link, wMax_hw)) { + if (!body.tryGetJointOmegaMaxRad(_link, wMax_hw)) { D_WARN("trajSet: failed to get joint max omega for link='%s'", _link.c_str()); wMax_hw = std::numeric_limits::infinity(); } @@ -99,7 +102,7 @@ namespace commands { double wMax_est = std::abs(vmax); wMax_est = std::min(wMax_est, (double)wMax_hw); - if (!robot.trySetJointOmegaRefMaxRad(_link, wMax_est)) { + if (!body.trySetJointOmegaRefMaxRad(_link, wMax_est)) { D_WARN("trajSet(TRAP): failed to set joint omega ref max for link='%s'", _link.c_str()); } @@ -149,7 +152,7 @@ namespace commands { double wMax_est = TWO_PI_d * fHz * amp; wMax_est = std::min(wMax_est, wMax_hw); - if (!robot.trySetJointOmegaRefMaxRad(_link, wMax_est)) { + if (!body.trySetJointOmegaRefMaxRad(_link, wMax_est)) { D_WARN("trajSet(SINE): failed to set joint omega ref max for link='%s'", _link.c_str()); } @@ -208,7 +211,7 @@ namespace commands { wMax_est = std::min(wMax_est, wMax_hw); // Set joint omega ref max - if (!robot.trySetJointOmegaRefMaxRad(_link, wMax_est)) { + if (!body.trySetJointOmegaRefMaxRad(_link, wMax_est)) { D_WARN("trajSet(MSINE): failed to set joint omega ref max for link='%s'", _link.c_str()); } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp index b557b663..676432b4 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp @@ -1,7 +1,10 @@ -// DSFE_Core WaitCmd.cpp +/* + * File: DSL/WaitCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Commands/WaitCmd.h" +#include "DSL/Commands/WaitCmd.h" #include "EngineLib/LogMacros.h" @@ -17,7 +20,7 @@ namespace commands { } // Update the command - program_data::CmdResult WaitCmd::update(CommandContext& cntx, double dt) { + CmdResult WaitCmd::update(CommandContext& cntx, double dt) { if (!_started) { markFailed("wait() not started."); return CmdResult{ CmdState::Failed, {}, "wait() not started." }; } _remainingTime -= dt; if (_remainingTime <= 0.0) { markCompleted(); return CmdResult{ CmdState::Executed, {}, "" }; } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp index e5770ba3..4cababd7 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp @@ -1,20 +1,21 @@ -// DSFE_Core Parser.cpp +/* + * File: DSL/Parser.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/Parser.h" +#include "DSL/Parser.h" +#include "DSL/RegisterCommand.h" +#include "DSL/CommandFactory.h" +#include "DSL/Commands/ParallelGroupCmd.h" +#include "DSL/Utils.h" -#include "Interpreter/RegisterCommand.h" -#include "Interpreter/CommandFactory.h" - -#include "Interpreter/Commands/ParallelGroupCmd.h" - -#include "Interpreter/Utils.h" #include "EngineLib/LogMacros.h" using namespace std; using namespace utils; -namespace interpreter { +namespace dsl { // --- Handlers --- // Determine if a command requires an identifier @@ -419,5 +420,5 @@ namespace interpreter { buildProgram(); } -} // namespace interpreter +} // namespace dsl diff --git a/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp b/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp index 47c17d0e..6b2c434c 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp @@ -1,7 +1,10 @@ -// DSFE_Core RegisterCommand.cpp +/* + * File: DSL/RegisterCommand.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/RegisterCommand.h" +#include "DSL/RegisterCommand.h" // Motion commands #include "Interpreter/Commands/SpinCmd.h" diff --git a/DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp b/DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp index b5e37efc..228b0779 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp @@ -1,14 +1,17 @@ +/* + * File: DSL/RunWrapper.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -// File: RunWrapper.cpp -// GitHub: SaltyJoss -#include "Interpreter/RunWrapper.h" + +#include "DSL/RunWrapper.h" #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" extern DSFE_API Debug gLog; -namespace interpreter { +namespace dsl { // Constructor RunWrapper::RunWrapper(Parser* parser, IStoredProgram* program) : _parser(parser), _program(program) { if (_parser == nullptr) { diff --git a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp index 4fc7854c..dd0a5f12 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp @@ -1,13 +1,17 @@ -// DSFE_Core StoredProgram.cpp +/* + * File: DSL/StoredProgram.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/StoredProgram.h" +#include "DSL/StoredProgram.h" + #include "Platform/ISimulationCore.h" -#include "Robots/RobotSystem.h" +#include "Systems/RigidBodySystem.h" #include "EngineLib/LogMacros.h" -namespace interpreter { +namespace dsl { StoredProgram::StoredProgram(core::ISimulationCore* core) : _currentLineNumber(0), PC(0), _core(core), _cntx(core) { } @@ -74,14 +78,14 @@ namespace interpreter { // Stop simulation void StoredProgram::stopSim() { if (_core->isSimRunning()) { _core->stopSimulation(); } - if (_core->hasRobot()) { _cntx.motion().Robot().stopAll(); } + if (_core->hasRigidBody()) { _cntx.motion().RigidBody().stopAll(); } } // Pause program execution void StoredProgram::pause() { if (!_core) { return; } _state = ProgramState::Paused; - if (_core->hasRobot()) { _cntx.motion().Robot().stopAll(); } + if (_core->hasRigidBody()) { _cntx.motion().RigidBody().stopAll(); } } // Wait for simulation to run for dt seconds @@ -148,8 +152,8 @@ namespace interpreter { void StoredProgram::setIntegratorMethod(IntegratorMethod method) { _integratorMethod = method; if (_core) { - if (_core->hasRobot()) { - auto& rs = _core->robotSystem(); + if (_core->hasRigidBody()) { + auto& rs = _core->rigidBodySystem(); if (method == IntegratorMethod::AD_ImplicitEuler || method == IntegratorMethod::AD_ImplicitMidpoint || method == IntegratorMethod::AD_GLRK2 || method == IntegratorMethod::AD_GLRK3) { _core->setADIntegrationMethod(static_cast(method)); } @@ -180,8 +184,8 @@ namespace interpreter { void StoredProgram::setGravity(double gravity) { _gravity = gravity; if (_core) { - if (_core->hasRobot()) { - auto& rs = _core->robotSystem(); + if (_core->hasRigidBody()) { + auto& rs = _core->rigidBodySystem(); rs.setGravity(gravity); } } @@ -189,11 +193,11 @@ namespace interpreter { // Get Gravity double StoredProgram::getGravity() const { if (_core) { - if (_core->hasRobot()) { - const auto& rs = _core->robotSystem(); + if (_core->hasRigidBody()) { + const auto& rs = _core->rigidBodySystem(); return rs.getGravity(); } } return _gravity; } -} // namespace interpreter \ No newline at end of file +} // namespace dsl \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp b/DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp index ed1d066a..a9b0a40f 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp @@ -1,10 +1,13 @@ -// DSFE_Core UIContext.cpp +/* + * File: DSL/UIContext.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Interpreter/UIContext.h" +#include "DSL/UIContext.h" #include "Scene/SimulationCore.h" -#include "Robots/RobotSystem.h" +#include "Systems/RigidBodySystem.h" #include "Platform/DataManager.h" #include "EngineLib/LogMacros.h" @@ -16,7 +19,7 @@ using namespace utils; namespace commands { // Constructor UIContext::UIContext(core::ISimulationCore* core) - : _core(core), _robot(core ? core->robotSystem() : nullptr), + : _core(core), _rigidBody(core ? core->rigidBodySystem() : nullptr), _angularUnits(AngularUnits::DegPerSec) { } @@ -123,14 +126,14 @@ namespace commands { // --- ROBOT LOAD AND CLEAR METHODS --- - // Loads a robot by name and updates the context with the new robot system - OpResult UIContext::loadRobot(const std::string& robotName) { + // Loads a rigidBody by name and updates the context with the new rigidBody system + OpResult UIContext::loadRigidBody(const std::string& rigidBodyName) { if (!_core) { return OpResult::Failure("Simulation manager is null."); } - if (robotName.empty()) return OpResult::Failure("Robot name is empty."); + if (rigidBodyName.empty()) return OpResult::Failure("RigidBody name is empty."); - _core->loadRobot(robotName); - _robot = _core->robotSystem(); - if (!_robot) return OpResult::Failure("Robot system is null after load."); + _core->loadRigidBody(rigidBodyName); + _rigidBody = _core->rigidBodySystem(); + if (!_rigidBody) return OpResult::Failure("RigidBody system is null after load."); return OpResult::Success(true); } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp index 773cf221..b6dfda82 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp +++ b/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp @@ -1,8 +1,9 @@ -// DSFE_Core Utils.cpp +/* + * File: DSL/Utils.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" - -#include "Interpreter/Utils.h" - +#include "DSL/Utils.h" #include "EngineLib/LogMacros.h" using namespace mathlib; @@ -198,19 +199,7 @@ namespace utils { } return mask; } - - //// Helper function to try parsing an ObjectID from a string (e.g. "obj123" or "123") - //bool utils::tryParseObjID(const std::string& s, scene::ObjectID& out) { - // std::string_view v = s; - // if (v.rfind("obj", 0) == 0) v.remove_prefix(3); - - // unsigned id = 0; - // auto res = std::from_chars(v.data(), v.data() + v.size(), id); - // if (res.ec != std::errc{} || res.ptr != v.data() + v.size()) return false; - // out = (scene::ObjectID)id; - // return true; - //} - + // Helper functions to convert between degrees and radians double degToRad(double degrees) { return degrees * ( PI_d / 180.0); } mathlib::Vec3 degToRad(mathlib::Vec3& degrees) { diff --git a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp index b03433a8..3f623047 100644 --- a/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp +++ b/DSFE_App/DSFE_Core/src/Numerics/IntegrationService.cpp @@ -1,6 +1,8 @@ +/* + * File: Numerics/IntegrationService.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -// File: IntegrationService.cpp -// GitHub: SaltyJoss #include "Numerics/IntegrationService.h" using namespace mathlib; diff --git a/DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp b/DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp new file mode 100644 index 00000000..5cadadf6 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp @@ -0,0 +1,13 @@ +/* + * File: Physics/RigidBodyDynamics.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" +#include "Systems/RigidBodyDynamics.h" + +namespace systems { + // Constructor + RigidBodyDynamics::RigidBodyDynamics() + : _kinematics(std::make_unique()) { + } +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp b/DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp new file mode 100644 index 00000000..84200e3c --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp @@ -0,0 +1,14 @@ +/* + * File: Physics/RigidBodyKinematics.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "Systems/RigidBodyKinematics.h" + +using namespace mathlib; +using namespace constants; + +namespace systems { + RigidBodyKinematics::RigidBodyKinematics() {} +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp b/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp index c9dbc7ff..5fa6cbf1 100644 --- a/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp +++ b/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp @@ -1,4 +1,7 @@ -// DSFE_Core DataManager.cpp +/* + * File: Platform/DataManager.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" #include "Platform/DataManager.h" diff --git a/DSFE_App/DSFE_Core/src/Platform/Logger.cpp b/DSFE_App/DSFE_Core/src/Platform/Logger.cpp index cce76316..d9a3192f 100644 --- a/DSFE_App/DSFE_Core/src/Platform/Logger.cpp +++ b/DSFE_App/DSFE_Core/src/Platform/Logger.cpp @@ -1,6 +1,8 @@ +/* + * File: Platform/Logger.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -// File: Logger.cpp -// GitHub: SaltyJoss #include "Platform/Logger.h" DSFE_API Debug gLog; \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Platform/Paths.cpp b/DSFE_App/DSFE_Core/src/Platform/Paths.cpp index 0d546c44..26741459 100644 --- a/DSFE_App/DSFE_Core/src/Platform/Paths.cpp +++ b/DSFE_App/DSFE_Core/src/Platform/Paths.cpp @@ -1,4 +1,7 @@ -// DSFE_Core Paths.cpp +/* + * File: Platform/Paths.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" #include "Platform/Paths.h" // Windows-specific includes for known folder paths diff --git a/DSFE_App/DSFE_Core/src/Platform/StudyRunner.cpp b/DSFE_App/DSFE_Core/src/Platform/StudyRunner.cpp index 04d8d63d..ab608751 100644 --- a/DSFE_App/DSFE_Core/src/Platform/StudyRunner.cpp +++ b/DSFE_App/DSFE_Core/src/Platform/StudyRunner.cpp @@ -1,8 +1,11 @@ -// DSFE_Core StudyRunner.cpp +/* + * File: Platform/StudyRunner.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" #include "Platform/StudyRunner.h" -#include "Interpreter/Parser.h" +#include "DSL/Parser.h" #include #include #include @@ -65,8 +68,8 @@ std::vector StudyRunner::runStudies(const std::vector& conf simCore->setFixedDt(cfg.dt); simCore->setIntegrationMethod(cfg.method); - auto program = std::make_unique(simCore.get()); - interpreter::Parser parser(program.get()); + auto program = std::make_unique(simCore.get()); + dsl::Parser parser(program.get()); parser.parse(scriptText); program->start(); @@ -136,8 +139,8 @@ std::vector StudyRunner::runStudies(const std::vector& conf 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()); + auto program = std::make_unique(simCore.get()); + dsl::Parser parser(program.get()); // Parse the script text to build the program for this run parser.parse(script); // start it diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 8f6288c0..af558ebd 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -1,14 +1,17 @@ -// DSFE_Core SimulationCore.cpp +/* + * File: Scene/SimulationCore.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" #include "Scene/SimulationCore.h" -#include "Robots/RobotSystem.h" -#include "Robots/RobotModel.h" -#include "Robots/TrajectoryManager.h" +#include "Systems/RigidBodySystem.h" +#include "Systems/RigidBodyModel.h" +#include "Systems/TrajectoryManager.h" #include "SingleBodySystem/Body.h" -#include "Interpreter/StoredProgram.h" -#include "Interpreter/Parser.h" +#include "DSL/StoredProgram.h" +#include "DSL/Parser.h" #include "Platform/Paths.h" #include "EngineLib/LogMacros.h" @@ -16,11 +19,11 @@ namespace core { // Owned constructed subsystems (default) SimulationCore::SimulationCore() - : _trajOwned(std::make_unique()), _robotOwned(std::make_unique()), + : _trajOwned(std::make_unique()), _rigidBodyOwned(std::make_unique()), _singleBodyOwned(std::make_unique()) { _traj = _trajOwned.get(); - _robot = _robotOwned.get(); + _rigidBody = _rigidBodyOwned.get(); _singleBody = _singleBodyOwned.get(); startExportThread(); @@ -32,19 +35,19 @@ namespace core { } // Non-owning constructor (used when subsystems are managed externally, e.g. by the SimulationManager) - SimulationCore::SimulationCore(robots::RobotSystem& robot, control::TrajectoryManager& traj) - : _robot(&robot), _traj(&traj), _singleBody(nullptr) { + SimulationCore::SimulationCore(systems::RigidBodySystem& rigidBody, control::TrajectoryManager& traj) + : _rigidBody(&rigidBody), _traj(&traj), _singleBody(nullptr) { startExportThread(); } // Simulation System void SimulationCore::setupSimulationIntegrator() { - if (!_robot && !_singleBody) { return; } + if (!_rigidBody && !_singleBody) { return; } integration::IntegrationService* intgr; integration::DifferentiableIntegrator* adIntgr; - if (_robot) { - intgr = _robot->getIntegrator(); - adIntgr = _robot->getADIntegrator(); + if (_rigidBody) { + intgr = _rigidBody->getIntegrator(); + adIntgr = _rigidBody->getADIntegrator(); } if (_singleBody) { intgr = _singleBody->getIntegrator(); @@ -56,24 +59,24 @@ namespace core { adIntgr->runtimeState()->last_dt_taken = _dt; adIntgr->runtimeState()->last_dt_sug = _dt; } - // Set the integration method for the simulation (also updates the robot's integrator if it exists) + // Set the integration method for the simulation (also updates the rigidBody's integrator if it exists) void SimulationCore::setIntegrationMethod(integration::eIntegrationMethod method) { - if (!_robot && !_singleBody) { return; } + if (!_rigidBody && !_singleBody) { return; } if (_singleBody) { _singleBody->setStandardIntegrator(method); } - if (_robot) { _robot->setStandardIntegrator(method); } + if (_rigidBody) { _rigidBody->setStandardIntegrator(method); } } - // Set the auto-diff integration method for the simulation (also updates the robot's AD integrator if it exists) + // Set the auto-diff integration method for the simulation (also updates the rigidBody's AD integrator if it exists) void SimulationCore::setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method) { - if (!_robot && !_singleBody) { return; } + if (!_rigidBody && !_singleBody) { return; } if (_singleBody) { _singleBody->setADIntegrator(method); } - if (_robot) { _robot->setADIntegrator(method); } + if (_rigidBody) { _rigidBody->setADIntegrator(method); } } - // Get the name of the current integration method (returns "no_robot" if no robot is loaded) + // Get the name of the current integration method (returns "no_rigidBody" if no rigidBody is loaded) std::string SimulationCore::integrationMethodName() const { std::string intName; - if (_robot) { - const auto state = _robot->runtimeIntegratorState(); - intName = (_robot->autoDiffEnabled()) ? _robot->AD_integratorName() : _robot->getIntegratorName(); + if (_rigidBody) { + const auto state = _rigidBody->runtimeIntegratorState(); + intName = (_rigidBody->autoDiffEnabled()) ? _rigidBody->AD_integratorName() : _rigidBody->getIntegratorName(); return intName; } else if(_singleBody) { @@ -88,26 +91,26 @@ namespace core { } // Get the current integration method integration::eIntegrationMethod SimulationCore::integrationMethod() const { - if (_robot) { return _robot->getIntegrationMethod(); } + if (_rigidBody) { return _rigidBody->getIntegrationMethod(); } if (_singleBody) { return _singleBody->getIntegrationMethod(); } return integration::eIntegrationMethod::RK4; } // Get the current auto-diff integration method integration::eAutoDiffIntegrationMethod SimulationCore::autoDiffIntegrationMethod() const { - if (_robot) { return _robot->AD_IntegrationMethod(); } + if (_rigidBody) { return _rigidBody->AD_IntegrationMethod(); } if (_singleBody) { return _singleBody->AD_IntegrationMethod(); } return integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler; // default return value } void SimulationCore::enableAutoDiff(bool enable) { - if (!_robot) { return; } - _robot->enableAutoDiff(enable); + if (!_rigidBody) { return; } + _rigidBody->enableAutoDiff(enable); } bool SimulationCore::autoDiffEnabled() const { - if (!_robot) { return false; } - return _robot->autoDiffEnabled(); + if (!_rigidBody) { return false; } + return _rigidBody->autoDiffEnabled(); } - // Fixed timestep loop for physics and robot updates, called from the main render loop with the frame delta time + // Fixed timestep loop for physics and rigidBody updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { double simTime = _simTime.load(); @@ -140,23 +143,23 @@ namespace core { _scriptRunning.store(false); } - const auto state = _robot->runtimeIntegratorState(); + const auto state = _rigidBody->runtimeIntegratorState(); - // Update physics and robot system if sim is running + // Update physics and rigidBody system if sim is running if (_simRunning.load()) { simTime += _dt; - // Update robot trajectory inputs and step the robot forward in time - if (hasRobot()) { - _robot->updateTrajectoryInputs(*_traj, simTime); - _robot->step(_dt, simTime); + // Update rigidBody trajectory inputs and step the rigidBody forward in time + if (hasRigidBody()) { + _rigidBody->updateTrajectoryInputs(*_traj, simTime); + _rigidBody->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); + _telemetry.update(simTime, *_rigidBody, _traj, diagnostics::eTelemetryLevel::FULL); } if (hasSingleBody()) { _singleBody->step(_dt, simTime); } } @@ -182,15 +185,15 @@ namespace core { _accum = 0.0; // Reset simulation system - if (_robot) { - _robot->resetRobot(); + if (_rigidBody) { + _rigidBody->resetRigidBody(); _trajRefBuffer.clear(); // 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 joints = _rigidBody->jointCount(); double expectedSeconds = expectedMinutes * 60.0; size_t steps = static_cast(expectedSeconds / _dt); @@ -199,13 +202,13 @@ namespace core { size_t total = static_cast(std::min(total64, MAX_LOG_ENTRIES)); // Internal double buf for high-rate joint log - _robot->useInternalLogBuffer(true); - _robot->reserveInternalLogBuffers(total); + _rigidBody->useInternalLogBuffer(true); + _rigidBody->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); + _rigidBody->setRefBuffer(&_trajRefBuffer); } if (_singleBody) { _singleBody->resetBody(); @@ -233,7 +236,7 @@ namespace core { if (!_simRunning.load()) { return; } D_RUNTIME("stopping simulation"); - auto buf = _robot->claimExportLogBuffer(); // Claim the export log buffer from the robot + auto buf = _rigidBody->claimExportLogBuffer(); // Claim the export log buffer from the rigidBody if (buf) { enqueueExportBuffer(std::move(buf)); } flushExports(); @@ -247,13 +250,13 @@ namespace core { } // Exporst the logged joint data to HDF5 format using the custom macro for each log entry - void SimulationCore::exportLogsToHDF5(const robots::JointLogBuffer& exportBuf) { + void SimulationCore::exportLogsToHDF5(const systems::JointLogBuffer& exportBuf) { auto t0 = std::chrono::steady_clock::now(); - const auto state = _robot->runtimeIntegratorState(); - const std::string intName = (state && state->autoDiff) ? _robot->AD_integratorName() : _robot->getIntegratorName(); - const std::string robotName = _robot->hasRobot() ? _robot->robotName() : "no_robot"; - const std::string header = robotName + "_sim_" + intName; + const auto state = _rigidBody->runtimeIntegratorState(); + const std::string intName = (state && state->autoDiff) ? _rigidBody->AD_integratorName() : _rigidBody->getIntegratorName(); + const std::string rigidBodyName = _rigidBody->hasRigidBody() ? _rigidBody->rigidBodyName() : "no_rigidBody"; + const std::string header = rigidBodyName + "_sim_" + intName; // Check if there are any log entries to export const size_t N = exportBuf.size(); @@ -274,8 +277,8 @@ namespace core { // 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"; + const std::string rigidBodyName = _rigidBody->hasRigidBody() ? _rigidBody->rigidBodyName() : "no_rigidBody"; + const std::string header = rigidBodyName + "_traj_ref"; // Check if there are any log entries const size_t N = _trajRefBuffer.size(); @@ -314,23 +317,23 @@ namespace core { // -------------------------------------------------- // Run a script synchronously to completion, blocking the main thread. Returns true if completed successfully - bool SimulationCore::runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) { + bool SimulationCore::runScriptToCompletion(dsl::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", "implicit_euler", "implicit_midpoint", "glrk2", "glrk3" }; const std::string methodName = names[static_cast(method)]; - LOG_INFO("SimulationCore::runScriptToCompletion -> START method=%s dt=%.6f hasRobot=%d", methodName.c_str(), _dt, (int)hasRobot()); + LOG_INFO("SimulationCore::runScriptToCompletion -> START method=%s dt=%.6f hasRigidBody=%d", methodName.c_str(), _dt, (int)hasRigidBody()); - // Reset robot state - _robot->resetRobot(); + // Reset rigidBody state + _rigidBody->resetRigidBody(); _traj->clearAll(); // Clear reference buffer (external for now) _trajRefBuffer.clear(); // Inject reference buffer only - _robot->setRefBuffer(&_trajRefBuffer); - // Set integrator on both physics and robot systems - _robot->setStandardIntegrator(method); + _rigidBody->setRefBuffer(&_trajRefBuffer); + // Set integrator on both physics and rigidBody systems + _rigidBody->setStandardIntegrator(method); scriptParallelisation(program); @@ -339,7 +342,7 @@ namespace core { return (_telemetry.ring.size() >= 2); } - void SimulationCore::scriptParallelisation(interpreter::IStoredProgram* program) { + void SimulationCore::scriptParallelisation(dsl::IStoredProgram* program) { // Reset simulation state _simTime.store(0.0, std::memory_order_relaxed); _simRunning.store(false); @@ -374,13 +377,13 @@ namespace core { // Step the program (DSL command execution) program->step(dt); - // Step physics and robot if sim is running + // Step physics and rigidBody if sim is running if (_simRunning.load()) { simTime += dt; - if (hasRobot()) { + if (hasRigidBody()) { // Update Trajectory Inputs - _robot->updateTrajectoryInputs(*_traj, simTime); - _robot->step(dt, simTime); + _rigidBody->updateTrajectoryInputs(*_traj, simTime); + _rigidBody->step(dt, simTime); // Telemetry beginRun if (!_telemetryBegun) { _telemetry.beginRun(simTime, _telHz, 300.0); @@ -388,7 +391,7 @@ namespace core { 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); + _telemetry.update(simTime, *_rigidBody, _traj, diagnostics::eTelemetryLevel::FULL); } } } @@ -428,20 +431,20 @@ namespace core { // --- Setters and Getters for Systems and State --- - // Accessor for the robot system (non-const and const versions) - robots::RobotSystem& SimulationCore::robotSystem() { return *_robot; } - // Setter and checker for Robot System - void SimulationCore::setRobotSystem(robots::RobotSystem* robot) { _robot = robot; } - bool SimulationCore::hasRobot() const { return _robot && _robot->hasRobot(); } - // Loads a robot into the robot system by name - void SimulationCore::loadRobot(const std::string& name) { - loadRobotInternal(name); - _robotPresentationDirty = true; + // Accessor for the rigidBody system (non-const and const versions) + systems::RigidBodySystem& SimulationCore::rigidBodySystem() { return *_rigidBody; } + // Setter and checker for RigidBody System + void SimulationCore::setRigidBodySystem(systems::RigidBodySystem* rigidBody) { _rigidBody = rigidBody; } + bool SimulationCore::hasRigidBody() const { return _rigidBody && _rigidBody->hasRigidBody(); } + // Loads a rigidBody into the rigidBody system by name + void SimulationCore::loadRigidBody(const std::string& name) { + loadRigidBodyInternal(name); + _rigidBodyPresentationDirty = true; } - // Internal method to load a robot, assumes ownership of the robot system - void SimulationCore::loadRobotInternal(const std::string& name) { - if (!_robot) { LOG_ERROR("Cannot load robot: RobotSystem not set"); return; } - _robot->loadRobot(name); + // Internal method to load a rigidBody, assumes ownership of the rigidBody system + void SimulationCore::loadRigidBodyInternal(const std::string& name) { + if (!_rigidBody) { LOG_ERROR("Cannot load rigidBody: RigidBodySystem not set"); return; } + _rigidBody->loadRigidBody(name); } // Accessor for the single body system (non-const and const versions) @@ -466,8 +469,8 @@ namespace core { control::TrajectoryManager& SimulationCore::trajectoryManager() { return *_traj; } // Setters for the metric buffers - void SimulationCore::setJointLogBuffer(robots::JointLogBuffer* buf) { _jointLogBuffer = *buf; } - void SimulationCore::setTrajRefBuffer(robots::TrajRefBuffer* buf) { _trajRefBuffer = *buf; } + void SimulationCore::setJointLogBuffer(systems::JointLogBuffer* buf) { _jointLogBuffer = *buf; } + void SimulationCore::setTrajRefBuffer(systems::TrajRefBuffer* buf) { _trajRefBuffer = *buf; } // Accessor for the telemetry recorder (non-const and const versions) diagnostics::TelemetryRecorder& SimulationCore::telemetry() { return _telemetry; } @@ -475,8 +478,8 @@ namespace core { 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; } + void SimulationCore::setActiveProgram(dsl::IStoredProgram* p) { _activeProgram = p; } + dsl::IStoredProgram* SimulationCore::activeProgram() const { return _activeProgram; } // Thread-based methods @@ -500,7 +503,7 @@ namespace core { // Main loop for the export thread, waits for export buffers to be enqueued and processes them void SimulationCore::exportThreadMain() { while (true) { - std::unique_ptr buf; + std::unique_ptr buf; { std::unique_lock lock(_expMutex); _expCondVar.wait(lock, [this]() { @@ -523,7 +526,7 @@ namespace core { } } - void SimulationCore::enqueueExportBuffer(std::unique_ptr buf) { + void SimulationCore::enqueueExportBuffer(std::unique_ptr buf) { { std::lock_guard lock(_expMutex); ++_exportsInFlight; diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp new file mode 100644 index 00000000..ff79e576 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp @@ -0,0 +1,46 @@ +/* + * File: Systems/RigidBodySnapshot.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "Systems/RigidBodySimSnapshot.h" + +namespace systems { + // Method to check if a joint affects a link + bool RigidBodyConstModel::jointAffectsLink(size_t jIdx, size_t lIdx) const { + if (jIdx >= joints.size() || lIdx >= links.size()) { return false; } + + const std::string& targetJointChild = joints[jIdx].child; + const std::string& targetLinkName = links[lIdx].name; + + // Check if the joint is an ancestor of the link in the kinematic tree + std::string current = targetLinkName; + + while (true) { + if (current == targetJointChild) { return true; } // joint affects this link + bool movedUp = false; + for (const auto& joint : joints) { + if (joint.child == current) { + current = joint.parent; // move up to the parent link + movedUp = true; + break; + } + } + if (!movedUp) { break; } // reached the root link without finding the joint + } + + return false; // joint does not affect this link + } + + // Method to get the index of a link by name, returns -1 if not found + int RigidBodyConstModel::linkIndex(const std::string& linkName) const { + auto it = linkNameToIndex.find(linkName); + if (it != linkNameToIndex.end()) { + return it->second; + } + else { + return -1; // not found + } + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp new file mode 100644 index 00000000..14b370ca --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -0,0 +1,986 @@ +/* + * File: Systems/RigidBodySystem.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "Systems/RigidBodySystem.h" +#include "Systems/RigidBodyLoader.h" + +#include +#include +#include + +#include +#include "Systems/TrajectoryManager.h" +#include "Platform/Paths.h" + +#include "EngineLib/LogMacros.h" +#include "Platform/DataManager.h" + +using namespace mathlib; +using namespace constants; + +namespace systems { + // Constructor + RigidBodySystem::RigidBodySystem() + : _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), + _AD_integrator(std::make_unique()), _curIntMethod_AD(integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler), + _kinematics(std::make_unique()), _dynamics(std::make_unique()), + _torqueMode(eTorqueMode::CONTROLLED) { + if (!_integrator ) { LOG_WARN("RigidBodySystem got null IntegrationService*"); } + } + // Destructor + RigidBodySystem::~RigidBodySystem() = default; + + const systems::RigidBodyModel& RigidBodySystem::model() const { return _body; } + + // Helper function to convert std::vector to Eigen::VectorXd + static VecX toVecX(const std::vector& a) { + VecX v(a.size()); + for (size_t i = 0; i < a.size(); ++i) { v(i) = a[i]; } + return v; + } + + // --- HELPER METHODS --- + + // Method to clamp a joint angle to its limits + double RigidBodySystem::clampJointAngle(const RigidBodyJoint& joint, double angleRad) { + if (joint.limits.continuous) { return wrapRad(angleRad); } + else { return std::clamp(angleRad, joint.limits.minAngle, joint.limits.maxAngle); } + } + + // 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; + + // Check if we're in the "soft zone" near the velocity limit + const double absw = std::abs(omega); // [rad/s] + const double wSoft = 0.90 * wMax; // [rad/s] + + if (absw <= wSoft) { return; } + + // Check if we're above the hard limit (with some tolerance) + const double t = (absw - wSoft) / (wMax - wSoft); // [0, 1] as we go from wSoft to wMax + const double T = 0.2; // [s], time constant for how quickly the wall ramps up + const double wall = (I_eff / T) * (t * t) * (absw - wSoft); // [Nm] + + // Apply opposing torque to reduce |omega| + tau -= wall * (omega >= 0.0 ? 1.0 : -1.0); + } + + // --- ROBOT STATE INTEGRATION METHODS --- + + // Method to build a name-to-index map for rigidBody links + void RigidBodySystem::buildLinkIndex() { + _link_idx.clear(); + for (size_t i = 0; i < _body.links.size(); i++) { + _link_idx[_body.links[i].name] = (int)i; + LOG_INFO("Link %zu: %s -> index %d", i, _body.links[i].name.c_str(), (int)i); + } + } + + // Method to build the spatial model (kinematic tree) from the rigidBody model + void RigidBodySystem::buildSpatialModel() { + _spatialModel.joints.clear(); + const size_t n = _body.joints.size(); + _spatialModel.joints.resize(n); + + for (size_t i = 0; i < n; ++i) { + const RigidBodyJoint& j = _body.joints[i]; + auto& sj = _spatialModel.joints[i]; + + sj.name = j.name; + sj.type = j.type; + + // Find parent joint + sj.parent = -1; + for (size_t p = 0; p < n; ++p) { + if (_body.joints[p].child == j.parent) { + sj.parent = (int)p; + break; + } + } + + // Build XTree + mathlib::Mat3 R = j.origin_q.toRotationMatrix(); + mathlib::Vec3 r = j.origin_xyz; + sj.Xtree = mathlib::spatialTransform(R, r); + + // Build Spatial Inertia + int childLinkIdx = -1; + for (size_t l = 0; l < _body.links.size(); ++l) { + if (_body.links[l].name == j.child) { + childLinkIdx = (int)l; + break; + } + } + + if (childLinkIdx >= 0) { + const RigidBodyLink& link = _body.links[childLinkIdx]; + mathlib::Mat3 I_com; + + const auto& I = link.inertial.inertia; + I_com << + I.ixx, I.ixy, I.ixz, + I.ixy, I.iyy, I.iyz, + I.ixz, I.iyz, I.izz; + + sj.inertia = mathlib::spatialInertia( + link.inertial.mass, + link.inertial.com_xyz, + I_com + ); + } + + // Build S vector (motion subspace) + switch (j.type) { + case eJointType::REVOLUTE: + sj.S = mathlib::SpatialVec(j.axis.normalized(), mathlib::Vec3::Zero()); + break; + case eJointType::PRISMATIC: + sj.S = mathlib::SpatialVec(mathlib::Vec3::Zero(), j.axis.normalized()); + break; + default: + sj.S = mathlib::SpatialVec(); + break; + } + } + LOG_INFO("SpatialModel built: joints=%d", (long long)_spatialModel.joints.size()); + } + + // Method to pack rigidBody joint states into a state vector + mathlib::VecX RigidBodySystem::packState() const { + const size_t n = static_cast(_body.joints.size()); + mathlib::VecX x(2 * n); + + // Pack angles and velocities + for (size_t i = 0; i < n; ++i) { + auto& j = _body.joints[i]; + + // Current states + x[i] = j.q; + x[i + n] = j.qd; + } + return x; // state vector + } + + // Method to unpack state vector into rigidBody joints + void RigidBodySystem::unpackState(const mathlib::VecX& x) { + const size_t n = static_cast(_body.joints.size()); + + // Resize clamping vectors if necessary + if (_clampTheta.size() != n) { _clampTheta.assign(n, 0); } + if (_clampOmega.size() != n) { _clampOmega.assign(n, 0); } + + // For each joint + for (size_t i = 0; i < n; ++i) { + auto& j = _body.joints[i]; + + // Current states + double theta_in = x[i]; // [rad] + double omega_in = x[i + n]; // [rad/s] + + // Clamp joint angle + double theta_out = clampJointAngle(j, theta_in); + + // max |omega| + double wMax_hw = std::abs(j.limits.maxqd); + double omega_out = omega_in; + + // Velocity limit clamping + if (wMax_hw > 0.0f) { + const double eps = 0.05f; + if (std::abs(omega_in) > (1.0f + eps) * wMax_hw) { + omega_out = std::clamp(omega_in, -wMax_hw, wMax_hw); + } + } + + // Velocity limit enforcement + if (theta_out != theta_in) { + const double upperLimit = j.limits.maxAngle; + const double lowerLimit = j.limits.minAngle; + if (theta_out >= upperLimit && omega_in > 0.0f) { omega_out = 0.0f; } + if (theta_out <= lowerLimit && omega_in < 0.0f) { omega_out = 0.0f; } + } + + // Record clamping + _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; + _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; + // Update joint states + j.q = theta_out; + j.qd = omega_out; + } + } + + // Method to pack rigidBody joint states into a state vector + mathlib::VecX_T> RigidBodySystem::packState_AD() const { + using Dual = DualNumber_T; // only hardcoded since I am testing the same arm, TODO provide a better final way to derive the NVar val. + const size_t n = static_cast(_body.joints.size()); + mathlib::VecX_T x(2 * n); + + // Pack angles and velocities + for (size_t i = 0; i < n; ++i) { + auto& j = _body.joints[i]; + + // Current states + x[i] = Dual(j.q, { 0.0 }); + x[i + n] = Dual(j.qd, { 0.0 }); + } + return x; // state vector + } + + // Method to unpack state vector into rigidBody joints + void RigidBodySystem::unpackState_AD(const mathlib::VecX_T>& x) { + using Dual = DualNumber_T; + const size_t n = static_cast(_body.joints.size()); + + // Resize clamping vectors if necessary + if (_clampTheta.size() != n) { _clampTheta.assign(n, 0); } + if (_clampOmega.size() != n) { _clampOmega.assign(n, 0); } + + // For each joint + for (size_t i = 0; i < n; ++i) { + auto& j = _body.joints[i]; + + // Current states + Dual theta_in = x[i]; // [rad] + Dual omega_in = x[i + n]; // [rad/s] + + // Clamp joint angle + Dual theta_out = clampJointAngle_T(j, theta_in); + + // max |omega| + const double wMax_hw = mathlib::abs(j.limits.maxqd); + Dual omega_out = omega_in; + + // Velocity limit clamping + if (wMax_hw > 0.0) { + omega_out = wMax_hw * mathlib::tanh(omega_in / wMax_hw); // smoothly clamp omega to wMax_hw using a tanh function + } + + // Velocity limit enforcement + if (theta_out != theta_in) { + const double upperLimit = j.limits.maxAngle; + const double lowerLimit = j.limits.minAngle; + if (theta_out >= upperLimit && omega_in > Dual(0)) { omega_out = Dual(0); } + if (theta_out <= lowerLimit && omega_in < Dual(0)) { omega_out = Dual(0); } + } + + // Record clamping + _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; + _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; + // Update joint states + j.q = mathlib::real(theta_out); + j.qd = mathlib::real(omega_out); + } + } + + // Method to pack reference state vector (target angles and velocities) for control + mathlib::VecX RigidBodySystem::packRefState() const { + const size_t n = (int)_body.joints.size(); + mathlib::VecX x(2 * n); + for (size_t i = 0; i < n; ++i) { + auto& j = _body.joints[i]; + + // Pack reference angles and velocities + x[i] = j.q_ref; + x[i + n] = j.qd_ref; + } + return x; // reference state vector + } + + // Method to unpack reference state vector into rigidBody joints + void RigidBodySystem::unpackRefState(const mathlib::VecX& x) { + const size_t n = (int)_body.joints.size(); + for (size_t i = 0; i < n; ++i) { + auto& j = _body.joints[i]; + j.q_ref = x[i]; // [rad] + j.qd_ref = x[i + n]; // [rad/s] + j.q_ref = clampJointAngle(j, j.q_ref); // [rad] + } + } + + // Method to enforce joint limits after integration + void RigidBodySystem::enforceJointLimits(RigidBodyJoint& j) { + if (j.limits.continuous) { return; } + + const double lo = j.limits.minAngle; + const double hi = j.limits.maxAngle; + + 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 rigidBody state by dt using the selected integrator + void RigidBodySystem::step(double dt, double simTime) { + if (!_hasBody) { return; } + + if (_useAutoDiff) { + step_AD(dt, simTime); + return; + } + + _simTime = simTime; + const size_t n = _body.joints.size(); + mathlib::VecX x = packState(); + + auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); + + unpackState(result.stepOut.x_next); + _dynamics->setDt(result.stepOut.dt_taken); + + const auto scratchCopy = _dynScratch; + const auto resultCopy = result; + + postStepUpdate(resultCopy.stepOut.x_next, scratchCopy, resultCopy); + + // Update base pose if free-floating + if (_baseIsFree) { + integrateBaseTranslation(dt); + updateBaseRootPose(); + } + + // Update kinematics + computeRigidBodyKinematics(_worldTransforms); + } + + // Method to step the reference trajectory and update joint reference states + void RigidBodySystem::updateTrajectoryInputs(control::TrajectoryManager& traj, double t) { + if (!_hasBody) { return; } + + const size_t n = _body.joints.size(); + if (n <= 0) { return; } + + // Sample trajectories ("ground truth" inputs) + for (size_t i = 0; i < n; ++i) { + RigidBodyJoint& j = _body.joints[i]; + control::TrajState s{}; + // Try to evaluate trajectory + if (traj.tryEval(std::string(j.child), t, s)) { + j.q_ref = clampJointAngle(j, s.q); // set ref angle + j.qd_ref = s.qd; + j.qdd_ref = s.qdd; + } + // Store inputs + else { + j.qdd_ref = 0.0f; + j.qd_ref = 0.0f; + } + + auto* buf = _refBuffer; + if (buf) { + // Sim Metadata + buf->sim_time.push_back(t); + // Reference states + 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); + } + } + } + + // --- ROBOT LOADING AND RESET METHODS --- + + // Method to load a rigidBody model by name + void RigidBodySystem::loadRigidBody(const std::string& name) { + if (name == _loadedName) { + LOG_INFO("RigidBody '%s' is already loaded, skipping load.", name.c_str()); + D_INFO("RigidBody '%s' is already loaded, skipping load.", name.c_str()); + return; + } + + // Reset control parameters to target values so that if the new rigidBody has different defaults, we start with those + resetNaturalFrequencyToTarget(); + resetDampingRatioToTarget(); + + // Construct path to rigidBody JSON file + const std::filesystem::path jsonPath = paths::assets() / "objects" / "RigidBodyic_Arm_Models" / name / (name + ".json"); + if (!std::filesystem::exists(jsonPath)) { + LOG_ERROR("RigidBody JSON file not found -> %s", jsonPath.string().c_str()); + D_ERROR("RigidBody JSON file not found -> %s", jsonPath.string().c_str()); + return; + } + + // Load rigidBody model from JSON + _body = systems::RigidBodyLoader::loadFromJSON(jsonPath.string()); + const size_t n = _body.joints.size(); + const size_t m = _body.links.size(); + + _constModel.name = _body.name; + _constModel.scale = _body.scale; + + _constModel.baseFrame = _body.baseFrame; + _constModel.baseFrameIsAligned = _body.baseFrameIsEngineAligned; + + _constModel.links = _body.links; + _constModel.joints = _body.joints; + + _constModel.linkNameToIndex.clear(); + for (size_t i = 0; i < _constModel.links.size(); ++i) { + _constModel.linkNameToIndex[_constModel.links[i].name] = (int)i; + } + + LOG_INFO_ONCE( + "CONST MODEL: links=%lld joints=%lld", + (long long)_constModel.links.size(), + (long long)_constModel.joints.size() + ); + + _loadedName = name; + _baseIsFree = false; + + // Check if any joint is free-floating to determine if the base is free + for (const auto& joint : _body.joints) { + if (joint.type == eJointType::FREE) { + _baseIsFree = true; + break; + } + } + + _root_home = _body.baseFrame; + _root_pose = _root_home; + + _q_home = _body.makeJointVector(); + _home_valid = true; + + buildLinkIndex(); + buildSpatialModel(); + _hasBody = true; + + resetRigidBody(); + + LOG_INFO("Loaded RigidBody model -> %s", name.c_str()); + D_SUCCESS("Loaded RigidBody model -> %s", name.c_str()); + } + + // Method to reset the rigidBody to its home position + void RigidBodySystem::resetRigidBody() { + if (!_hasBody || !_home_valid) { LOG_ERROR("Reset aborterd."); return; } + _root_pose = _root_home; + _body.setJointVector(_q_home); + + for (auto& joint : _body.joints) { + joint.qd = 0.0; + joint.q_ref = joint.q; + joint.qd_ref = 0.0; + joint.qdd_ref = 0.0; + } + + // Reset base state if free-floating + _basePos = Vec3(0, 0, 0); + _baseVel = Vec3(0, 0, 0); + _baseAcc = Vec3(0, 0, 0); + + // Assuming base orientation is represented as a yaw angle for simplicity + _baseYaw = 0.0; + _baseYawRate = 0.0; + _baseYawAcc = 0.0; + + _dynScratch.clear(); + _dynScratch_AD.clear(); + + _dynResult.resize(0); + _dynResult_AD.resize(0); + + _dynScratch.resize(_body.joints.size(), _body.links.size()); + _dynScratch_AD.resize(_body.joints.size(), _body.links.size()); + _dynResult.resize(_body.joints.size()); + _dynResult_AD.resize(_body.joints.size()); + + _dynScratch.g.setConstant(_gravity); + + // Reset adaptive integrator so it doesn't carry a stale step size + _integrator->resetAdaptiveState(); + + computeRigidBodyKinematics(_worldTransforms); + D_INFO("RigidBody reset to home position."); + D_SUCCESS("RigidBody reset to home position."); + } + + void RigidBodySystem::stopAll() { + if (!_hasBody) return; + for (auto& joint : _body.joints) { + joint.qd = 0.0f; + joint.q_ref = joint.q; + } + } + + integration::IntegrationService* RigidBodySystem::getIntegrator() { return _integrator.get(); } + const integration::IntegrationService* RigidBodySystem::getIntegrator() const { return _integrator.get(); } + void RigidBodySystem::setStandardIntegrator(integration::eIntegrationMethod m) { + _curIntMethod = m; _integrator->setIntegrationMethod(m); + } + + integration::DifferentiableIntegrator* RigidBodySystem::getADIntegrator() { return _AD_integrator.get(); } + const integration::DifferentiableIntegrator* RigidBodySystem::getADIntegrator() const { return _AD_integrator.get(); } + void RigidBodySystem::setADIntegrator(integration::eAutoDiffIntegrationMethod m) { + _curIntMethod_AD = m; _AD_integrator->setIntegrationMethod(m); + } + + std::shared_ptr RigidBodySystem::runtimeIntegratorState() { + return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); + } + + std::shared_ptr RigidBodySystem::runtimeIntegratorState() const { + return _useAutoDiff ? _AD_integrator->runtimeState() : _integrator->runtimeState(); + } + + // --- ROBOT KINEMATICS AND JOINT STATE METHODS --- + + std::string RigidBodySystem::findRootLink() const { + std::unordered_set children; + for (const auto& joint : _body.joints) { children.insert(joint.child); } + for (const auto& link : _body.links) { + if (children.find(link.name) == children.end()) { + return link.name; + } + } + return _body.links.empty() ? "" : _body.links.front().name; // fallback + } + + // Method to update the pose of each rigidBody link based on current joint angles using forward kinematics + void RigidBodySystem::computeRigidBodyKinematics(std::vector& world) { + if (!_hasBody) { + world.clear(); + return; + }; + + world.resize(_body.links.size()); + for (auto& T : world) { T = Mat4::Identity(); } + + // Find root link + const std::string rootName = findRootLink(); + auto itRoot = _link_idx.find(rootName); + if (itRoot == _link_idx.end()) { + LOG_WARN_ONCE("RigidBodySystem::updateRigidBodyKinematics: root link '%s' not found in link index", rootName.c_str()); + return; + } + + // Set root link pose + int rootIdx = itRoot->second; + world[rootIdx] = _root_pose; + + // parent -> children joints + std::unordered_map> children; + children.reserve(_body.joints.size()); + for (const auto& j : _body.joints) children[j.parent].push_back(&j); + + std::stack st; + st.push(rootName); + + // Traverse the kinematic tree using DFS + while (!st.empty()) { + std::string parentName = st.top(); + st.pop(); + + // Skip if parent link not found + auto itP = _link_idx.find(parentName); + if (itP == _link_idx.end()) { continue; } + int pIdx = itP->second; + + const Mat4& T_parent = world[pIdx]; + + + // Find children joints + auto it = children.find(parentName); + if (it == children.end()) continue; + + // For each child joint + for (const RigidBodyJoint* jp : it->second) { + const RigidBodyJoint& j = *jp; + auto itC = _link_idx.find(j.child); + if (itC == _link_idx.end()) { continue; } + int cIdx = itC->second; + + // Joint origin transform + Mat4 T_joint = Mat4::Identity(); + T_joint.block<3, 1>(0, 3) = j.origin_xyz; + + // Joint origin rotation + Mat4 R_joint = Mat4::Identity(); + R_joint.block<3, 3>(0, 0) = j.origin_q.toRotationMatrix(); + + // Compute child link pose in world frame + Mat4 T_child = T_parent * T_joint * R_joint; + + Vec3 axis = j.axis.norm() > 1e-8 ? j.axis.normalized() : Vec3(0, 0, 1); // default axis if zero + + // Apply joint rotation for revolute joints + if (j.type == eJointType::REVOLUTE) { + Mat4 R_q = Mat4::Identity(); + R_q.block<3, 3>(0, 0) = Eigen::AngleAxisd(j.q, axis).toRotationMatrix(); + T_child = T_child * R_q; + } + else if (j.type == eJointType::PRISMATIC) { + Mat4 T_q = Mat4::Identity(); + T_q.block<3, 1>(0, 3) = axis * j.q; // translate along joint axis by q + T_child = T_child * T_q; + } + + // FIXED joints: no motion + world[cIdx] = T_child; + st.push(j.child); + } + } + } + + // --- JOINT STATE GETTERS AND SETTERS --- + + // Method to get the angle of a specific rigidBody joint + bool RigidBodySystem::tryGetJointAngleRad(const std::string& childLink, double& outAngle) const { + if (!_hasBody) { return false; } + // Find joint child matching childLink + for (const auto& joint : _body.joints) { + if (joint.child == childLink) { + outAngle = joint.q; + return true; + } + } + return false; + } + + // Method to set the angle of a specific rigidBody joint + bool RigidBodySystem::trySetJointAngleRad(const std::string& childLink, double angleRad) { + if (!_hasBody) { return false; } + // Find joint child matching childLink + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + joint.q = clampJointAngle(joint, angleRad); // clamp to joint limits + return true; + } + } + return false; + } + + // Method to get the angular velocity of a specific rigidBody joint + bool RigidBodySystem::tryGetJointOmegaRad(const std::string& childLink, double& outOmega) const { + if (!_hasBody) { return false; } + // Find joint child matching childLink + for (const auto& joint : _body.joints) { + if (joint.child == childLink) { + outOmega = joint.qd; + return true; + } + } + return false; + } + + // Method to set the angular velocity of a specific rigidBody joint + bool RigidBodySystem::trySetJointOmegaRad(const std::string& childLink, double omegaRad) { + if (!_hasBody) { return false; } + // Find joint child matching childLink + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + joint.qd = omegaRad; + return true; + } + } + return false; + } + + // Method to directly inject an angular velocity into the state vector for a specific rigidBody joint (bypassing any clamping or limits) + bool RigidBodySystem::injectJointOmegaRad(const std::string& childLink, double omega) { + if (!_hasBody) return false; + const size_t n = _body.joints.size(); + // Find joint child matching childLink + for (size_t i = 0; i < n; ++i) { + if (_body.joints[i].child == childLink) { + // Modify actual state vector + mathlib::VecX x = packState(); + x[i + n] = omega; // velocity slot + unpackState(x); + return true; + } + } + return false; + } + + // Method to get the target angle (reference) of a specific rigidBody joint in radians + bool RigidBodySystem::tryGetJointTargetRad(const std::string& childLink, double& outTargetRad) const { + if (!_hasBody) { return false; } + for (const auto& joint : _body.joints) { + if (joint.child == childLink) { + outTargetRad = joint.q_ref; + return true; + } + } + return false; + } + + // Method to set the target angle (reference) of a specific rigidBody joint in radians + bool RigidBodySystem::trySetJointTargetRad(const std::string& childLink, double targetRad) { + if (!_hasBody) { return false; } + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + if (joint.limits.continuous) { joint.q_ref = wrapRad(targetRad); } + else { joint.q_ref = clampJointAngle(joint, targetRad); } // clamp to joint + return true; + } + } + return false; + } + + // Method to get the maximum angular velocity of a specific rigidBody joint in radians + bool RigidBodySystem::tryGetJointOmegaMaxRad(const std::string& childLink, double& maxOmegaRad) const { + if (!_hasBody) { return false; } + for (const auto& joint : _body.joints) { + if (joint.child == childLink) { + maxOmegaRad = joint.limits.maxqd; + return true; + } + } + return false; + } + + // Method to set the maximum angular velocity of a specific rigidBody joint in radians + bool RigidBodySystem::trySetJointOmegaMaxRad(const std::string& childLink, double maxOmegaRad) { + if (!_hasBody) { return false; } + if (maxOmegaRad <= 0.0f) { return false; } + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + joint.limits.maxqd = maxOmegaRad; + return true; + } + } + return false; + } + + // Method to increment the target angle (reference) of a specific rigidBody joint in radians + bool RigidBodySystem::tryAddJointTargetRad(const std::string& childLink, double deltaRad) { + if (!_hasBody) { return false; } + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + double t = joint.q_ref + deltaRad; + if (joint.limits.continuous) { t = wrapRad(t); } + else { t = std::clamp(t, joint.limits.minAngle, joint.limits.maxAngle); } + joint.q_ref = t; // clamp to joint limits + return true; + } + } + return false; + } + + // Method to set the reference angular velocity of a specific rigidBody joint in radians + bool RigidBodySystem::trySetJointOmegaRefRad(const std::string& childLink, double omegaRefRad) { + if (!_hasBody) { return false; } + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + joint.qd_ref = omegaRefRad; + return true; + } + } + return false; + } + + // Method to set the reference angular acceleration of a specific rigidBody joint in radians + bool RigidBodySystem::trySetJointAlphaRefRad(const std::string& childLink, double alphaRefRad) { + if (!_hasBody) { return false; } + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + joint.qdd_ref = alphaRefRad; + return true; + } + } + return false; + } + + // Method to set the max Omega reference of a specific rigidBody joint in radians + bool RigidBodySystem::trySetJointOmegaRefMaxRad(const std::string& childLink, double maxOmegaRad) { + if (!_hasBody) { return false; } + if (maxOmegaRad <= 0.0f) { return false; } + for (auto& joint : _body.joints) { + if (joint.child == childLink) { + joint.limits.omegaRefMaxRad_s = maxOmegaRad; + return true; + } + } + return false; + } + + // Method to zero the reference derivatives (velocity and acceleration) of a specific rigidBody joint + bool RigidBodySystem::tryZeroJointRefDerivatives() { + if (!_hasBody) { return false; } + for (auto& joint : _body.joints) { + joint.qd_ref = 0.0f; + joint.qdd_ref = 0.0f; + } + return true; + } + + // Method to check if a specific rigidBody joint is at its target angle within a tolerance (radians) + bool RigidBodySystem::isJointAtTargetRad(const std::string& childLink, double tolRad) const { + if (!_hasBody) { return false; } + if (tolRad < 0.0f) { tolRad = -tolRad; } + + for (const auto& joint : _body.joints) { + if (joint.child == childLink) { + double err = joint.q_ref - joint.q; + if (joint.limits.continuous) { err = wrapToPi(err); } + err = std::abs(err); + return err <= tolRad; + } + } + return false; + } + + // Method to check if a specific rigidBody joint is at its target angle within a tolerance (degrees) + bool RigidBodySystem::isJointAtTargetDeg(const std::string& childLink, double tolDeg) const { + return isJointAtTargetRad(childLink, radians(tolDeg)); + } + + // Method to check if a specific rigidBody joint is near a target angle within a tolerance (radians) + bool RigidBodySystem::isJointNearAngleRad(const std::string& childLink, double targetRad, double tolRad) const { + if (!_hasBody) { return false; } + tolRad = std::abs(tolRad); + + for (const auto& joint : _body.joints) { + if (joint.child == childLink) { + double err = targetRad - joint.q; + if (joint.limits.continuous) { err = wrapToPi(err); } + return std::abs(err) <= tolRad; + } + } + return false; + } + + // Method to check if a specific rigidBody joint is near a target angle within a tolerance (degrees) + bool RigidBodySystem::isJointNearAngleDeg(const std::string& childLink, double targetDeg, double tolDeg) const { + return isJointNearAngleRad(childLink, radians(targetDeg), radians(tolDeg)); + } + + // --- ROBOT LINK AND ROOT POSE METHODS --- + + // Method to set the rotation angle of a specific rigidBody link angle in degrees + bool RigidBodySystem::setRigidBodyLinkRotation(const std::string& childLinkName, double angleDeg) { + for (auto& j : _body.joints) { + if (j.child == childLinkName) { + j.q = radians(angleDeg); + computeRigidBodyKinematics(_worldTransforms); + return true; + } + } + return false; + } + + Mat4 RigidBodySystem::setRigidBodyRoot(const Vec3& pos, const Quat& rot) { + Mat4 T = Mat4::Identity(); + T.block<3, 1>(0, 3) = pos; + Mat4 R = Mat4::Identity(); + R.block<3, 3>(0, 0) = rot.toRotationMatrix(); + return T * R; + } + + // Method to set the rigidBody root pose in world coordinates + void RigidBodySystem::setRigidBodyRootPose(const Vec3& pos, const Quat& rot) { + _root_pose = setRigidBodyRoot(pos, rot); + } + + // Method to set the rigidBody root home pose in world coordinates + void RigidBodySystem::setRigidBodyRootHome(const Vec3& pos, const Quat& rot) { + _root_home = setRigidBodyRoot(pos, rot);; + _root_pose = _root_home; + } + + // Method to set the default pose of the rigidBody using joint angles in degrees + bool RigidBodySystem::setDefaultPoseDeg() { + if (!_hasBody) { return false; } + _q_home = _body.makeJointVector(); + _home_valid = true; + return true; + } + + // --- ROBOT BASE INTEGRATION METHODS --- + + // Method to set the default pose of the rigidBody using joint angles in radians + double RigidBodySystem::computeForwardDrive() const { + double drive = 0.0; + for (const auto& j : _body.joints) { + if (j.name.find("hip_pitch") != std::string::npos) { drive += -j.qd; } + } + return drive; + } + + // Method to integrate the base translation of the rigidBody based on leg joint angles (for legged systems) + void RigidBodySystem::integrateBaseTranslation(double dt) { + double hipL = 0.0; + double hipR = 0.0; + + tryGetJointAngleRad("left_hip_pitch_link", hipL); + tryGetJointAngleRad("right_hip_pitch_link", hipR); + + // Positive when left leg is in stance + const double gaitPhase = hipR - hipL; + + // Tunable gain: rad -> N + const double driveGain = 180.0; + double F_forward = -driveGain * gaitPhase; + _lastBaseForwardForce = F_forward; + + Vec3 dampingForce = -_baseLinearDamping * _baseVel; + Vec3 F_world(F_forward, 0.0, 0.0); + F_world += dampingForce; + _baseAcc = F_world / _baseMass; + + _baseVel += _baseAcc * dt; + _basePos += _baseVel * dt; + + LOG_INFO_ONCE("hipL=%.3f hipR=%.3f gaitPhase=%.3f", hipL, hipR, hipR - hipL); + LOG_INFO_ONCE("baseVel = (%.3f, %.3f, %.3f)", _baseVel.x(), _baseVel.y(), _baseVel.z()); + } + + // Method to update the rigidBody root pose based on the integrated base translation (for legged systems) + void RigidBodySystem::updateBaseRootPose() { + Mat4 T = Mat4::Identity(); + T.block<3, 1>(0, 3) = Vec3(_basePos.x(), _basePos.y(), _basePos.z()); + + Mat4 R = Mat4::Identity(); + R.block<3, 3>(0, 0) = Eigen::AngleAxisd(_baseYaw, Vec3(0, 1, 0)).toRotationMatrix(); + + _root_pose = T * R * _root_home; + } + + // --- ROBOT SYSTEM CONFIGURATION METHODS --- + + // Method to set the gravity strength for the rigidBody system + void RigidBodySystem::setGravity(double g) { + _gravity = g; + _dynamics->setGravity(g); + } + + // Set the torque mode for the rigidBody system + void RigidBodySystem::setTorqueMode(eTorqueMode mode) { _body.torqueMode = mode; } + + // Method to claim the current active log buffer for exporting logged data (returns pointer to buffer active before swap) + std::unique_ptr RigidBodySystem::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 + + auto out = std::make_unique(); // create a new buffer to return to caller + out->swap(_logBuffers[prev]); // swap contents of previous active buffer with new buffer + + return out; + } + + // Method to enable or disable the use of internal log buffers for recording joint metrics during simulation + void RigidBodySystem::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 RigidBodySystem::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 systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Systems/SystemLoader.cpp b/DSFE_App/DSFE_Core/src/Systems/SystemLoader.cpp new file mode 100644 index 00000000..07c776cb --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/SystemLoader.cpp @@ -0,0 +1,503 @@ +/* + * File: Systems/SystemLoader.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "Systems/RigidBodyLoader.h" +#include + +#include "EngineLib/LogMacros.h" +#include + +using json = nlohmann::json; +using kinematics::JointType_DH; +using kinematics::DH_Params; + +using namespace constants; + +namespace systems { + // --- Static Helper Functions --- + + // 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(); + } + + // Parse DH joint type from string + static JointType_DH parseDHType(const std::string& s) { + std::string t = s; + for (char& c : t) { c = static_cast(std::tolower((unsigned char)c)); } + if (t == "revolute" || t == "r") { return JointType_DH::Revolute; } + return JointType_DH::Prismatic; + } + + // Read a vec3 from a JSON array + static Vec3 readVec3(const json& j, const char* key, Vec3 fallback = {}) { + if (!j.contains(key) || !j[key].is_array() || j[key].size() != 3) { return fallback; } + return Vec3(j[key][0].get(), j[key][1].get(), j[key][2].get()); + } + + // --- RigidBodyLoader Link and Joint Parsing --- + + // link parsing + + // Parse materials block if present + // Each material is defined as: "materials": { "mat_name": [r, g, b, a] } + static void loadMaterials(const json& data, RigidBodyModel& rigidBody) { + if (!data.contains("material")) { return; } + const auto& mat = data["material"]; + + // New nested format: "material" -> "Color" -> { name: [r,g,b,a] } + if (mat.contains("Color") && mat["Color"].is_object()) { + for (auto& [name, col] : mat["Color"].items()) { + if (col.is_array() && col.size() == 4) { + rigidBody.materials[name] = Vec4( + col[0].get(), + col[1].get(), + col[2].get(), + col[3].get() + ); + } + else { + LOG_WARN("Material Color '%s' has invalid format, expected array of 4 floats", name.c_str()); + } + } + } + else { + // Legacy flat format: "material" -> { name: [r,g,b,a] } + for (auto& [name, col] : mat.items()) { + if (col.is_array() && col.size() == 4) { + rigidBody.materials[name] = Vec4( + col[0].get(), + col[1].get(), + col[2].get(), + col[3].get() + ); + } + } + } + } + + // Helper to parse material properties from a JSON object into color/metallic/roughness + static void parseMaterialObject(const json& m, const std::unordered_map& materials, const std::string& context, + Vec4& outColor, float& outMetallic, float& outRoughness) { + if (m.contains("Color") && m["Color"].is_string()) { + const std::string colorName = m["Color"].get(); + auto it = materials.find(colorName); + if (it != materials.end()) { + outColor = it->second; + } + else { + LOG_WARN("%s references undefined color '%s', using default Grey", context.c_str(), colorName.c_str()); + outColor = Vec4(0.4, 0.4, 0.4, 1.0); + } + } + if (m.contains("Metallic") && m["Metallic"].is_number()) { + outMetallic = m["Metallic"].get(); + } + if (m.contains("Roughness") && m["Roughness"].is_number()) { + outRoughness = m["Roughness"].get(); + } + } + + // Parse visual geometry, supporting both single mesh and multiple meshes, as well as material properties + static void parseVisual(const json& linkData, const std::unordered_map& materials, RigidBodyLink& link) { + if (!linkData.contains("visual")) { return; } + const auto& v = linkData["visual"]; + + // Visual geometry origin + link.visual.origin_xyz = readVec3(v, "origin_xyz", link.visual.origin_xyz); + link.visual.origin_rpy = readVec3(v, "origin_rpy", link.visual.origin_rpy); + + // Single Mesh + if (v.contains("mesh") && v["mesh"].is_string()) { + VisualMeshEntry entry; + entry.meshFile = v["mesh"].get(); + link.visual.meshEntries.push_back(entry); + } + + // Multiple meshes + if (v.contains("meshes") && v["meshes"].is_array()) { + for (const auto& m : v["meshes"]) { + if (!m.is_string()) { + LOG_WARN("Invalid mesh entry in link %s (expected string)", link.name.c_str()); + continue; + } + + VisualMeshEntry entry; + entry.meshFile = m.get(); + link.visual.meshEntries.push_back(entry); + } + } + + Vec4 material{ 0.7, 0.0, 0.2, 1.0 }; // Default material if not specified + float metallic = 0.5f; + float roughness = 0.5f; + bool hasMaterial = false; + + // Material assignement + if (v.contains("material") && v["material"].is_object()) { + parseMaterialObject(v["material"], materials, "Link " + link.name, + material, metallic, roughness); + hasMaterial = true; + } + else if (v.contains("material") && v["material"].is_string()) { + const std::string matName = v["material"].get(); + auto it = materials.find(matName); + + if (it != materials.end()) { + material = it->second; + hasMaterial = true; + } + else { + LOG_WARN("Link %s references undefined material '%s', using default colour", link.name.c_str(), matName.c_str()); + } + } + + // Assign material properties to all mesh entries + for (auto& entry : link.visual.meshEntries) { + if (hasMaterial) { + entry.material = material; + entry.metallic = metallic; + entry.roughness = roughness; + entry.hasMaterial = hasMaterial; + } + } + } + + // Parse collision geometry material properties + static void parseCollisionMaterial(const json& collisionData, const RigidBodyModel& rigidBody, CollisionShape& shape) { + if (!collisionData.contains("material")) { return; } + const auto& m = collisionData["material"]; + + if (m.is_object()) { + parseMaterialObject(m, rigidBody.materials, "Collision", + shape.material, shape.metallic, shape.roughness); + } + } + + // Parse collision geometry, supporting multiple collision shapes per link + static void parseCollisions(const json& linkData, const RigidBodyModel& rigidBody, RigidBodyLink& link) { + if (!linkData.contains("collision")) { return; } + for (const auto& c : linkData["collision"]) { + if (!linkData["collision"].is_array()) { + LOG_WARN("Collision block is not an array in link %s", link.name.c_str()); + return; + } + + CollisionShape s; + s.type = c.value("type", ""); + s.origin_xyz = readVec3(c, "origin_xyz", s.origin_xyz); + s.origin_rpy = readVec3(c, "origin_rpy", s.origin_rpy); + + if (s.type == "cylinder") { + s.size.x() = c.value("radius", 0.0f); // radius + s.size.y() = c.value("length", 0.0f); // length + } + else if (s.type == "box") { s.size = readVec3(c, "size", s.size); } + else if (s.type == "mesh") { s.meshFile = c.value("mesh", ""); } + + parseCollisionMaterial(c, rigidBody, s); + link.collisions.push_back(s); + } + } + + // Parse inertial properties, including mass, center of mass, and inertia tensor + static void parseInertial(const json& linkData, RigidBodyLink& link) { + if (!linkData.contains("inertial")) { return; } + const auto& I = linkData["inertial"]; + link.inertial.mass = I.value("mass", link.inertial.mass); + link.inertial.com_xyz = readVec3(I, "com_xyz", link.inertial.com_xyz); + + if (I.contains("inertia")) { + const auto& J = I["inertia"]; + link.inertial.inertia.ixx = J.value("ixx", link.inertial.inertia.ixx); + link.inertial.inertia.ixy = J.value("ixy", link.inertial.inertia.ixy); + link.inertial.inertia.ixz = J.value("ixz", link.inertial.inertia.ixz); + link.inertial.inertia.iyy = J.value("iyy", link.inertial.inertia.iyy); + link.inertial.inertia.iyz = J.value("iyz", link.inertial.inertia.iyz); + link.inertial.inertia.izz = J.value("izz", link.inertial.inertia.izz); + } + } + + // joint parsing + + // Parse joint origin, supporting both the "origin" block (with "origin_xyz" and "origin_rpy" inside) and the flat format with "origin_xyz" and "origin_rpy" directly in the joint block + static void parseJointOrigin(const json& jointData, RigidBodyJoint& joint) { + if (jointData.contains("origin")) { + const auto& o = jointData["origin"]; + joint.origin_xyz = readVec3(o, "origin_xyz", joint.origin_xyz); + joint.origin_rpy = readVec3(o, "origin_rpy", joint.origin_rpy); + } else { + joint.origin_xyz = readVec3(jointData, "origin_xyz", Vec3::Zero()); + joint.origin_rpy = readVec3(jointData, "origin_rpy", Vec3::Zero()); + } + joint.origin_q = rpyRadToQuat(joint.origin_rpy); + } + + // Parse joint axis, supporting both the "axis" block (with "axis_xyz" inside) and the flat format with "axis" directly in the joint block + static void parseJointAxis(const json& jointData, RigidBodyJoint& joint) { + joint.axis = Vec3(0.0f, 0.0f, 1.0f); // default axis + if (jointData.contains("axis") && jointData["axis"].is_array() && jointData["axis"].size() == 3) { + const auto& a = jointData["axis"]; + joint.axis = Vec3( + a[0].get(), + a[1].get(), + a[2].get() + ); + if (joint.axis.norm() < 1e-6f) { + LOG_WARN("Joint %s has zero-length axis, defaulting to (0,0,1)", joint.name.c_str()); + joint.axis = Vec3(0.0f, 0.0f, 1.0f); + } + else { joint.axis.normalize(); } + } + + // Parse joint type (e.g., "revolute", "prismatic") if provided. + if (jointData.contains("type") && jointData["type"].is_string()) { + const std::string typeStr = jointData["type"].get(); + if (typeStr == "revolute" || typeStr == "REVOLUTE") { + joint.type = eJointType::REVOLUTE; + } + else if (typeStr == "prismatic" || typeStr == "PRISMATIC") { + joint.type = eJointType::PRISMATIC; + } + else if (typeStr == "fixed" || typeStr == "FIXED") { + joint.type = eJointType::FIXED; + } + else if (typeStr == "free" || typeStr == "FREE") { + joint.type = eJointType::FREE; + } + else { + LOG_WARN("Joint %s has unknown type '%s', defaulting to REVOLUTE", joint.name.c_str(), typeStr.c_str()); + } + } + } + + // Parse joint limits, including continuous revolute joints and prismatic joints + static void parseJointLimits(const json& jointData, RigidBodyJoint& joint) { + joint.limits.continuous = false; + joint.limits.minAngle = 0.0f; + joint.limits.maxAngle = 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; } + + const auto& L = jointData["limits"]; + + joint.limits.continuous = L.value("continuous", false); + joint.limits.maxqd = L.value("velocity", joint.limits.maxqd); + joint.limits.maxEffort = L.value("effort", joint.limits.maxEffort); + + if (!joint.limits.continuous) { + if (L.contains("lower") && L["lower"].is_number()) { joint.limits.minAngle = L["lower"].get(); } + else { LOG_WARN("Joint %s limits missing 'lower'", joint.name.c_str()); } + + if (L.contains("upper") && L["upper"].is_number()) { joint.limits.maxAngle = L["upper"].get(); } + else { LOG_WARN("Joint %s limits missing 'upper'", joint.name.c_str()); } + + if (joint.limits.maxAngle < joint.limits.minAngle) { + LOG_WARN("Joint %s has upper < lower (swapping).", joint.name.c_str()); + std::swap(joint.limits.minAngle, joint.limits.maxAngle); + } + } + else { joint.limits.minAngle = -3.14159265f; joint.limits.maxAngle = 3.14159265f; } + } + + // Parse joint dynamics parameters + static void parseJointDynamics(const json& jointData, RigidBodyJoint& joint) { + joint.dynamics.damping = 0.0; + joint.dynamics.friction = 0.0; + + if (!jointData.contains("dynamics") || !jointData["dynamics"].is_object()) { LOG_ERROR("Could not find joint dynamic data"); return; } + + const auto& D = jointData["dynamics"]; + joint.dynamics.damping = D.value("damping", joint.dynamics.damping); + joint.dynamics.friction = D.value("friction", joint.dynamics.friction); + joint.wn_target = D.value("wn_target", joint.wn_target); + joint.zeta_target = D.value("zeta_target", joint.zeta_target); + + if (joint.dynamics.damping < 0.0) { joint.dynamics.damping = 0.0; } + if (joint.dynamics.friction < 0.0) { joint.dynamics.friction = 0.0; } + } + + // Check if a joint is fixed based on its type string + static bool isFixedJoint(const json& jointData) { + if (!jointData.contains("type")) return false; + const std::string t = jointData["type"].get(); + return (t == "fixed" || t == "FIXED"); + } + + // Parse DH parameters if present + static bool parseDHParameters(const json& jointData, DH_Params& out) { + // Accept "dh" ONLY (your JSON uses "dh") + if (!jointData.contains("dh") || !jointData["dh"].is_object()) return false; + + const auto& dh = jointData["dh"]; + out.a = dh.value("a", 0.0); + out.alpha = dh.value("alpha", 0.0); + out.d = dh.value("d", 0.0); + out.theta = dh.value("theta0", 0.0); + out.type = parseDHType(dh.value("type", "revolute")); + return true; + } + + // Decide kinematics model based on presence of DH parameters + static eKinematicsModel decideKinematicsModel(const json& data) { + if (!data.contains("joints") || !data["joints"].is_array()) { return eKinematicsModel::URDF; } // no joints -> URDF + for (const auto& jointData : data["joints"]) { + if (!jointData.contains("dh") || !jointData["dh"].is_object()) { return eKinematicsModel::URDF; } // any missing -> URDF + } + return eKinematicsModel::DH; + } + + // --- RigidBodyLoader Loading, Public API --- + + RigidBodyModel RigidBodyLoader::loadFromJSON(const std::string& filepath) { + RigidBodyModel rigidBody; + LOG_INFO("Loading rigidBody model from JSON: %s", filepath.c_str()); + D_INFO("Loading rigidBody model from JSON: %s", filepath.c_str()); + + std::ifstream file(filepath); + if (!file.is_open()) { + LOG_ERROR("Failed to open JSON file: %s", filepath.c_str()); + D_FAIL("Failed to open JSON file: %s", filepath.c_str()); + return rigidBody; + } + json data = json::parse(file); + + rigidBody.name = data["name"].get(); + loadMaterials(data, rigidBody); + + // Visual frame (optional, defaults to JOINT) + if (data.contains("visual_frame")) { + const std::string vf = data["visual_frame"].get(); + if (vf == "joint") { rigidBody.visualFrame = eVisualFrame::JOINT; } + else if (vf == "link") { rigidBody.visualFrame = eVisualFrame::LINK; } + else if (vf == "world") { rigidBody.visualFrame = eVisualFrame::WORLD; } + else { LOG_WARN("Unknown visual_frame '%s', defaulting to JOINT", vf.c_str()); } + } + else { + rigidBody.visualFrame = eVisualFrame::JOINT; + } + + // Load rigidBody scale (default 1.0) + rigidBody.scale = data.value("scale", 1.0f); + + // Load base frame if present + if (data.contains("base_frame")) { + rigidBody.baseFrameIsEngineAligned = false; + const auto& bf = data["base_frame"]; + + // Read translation and rotation (RPY) from JSON, with defaults + Vec3 t = readVec3(bf, "origin_xyz", Vec3::Zero()); + Vec3 r = readVec3(bf, "origin_rpy", Vec3::Zero()); + Quat q = rpyRadToQuat(r); + + rigidBody.baseFrame = Mat4::Identity(); + rigidBody.baseFrame.block<3, 3>(0, 0) = q.toRotationMatrix(); + rigidBody.baseFrame.block<3, 1>(0, 3) = t; + + LOG_INFO("Base frame loaded from JSON: translation=(%.3f, %.3f, %.3f), rotation_rpy=(%.3f, %.3f, %.3f)", + t.x(), t.y(), t.z(), + r.x(), r.y(), r.z()); + } + else { + rigidBody.baseFrameIsEngineAligned = true; + rigidBody.baseFrame = Mat4::Identity(); + LOG_INFO("No base frame specified in JSON, using identity (engine-aligned) by default."); + } + + if (rigidBody.name == "Z1") { + rigidBody.baseFrameIsEngineAligned = true; + } + + // Load links + for (auto& linkData : data["links"]) { + RigidBodyLink link; + link.name = linkData.value("name", ""); + + parseVisual(linkData, rigidBody.materials, link); + parseCollisions(linkData, rigidBody, link); + parseInertial(linkData, link); + + rigidBody.links.push_back(link); + + LOG_INFO("Link: %s | Mass: %.2f", link.name.c_str(), link.inertial.mass); + D_INFO("Link: %s | Mass: %.2f", link.name.c_str(), link.inertial.mass); + } + + // Decide kinematics model + rigidBody.kinematicsModel = decideKinematicsModel(data); + + // Load joints + for (auto& jointData : data["joints"]) { + RigidBodyJoint joint; + + // Load basic joint info + joint.name = jointData["name"].get(); + joint.parent = jointData["parent"].get(); + joint.child = jointData["child"].get(); + + parseJointOrigin(jointData, joint); + + // If it's a fixed joint, we can skip axis/limits/dynamics/control parsing and just set defaults. + if (isFixedJoint(jointData)) { + joint.type = eJointType::FIXED; + joint.axis = Vec3::Zero(); + joint.limits.continuous = false; + joint.limits.minAngle = 0.0; + joint.limits.maxAngle = 0.0; + } + else { + parseJointAxis(jointData, joint); + parseJointLimits(jointData, joint); + parseJointDynamics(jointData, joint); + } + + rigidBody.joints.push_back(joint); + + // If rigidBody is DH-mode, also parse DH table + if (rigidBody.kinematicsModel == eKinematicsModel::DH) { + DH_Params dh{}; + if (!parseDHParameters(jointData, dh)) { + LOG_WARN("Joint %s missing 'dh' unexpectedly; forcing URDF mode.", joint.name.c_str()); + rigidBody.kinematicsModel = eKinematicsModel::URDF; + rigidBody.dhParams.clear(); + } + else { + rigidBody.dhParams.push_back(dh); + } + } + + if (abs(joint.limits.minAngle) == abs(joint.limits.maxAngle) && !joint.limits.continuous) { + LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); + D_INFO("Joint: %s | Parent: %s, | Child: %s, | Max Speed: %.2f, | Angle Limit: +-%.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.maxqd, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); + } + else { + LOG_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: %.2f, | Friction: %.2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction); + D_INFO("Joint: %s | Parent: %s, | Child: %s, | Continuous: %s, | Max Speed: %.2f, | Min Angle: %.2f, | Max Angle: %.2f | Dampling: % .2f, | Friction : % .2f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), joint.limits.continuous ? "True" : "False", joint.limits.maxqd, joint.limits.minAngle, joint.limits.maxAngle, joint.dynamics.damping, joint.dynamics.friction);; + } + } + + if (rigidBody.kinematicsModel == eKinematicsModel::URDF) { rigidBody.dhParams.clear(); } + + LOG_INFO("RigidBody loaded: %d links, %d joints", (int)rigidBody.links.size(), (int)rigidBody.joints.size()); + D_SUCCESS("RigidBody loaded: %d links, %d joints", (int)rigidBody.links.size(), (int)rigidBody.joints.size()); + + return rigidBody; + } +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Systems/TrajectoryManager.cpp b/DSFE_App/DSFE_Core/src/Systems/TrajectoryManager.cpp new file mode 100644 index 00000000..89df0cec --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/TrajectoryManager.cpp @@ -0,0 +1,77 @@ +/* + * File: Systems/TrajectoryManager.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" +#include "Systems/TrajectoryManager.h" +#include "Systems/RigidBodySystem.h" + +#include +#include +#include + +namespace control { + // Clear trajectory for a specific rigidBody link + void TrajectoryManager::clear(const std::string& link) { _active.erase(link); } + + // Clears all active trajectories + void TrajectoryManager::clearAll() { _active.clear(); } + + // Evaluate the trajectory for a specific rigidBody link at time t, returning the desired state in out + bool TrajectoryManager::tryEval(const std::string& link, double t, control::TrajState& out) const { + auto it = _active.find(link); + if (it == _active.end()) { return false; } + const auto r = it->second->eval(t); + out.q = r.q; + out.qd = r.qd; + out.qdd = r.qdd; + return true; + } + + // Check if a trajectory is active for a specific rigidBody link + bool TrajectoryManager::hasActive(const std::string& link) const { + auto it = _active.find(link); + return (it != _active.end() && it->second); + } + + + // Set a trajectory for a specific rigidBody link + void TrajectoryManager::set(const std::string& link, std::unique_ptr traj) { + if (!traj) { + _active.erase(link); + return; + } + _active.insert_or_assign(link, std::move(traj)); + } + + // Apply active trajectories to the rigidBody at time t + void TrajectoryManager::apply(systems::RigidBodySystem& rigidBody, double t) { + for (auto it = _active.begin(); it != _active.end();) { + const std::string& link = it->first; + auto& traj = it->second; + + // Evaluate trajectory at time t + const auto ref = traj->eval(t); + + // Apply trajectory reference to rigidBody joint + const bool ok1 = rigidBody.trySetJointTargetRad(link, (float)ref.q); + if (!ok1) { D_ERROR("Bad link key '%s' (no joint.child match)", link.c_str()); } + + const bool ok2 = rigidBody.trySetJointOmegaRefRad(link, (float)ref.qd); + if (!ok2) { D_ERROR("Bad link key '%s' (no joint.child match)", link.c_str()); } + + const bool ok3 = rigidBody.trySetJointAlphaRefRad(link, (float)ref.qdd); + if (!ok3) { D_ERROR("Bad link key '%s' (no joint.child match)", link.c_str()); } + + // Remove finished trajectories + if (traj->finished(t)) { + D_WARN("Trajectory finished immediately: link='%s' t=%.6f", link.c_str(), t); + it = _active.erase(it); + } + else { + ++it; + } + + } + } +} // namespace control \ No newline at end of file From 18c2fb47f60bbbdc892e0a12a5f51899780d797c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 17:24:31 +0100 Subject: [PATCH 005/114] refator: Updated GUI files to use `RigidBody` rather than `Robot` for new naming format --- DSFE_App/DSFE_GUI/CMakeLists.txt | 8 +-- .../MainWindow/Widgets/ControlPanelWidget.h | 4 +- .../MainWindow/Widgets/DSLEditorWidget.h | 10 ++-- .../include/MainWindow/Workspace/Workspace.h | 4 +- .../include/Simulation/SimulationManager.h | 7 ++- .../include/Systems/MultiBodySystem.h | 10 ++-- .../RigidBodyBinding.h} | 2 +- .../RigidBodyPresentationBuilder.h} | 10 ++-- .../RigidBodyRenderer.h} | 18 +++---- .../MainWindow/Widgets/ControlPanelWidget.cpp | 22 ++++---- .../MainWindow/Widgets/DSLEditorWidget.cpp | 2 +- .../src/MainWindow/Workspace/Workspace.cpp | 4 +- .../src/Simulation/SimulationManager.cpp | 50 +++++++++--------- .../DSFE_GUI/src/Systems/MultiBodySystem.cpp | 10 ++-- .../RigidBodyPresentationBuilder.cpp} | 11 ++-- .../RigidBodyRenderer.cpp} | 51 +++++++------------ 16 files changed, 97 insertions(+), 126 deletions(-) rename DSFE_App/DSFE_GUI/include/{Robots/RobotBinding.h => Systems/RigidBodyBinding.h} (91%) rename DSFE_App/DSFE_GUI/include/{Robots/RobotPresentationBuilder.h => Systems/RigidBodyPresentationBuilder.h} (62%) rename DSFE_App/DSFE_GUI/include/{Robots/RobotRenderer.h => Systems/RigidBodyRenderer.h} (55%) rename DSFE_App/DSFE_GUI/src/{Robots/RobotPresentationBuilder.cpp => Systems/RigidBodyPresentationBuilder.cpp} (87%) rename DSFE_App/DSFE_GUI/src/{Robots/RobotRenderer.cpp => Systems/RigidBodyRenderer.cpp} (73%) diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index ba03493f..5c78cd93 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -118,6 +118,8 @@ set(SIMULATION_SRC ) set(SYSTEMS_SRC + src/Systems/RigidBodyPresentationBuilder.cpp + src/Systems/RigidBodyRenderer.cpp src/Systems/SingleBodySystem.cpp src/Systems/MultiBodySystem.cpp ) @@ -140,11 +142,6 @@ set(WIDGETS_SRC include/MainWindow/DSL/DSLSyntaxHighlighter.h ) -set(ROBOT_SRC - src/Robots/RobotPresentationBuilder.cpp - src/Robots/RobotRenderer.cpp -) - set(OBJECT_SRC src/Assets/MeshPresentationBuilder.cpp ) @@ -179,7 +176,6 @@ target_sources(DSFE_GUI PRIVATE ${SIMULATION_SRC} ${SYSTEMS_SRC} ${WIDGETS_SRC} - ${ROBOT_SRC} ${OBJECT_SRC} ${PLATFORM_SRC} ${ASSETS_SRC} diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 963ad0f4..17a89d23 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -21,7 +21,7 @@ namespace render { enum class QualityPreset; } -namespace robots { class RobotSystem; } +namespace systems { class RigidBodySystem; } namespace diagnostics { class TelemetryRecorder; struct JointTelemetry; } namespace gui { class SimulationManager; } @@ -154,7 +154,7 @@ namespace widgets { bool _jointSelected = false; bool diagRunning = false; bool _robotRequested = false; - bool _hasRobot = false; + bool _hasBody = false; bool _openStats = true; bool _useAutoDiff = false; // Simulation and diagnostics timing diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h index b2009fbb..15256f57 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h @@ -12,7 +12,7 @@ #include #include -#include "Interpreter/RunWrapper.h" +#include "DSL/RunWrapper.h" class QTextEdit; class QLabel; @@ -21,7 +21,7 @@ class QTabWidget; class QTimer; namespace gui { class SimulationManager; } -namespace interpreter { +namespace DSL { class Parser; class IStoredProgram; } @@ -57,9 +57,9 @@ namespace widgets { std::vector _activeRuns; // Vector to hold active runs and their futures gui::SimulationManager* _sim = nullptr; - interpreter::Parser* _parser = nullptr; - interpreter::IStoredProgram* _program = nullptr; - interpreter::RunWrapper* _wrapper = nullptr; + DSL::Parser* _parser = nullptr; + DSL::IStoredProgram* _program = nullptr; + DSL::RunWrapper* _wrapper = nullptr; ConsoleOutputWidget* _log = nullptr; DSLSyntaxHighlighter* _highlighter = nullptr; diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h index 5ef9323b..ff8736dc 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h @@ -13,14 +13,14 @@ namespace gui { int version = 1; QString name; - QString robotName; // empty = no robot + QString rigidBodyName; // empty = no rigid body loaded QString scriptText; // DSL script embedded — file is self-contained QString scriptPath; // original script file if one was opened (informational) // Simulation properties int integrationMethod = 0; // integration::eIntegrationMethod as int int adIntegrationMethod = 0; // integration::eAutoDiffIntegrationMethod as int - double simDt = 1.0 / 180.0; + double simDt = 1.0 / 180.0; double telemetryDt = 1.0 / 180.0; bool autoDiff = false; diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h index db079534..3d315774 100644 --- a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h @@ -41,8 +41,8 @@ namespace scene { namespace core { class ISimulationCore; } // Forward Declarations for Physics, Robots, Control, and Integration -namespace interpreter { class IStoredProgram; } -namespace robots { class RobotSystem; struct RobotModel; } +namespace dsl { class IStoredProgram; } +namespace systems { class RigidBodySystem; struct RigidBodyModel; } namespace control { class TrajectoryManager; } namespace integration { enum class eIntegrationMethod; } @@ -51,8 +51,7 @@ namespace gui { enum class ViewID { Manual = 0, Top, Right, Front, Follow, COUNT }; class SimulationRenderer; - //class SimulationSystemController; - + // Forward Declarations for eKeyCode enum class eKeyCode; diff --git a/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h b/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h index 9243390c..3abc37ac 100644 --- a/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h +++ b/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h @@ -2,7 +2,7 @@ #pragma once #include "Systems/ISimulationSystem.h" -#include "Robots/RobotBinding.h" +#include "Systems/RigidBodyBinding.h" #include #include @@ -12,16 +12,16 @@ #include "Platform/Logger.h" -namespace robots { struct RobotModel; } +namespace systems { struct RigidBodyModel; } namespace assets { class MeshLoader; } namespace gui { class MeshStore; class SimulationRenderer; - class MultiBodySystem : public ISimulationSystem { + class RigidBodySystem : public ISimulationSystem { public: - MultiBodySystem(const robots::RobotModel& model, + RigidBodySystem(const robots::RigidBodyModel& model, std::function&()> world_src, MeshStore& mesh_store, SimulationRenderer& renderer); @@ -30,7 +30,7 @@ namespace gui { void clear(SimulationScene& scene) override; private: - const robots::RobotModel& _model; + const robots::RigidBodyModel& _model; std::function&()> _world_src; MeshStore& _meshStore; SimulationRenderer& _renderer; diff --git a/DSFE_App/DSFE_GUI/include/Robots/RobotBinding.h b/DSFE_App/DSFE_GUI/include/Systems/RigidBodyBinding.h similarity index 91% rename from DSFE_App/DSFE_GUI/include/Robots/RobotBinding.h rename to DSFE_App/DSFE_GUI/include/Systems/RigidBodyBinding.h index b71a514b..7232efb6 100644 --- a/DSFE_App/DSFE_GUI/include/Robots/RobotBinding.h +++ b/DSFE_App/DSFE_GUI/include/Systems/RigidBodyBinding.h @@ -6,7 +6,7 @@ #include namespace gui { - struct RobotBinding { + struct RigidBodyBinding { std::unordered_map> link_to_renderables; void clear() { link_to_renderables.clear(); } }; diff --git a/DSFE_App/DSFE_GUI/include/Robots/RobotPresentationBuilder.h b/DSFE_App/DSFE_GUI/include/Systems/RigidBodyPresentationBuilder.h similarity index 62% rename from DSFE_App/DSFE_GUI/include/Robots/RobotPresentationBuilder.h rename to DSFE_App/DSFE_GUI/include/Systems/RigidBodyPresentationBuilder.h index 34295ca7..36783ff0 100644 --- a/DSFE_App/DSFE_GUI/include/Robots/RobotPresentationBuilder.h +++ b/DSFE_App/DSFE_GUI/include/Systems/RigidBodyPresentationBuilder.h @@ -1,4 +1,4 @@ -// DSFE_GUI RobotPresentationBuilder.h +// DSFE_GUI RigidBodyPresentationBuilder.h #pragma once #include @@ -7,14 +7,14 @@ #include namespace scene { class Object; } -namespace robots { struct RobotModel; } +namespace systems { struct RigidBodyModel; } -struct RobotRenderBinding { +struct RigidBodyRenderBinding { std::vector> ownedObjects; // Objects owned by this binding (for proper memory management) std::unordered_map> linkVisuals; // Map from link names to their visual objects }; -class RobotPresentationBuilder { +class RigidBodyPresentationBuilder { public: - static RobotRenderBinding build(const robots::RobotModel& model); + static RigidBodyRenderBinding build(const systems::RigidBodyModel& model); }; \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/include/Robots/RobotRenderer.h b/DSFE_App/DSFE_GUI/include/Systems/RigidBodyRenderer.h similarity index 55% rename from DSFE_App/DSFE_GUI/include/Robots/RobotRenderer.h rename to DSFE_App/DSFE_GUI/include/Systems/RigidBodyRenderer.h index 477f4b68..41d60891 100644 --- a/DSFE_App/DSFE_GUI/include/Robots/RobotRenderer.h +++ b/DSFE_App/DSFE_GUI/include/Systems/RigidBodyRenderer.h @@ -1,4 +1,4 @@ -// DSFE_GUI RobotRenderer.h +// DSFE_GUI RigidBodyRenderer.h #pragma once #include @@ -9,24 +9,20 @@ #include namespace scene { class Object; } -namespace robots { struct RobotModel; } +namespace systems { struct RigidBodyModel; } struct LinkRenderData { std::vector visuals; // Visual objects associated with this link std::vector collisions; // Collision objects associated with this link (not implemented yet) }; -struct RobotRenderBinding; +struct RigidBodyRenderBinding; -class RobotRenderer { +class RigidBodyRenderer { public: - //using spawnFn = std::function(const std::string&)>; // function type for loading meshes - - /*void instantiateRobotLinks(const robots::RobotModel& robot);*/ - void bind(const RobotRenderBinding& binding); - void applyTransforms(const robots::RobotModel& robot, const std::vector& world); - - void clearRobotModel(const robots::RobotModel& robot); + void bind(const RigidBodyRenderBinding& binding); + void applyTransforms(const systems::RigidBodyModel& robot, const std::vector& world); + void clearRigidBodyModel(const systems::RigidBodyModel& robot); private: struct linkRenderData { diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 63164a25..c8083377 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -20,7 +20,7 @@ #include "Scene/Camera.h" #include "Simulation/SimulationManager.h" -#include "Robots/RobotSystem.h" +#include "Systems/RigidBodySystem.h" #include "Analysis/Telemetry.h" #include "Platform/Paths.h" @@ -151,7 +151,7 @@ namespace widgets { void ControlPanelWidget::jointInfoPanel() { if (!_jointInfoGroup) { - _jointInfoGroup = new QGroupBox("Robot Joint Information"); + _jointInfoGroup = new QGroupBox("RigidBody Joint Information"); auto* layout = new QVBoxLayout(_jointInfoGroup); _jointInfoGroup->setLayout(layout); layout->addWidget(new QLabel("Joint")); @@ -169,13 +169,13 @@ namespace widgets { _contentLayout->addWidget(_jointInfoGroup); } - if (!_sim || !_sim->hasRobot()) { + if (!_sim || !_sim->hasRigidBody()) { _jointInfoGroup->setVisible(false); return; } - auto& rs = _sim->robotSystem(); - auto& joints = rs.joints(); - auto& links = rs.links(); + auto& body = _sim->rigidBodySystem(); + auto& joints = body.joints(); + auto& links = body.links(); _jointInfoGroup->setVisible(!joints.empty() && !links.empty()); if (joints.empty() || links.empty()) { _jointInfoGroup->setVisible(false); return; } @@ -352,11 +352,11 @@ namespace widgets { } void ControlPanelWidget::selectJointAndFollow(int jointIdx) { - if (!_sim || !_sim->hasRobot()) { return; } + if (!_sim || !_sim->hasRigidBody()) { return; } - auto& rs = _sim->robotSystem(); - auto& joints = rs.joints(); - auto& links = rs.links(); + auto& body = _sim->rigidBodySystem(); + auto& joints = body.joints(); + auto& links = body.links(); if (joints.empty()) { return; } jointIdx = std::clamp(jointIdx, 0, (int)joints.size() - 1); @@ -368,7 +368,7 @@ namespace widgets { _selection.index = jointIdx; _selection.source = SelectionSource::CONTROL_PANEL; - _sim->followRobotJoint(_currentJointName, glm::vec3(0.0f, 0.2f, 0.6f)); + _sim->followRigidBodyJoint(_currentJointName, glm::vec3(0.0f, 0.2f, 0.6f)); } void ControlPanelWidget::updateSimClock() { diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index 5bd9a201..cd27ef30 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -15,7 +15,7 @@ #include "Platform/Paths.h" #include "Numerics/IntegrationMethods.h" -#include "Interpreter/StoredProgram.h" +#include "DSL/StoredProgram.h" #include "Widgets/ConsoleOutputWidget.h" #include "DSL/DSLSyntaxHighlighter.h" diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp index 66040898..017e7c4f 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp @@ -12,7 +12,7 @@ namespace gui { o["name"] = name; QJsonObject content; - content["robot"] = robotName; + content["rigid_body"] = rigidBodyName; content["script_text"] = scriptText; content["script_path"] = scriptPath; o["content"] = content; @@ -40,7 +40,7 @@ namespace gui { w.name = o["name"].toString(); const QJsonObject content = o["content"].toObject(); - w.robotName = content["robot"].toString(); + w.rigidBodyName = content["rigid_body"].toString(); w.scriptText = content["script_text"].toString(); w.scriptPath = content["script_path"].toString(); diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index fa806431..43a3fed3 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -8,8 +8,8 @@ #include "Assets/MeshLoader.h" #include "Scene/Mesh.h" -#include "Robots/RobotModel.h" -#include "Robots/RobotSystem.h" +#include "Systems/RigidBodyModel.h" +#include "Systems/RigidBodySystem.h" #include "SingleBodySystems/SingleBodySystem.h" #include "Platform/ISimulationCore.h" @@ -122,36 +122,36 @@ namespace gui { // ---------------- Systems ---------------- - const bool SimulationManager::hasRobot() const { return _core && _core->hasRobot(); } - robots::RobotSystem& SimulationManager::robotSystem() { return _core->robotSystem(); } - bool SimulationManager::followRobotJoint(const std::string&, const glm::vec3&) { return false; } + const bool SimulationManager::hasRigidBody() const { return _core && _core->hasRigidBody(); } + systems::RigidBodySystem& SimulationManager::rigidBodySystem() { return _core->rigidBodySystem(); } + bool SimulationManager::followRigidBodyJoint(const std::string&, const glm::vec3&) { return false; } - void SimulationManager::load_robot(const std::string& name) { + void SimulationManager::load_rigidBody(const std::string& name) { if (!_core) { - LOG_ERROR("Simulation core not initialised, cannot load robot"); + LOG_ERROR("Simulation core not initialised, cannot load rigidBody"); return; } if (!_rendererInitialised) { - LOG_ERROR("Renderer not initialised, cannot load robot"); + LOG_ERROR("Renderer not initialised, cannot load rigidBody"); return; } - _core->loadRobot(name); - if (!_core->hasRobot()) { - LOG_ERROR("Failed to load robot: %s", name.c_str()); + _core->loadRigidBody(name); + if (!_core->hasRigidBody()) { + LOG_ERROR("Failed to load rigidBody: %s", name.c_str()); return; } - const auto& model = _core->robotSystem().model(); + const auto& model = _core->rigidBodySystem().model(); auto world_src = [this]() -> const std::vector& { - return _core->robotSystem().worldTransforms(); + return _core->rigidBodySystem().worldTransforms(); }; _systems.add(std::make_unique(model, world_src, _mesh_store, *_sim_renderer), _scene); - _core->clearRobotPresentationDirty(); - _currentRobotName = model.name; - LOG_INFO("Robot loaded: %s", model.name.c_str()); + _core->clearRigidBodyPresentationDirty(); + _currentRigidBodyName = model.name; + LOG_INFO("RigidBody loaded: %s", model.name.c_str()); } - void SimulationManager::clearRobot() { _systems.clear_all(_scene); _scene.clear(); } + void SimulationManager::clearRigidBody() { _systems.clear_all(_scene); _scene.clear(); } // -------------------------------------------------- // SIMULATION TICK & RENDER @@ -214,7 +214,7 @@ namespace gui { // Start the simulation void SimulationManager::startSimulation() { - if (!hasRobot()) { LOG_WARN("Cannot start simulation: no robot loaded"); return; } + if (!hasRigidBody()) { LOG_WARN("Cannot start simulation: no rigidBody loaded"); return; } _core->startSimulation(); } @@ -296,7 +296,7 @@ namespace gui { // Run a script to completion synchronously with a specific integrator bool SimulationManager::runScriptToCompletion(const std::string& scriptText, integration::eIntegrationMethod method) { - if (!hasRobot()) { return false; } + if (!hasRigidBody()) { return false; } // Map method enum to string name static const char* names[] = { "euler", "midpoint", "heun", "ralston", "rk4", "rk45", "implicit_euler", "implicit_midpoint", "glrk2", "glrk3" }; @@ -344,7 +344,7 @@ namespace gui { * WORKSPACE MANAGEMENT * -------------------------------------------------- */ - // Close the current workspace, clearing all systems, scene objects, and meshes. This is typically called before loading a new workspace or robot. + // Close the current workspace, clearing all systems, scene objects, and meshes. This is typically called before loading a new workspace or rigidBody. void SimulationManager::closeWorkspace() { // Order matters: // 1. Systems first (they hold renderable indices) @@ -354,7 +354,7 @@ namespace gui { _scene.clear(); _renderer.destroy_all_meshes(); _mesh_store.clear(); - _currentRobotName.clear(); + _currentRigidBodyName.clear(); _core->setScriptRunning(false); _core->stopSimulation(); @@ -374,14 +374,14 @@ namespace gui { _camera.setPosition(w.cameraPos); _camera.setYaw(w.cameraYaw); _camera.setPitch(w.cameraPitch); - if (!w.robotName.isEmpty()) { - load_robot(w.robotName.toStdString()); // Core re-load or skip; GUI visuals rebuilt fresh + if (!w.rigidBodyName.isEmpty()) { + load_rigidBody(w.rigidBodyName.toStdString()); // Core re-load or skip; GUI visuals rebuilt fresh } LOG_INFO("Workspace applied: '%s'", w.name.toUtf8().constData()); } - // Gather the current workspace state, filling the provided WorkspaceData structure with the current camera position, orientation, and robot name + // Gather the current workspace state, filling the provided WorkspaceData structure with the current camera position, orientation, and rigidBody name void SimulationManager::gatherWorkspace(gui::WorkspaceData& w) const { - w.robotName = QString::fromStdString(_currentRobotName); + w.rigidBodyName = QString::fromStdString(_currentRigidBodyName); w.integrationMethod = static_cast(integrationMethod()); w.adIntegrationMethod = static_cast(autoDiffIntegrationMethod()); w.autoDiff = autoDiffEnabled(); diff --git a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp index e202a1b9..dc7abb3c 100644 --- a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp @@ -1,12 +1,12 @@ // DSFE_GUI Systems/MultiBodySystem.cpp -#include "Systems/MultiBodySystem.h" +#include "Systems/MutliBodySystem.h" #include "Simulation/SimulationScene.h" #include "Simulation/MeshStore.h" #include "Simulation/SimulationRenderer.h" #include "Assets/MeshLoader.h" #include "Scene/Mesh.h" -#include "Robots/RobotModel.h" +#include "Systems/RigidBodyModel.h" #include "Platform/Paths.h" #include "EngineLib/LogMacros.h" @@ -23,7 +23,7 @@ namespace gui { return g; } - MultiBodySystem::MultiBodySystem(const robots::RobotModel& model, std::function&()> world_src, MeshStore& mesh_store, SimulationRenderer& renderer) + MultiBodySystem::MultiBodySystem(const systems::RigidBodyModel& model, std::function&()> world_src, MeshStore& mesh_store, SimulationRenderer& renderer) : _model(model), _world_src(world_src), _meshStore(mesh_store), _renderer(renderer) {} void MultiBodySystem::build(SimulationScene& scene) { @@ -32,7 +32,7 @@ namespace gui { for (const auto& link : _model.links) { auto& renderables = _binding.link_to_renderables[link.name]; for (const auto& entry : link.visual.meshEntries) { - fs::path full = paths::assets() / "objects" / "Robotic_Arm_Models" / entry.meshFile; + fs::path full = paths::assets() / "objects" / "RigidBodyic_Arm_Models" / entry.meshFile; auto meshes = loader.load(full.string()); if (meshes.empty()) { LOG_ERROR("No meshes in %s", full.string().c_str()); @@ -79,7 +79,7 @@ namespace gui { } void MultiBodySystem::clear(SimulationScene& scene) { - // Reset all renderables associated with the robot links to identity transforms and clear the binding map + // Reset all renderables associated with the body links to identity transforms and clear the binding map for (const auto& [link_name, renderables] : _binding.link_to_renderables) { for (uint32_t r_idx : renderables) { scene.set_transform(r_idx, glm::mat4(1.0f)); } } diff --git a/DSFE_App/DSFE_GUI/src/Robots/RobotPresentationBuilder.cpp b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp similarity index 87% rename from DSFE_App/DSFE_GUI/src/Robots/RobotPresentationBuilder.cpp rename to DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp index b8e04226..73c9652a 100644 --- a/DSFE_App/DSFE_GUI/src/Robots/RobotPresentationBuilder.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp @@ -9,29 +9,24 @@ #include #include "Platform/Paths.h" -using namespace robots; +using namespace systems; namespace fs = std::filesystem; // Build a RobotRenderBinding from a RobotModel by loading the visual meshes for each link -RobotRenderBinding RobotPresentationBuilder::build(const robots::RobotModel& model) { - RobotRenderBinding binding; +RobotRenderBinding RobotPresentationBuilder::build(const systems::RigidBodyModel& model) { + RigidBodyRenderBinding binding; assets::MeshLoader loader; for (const auto& link : model.links) { auto& visuals = binding.linkVisuals[link.name]; - for (const auto& mesh : link.visual.meshEntries) { fs::path fullPath = paths::assets() / "objects" / "Robotic_Arm_Models" / mesh.meshFile; - auto meshes = loader.load(fullPath.string()); - for (auto& m : meshes) { auto obj = std::make_unique(m); - obj->name = link.name; obj->category = scene::ObjectCategory::RobotLink; obj->transform.scale = glm::vec3(model.scale); - visuals.push_back(obj.get()); // Store raw pointer for rendering binding.ownedObjects.push_back(std::move(obj)); // Cache unique_ptr for memory management } diff --git a/DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyRenderer.cpp similarity index 73% rename from DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp rename to DSFE_App/DSFE_GUI/src/Systems/RigidBodyRenderer.cpp index 918c0d46..3a65c987 100644 --- a/DSFE_App/DSFE_GUI/src/Robots/RobotRenderer.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyRenderer.cpp @@ -1,7 +1,7 @@ -// DSFE_GUI RobotRenderer.cpp -#include "Robots/RobotRenderer.h" -#include "Robots/RobotPresentationBuilder.h" -#include "Robots/RobotModel.h" +// DSFE_GUI RigidBodyRenderer.cpp +#include "Systems/RigidBodyRenderer.h" +#include "Systems/RigidBodyPresentationBuilder.h" +#include "Systems/RigidBodyModel.h" #include @@ -58,20 +58,16 @@ static mathlib::Quat 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(); } -void RobotRenderer::bind(const RobotRenderBinding& binding) { +void RigidBodyRenderer::bind(const RigidBodyRenderBinding& binding) { linkRenderMap.clear(); - LOG_INFO("Link render map cleared"); LOG_INFO("binding entries = %zu", binding.linkVisuals.size()); - for (const auto& [linkName, visuals] : binding.linkVisuals) { LinkRenderData renderData; for (auto* obj : visuals) { renderData.visuals.push_back(obj); } @@ -79,34 +75,30 @@ void RobotRenderer::bind(const RobotRenderBinding& binding) { } } -// Method to apply the computed world transforms to the corresponding Object instances for each robot link -void RobotRenderer::applyTransforms(const robots::RobotModel& robot, const std::vector& world) { - const bool isAligned = robot.baseFrameIsEngineAligned; - const size_t n = robot.links.size(); - if (world.size() < robot.links.size()) { - LOG_ERROR("Transform mismatch: links=%zu world=%zu", robot.links.size(), world.size()); +// Method to apply the computed world transforms to the corresponding Object instances for each body link +void RigidBodyRenderer::applyTransforms(const systems::RigidBodyModel& body, const std::vector& world) { + const bool isAligned = body.baseFrameIsEngineAligned; + const size_t n = body.links.size(); + if (world.size() < body.links.size()) { + LOG_ERROR("Transform mismatch: links=%zu world=%zu", body.links.size(), world.size()); return; } for (size_t i = 0; i < n; ++i) { - const auto& link = robot.links[i]; + const auto& link = body.links[i]; auto it = linkRenderMap.find(link.name); if (it == linkRenderMap.end()) { continue; } glm::mat4 T = toGlm(world[i]); glm::vec3 pos = glm::vec3(T[3]); // Extract translation from the 4x4 matrix glm::quat q = glm::quat_cast(T); // Extract rotation as a quaternion - glm::quat q_rot = isAligned ? (q * q_corr) : q; for (size_t v = 0; v < it->second.visuals.size(); ++v) { auto* obj = it->second.visuals[v]; if (!obj) { continue; } - obj->transform.position = pos; obj->transform.rotQ = q_rot; - if (v < link.visual.meshEntries.size()) { const auto& meshMat = link.visual.meshEntries[v]; - obj->material.albedo = glm::vec3(meshMat.material.x(), meshMat.material.y(), meshMat.material.z()); obj->material.metallic = meshMat.metallic; obj->material.roughness = meshMat.roughness; @@ -115,20 +107,13 @@ void RobotRenderer::applyTransforms(const robots::RobotModel& robot, const std:: } } -// Method to clear the current robot from the scene -void RobotRenderer::clearRobotModel(const robots::RobotModel& robot) { - // Remove robot objects from _objects - for (auto& link : robot.links) { +// Method to clear the current body from the scene +void RigidBodyRenderer::clearRigidBodyModel(const systems::RigidBodyModel& body) { + // Remove body objects from _objects + for (auto& link : body.links) { auto it = linkRenderMap.find(link.name); if (it == linkRenderMap.end()) continue; - for (auto* obj : it->second.visuals) { - if (obj) { - // Remove the object from the scene graph or object manager - // Assuming a function removeObjectFromScene exists - // removeObjectFromScene(obj); - delete obj; // Or use smart pointers to manage memory automatically - } - } + for (auto* obj : it->second.visuals) { if (obj) { delete obj; } /* Smart pointers to memory manange */ } } - D_WARN("Old robot model removed"); + D_WARN("Old body model removed"); } \ No newline at end of file From c1f36794e946898cbd39d3f95faf646841772337 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 18:54:15 +0100 Subject: [PATCH 006/114] fixes: Correct Core changeover of names (Succesful Build of Core) * Working through GUI now (many errors) --- DSFE_App/DSFE_Core/CMakeLists.txt | 50 ++++++++-------- .../DSFE_Core/include/Analysis/MetricLogger.h | 2 +- DSFE_App/DSFE_Core/include/DSL/Command.h | 2 +- .../DSFE_Core/include/DSL/CommandContext.h | 5 +- .../include/DSL/Commands/RotateByCmd.h | 2 +- .../include/DSL/Commands/SelectCmd.h | 2 +- .../DSFE_Core/include/DSL/Commands/SpinCmd.h | 4 +- .../DSFE_Core/include/DSL/Commands/WaitCmd.h | 2 +- DSFE_App/DSFE_Core/include/DSL/ICommand.h | 6 +- .../DSFE_Core/include/DSL/IStoredProgram.h | 2 +- DSFE_App/DSFE_Core/include/DSL/Parser.h | 4 +- .../include/Physics/RigidBodyDynamics.h | 12 ++-- .../include/Physics/RigidBodyDynamics.inl | 60 +++++++++---------- .../include/Physics/RigidBodyKinematics.h | 2 +- .../include/Physics/RigidBodyKinematics.inl | 6 +- .../include/Physics/SpatialDynamics.h | 17 +++--- .../include/Physics/SpatialDynamics.inl | 44 +++++++------- .../DSFE_Core/include/Platform/DataManager.h | 4 +- .../include/Platform/ISimulationCore.h | 22 +++---- .../DSFE_Core/include/Platform/StudyRunner.h | 2 +- .../DSFE_Core/include/Scene/SimulationCore.h | 14 ++--- .../include/Systems/RigidBodyLoader.h | 2 +- .../include/Systems/RigidBodySnapshot.h | 2 +- .../include/Systems/RigidBodySystem.h | 44 +++++++------- .../include/Systems/RigidBodySystemStep.inl | 22 +++---- .../DSFE_Core/include/Systems/SpatialModel.h | 2 +- .../src/{Interpreter => DSL}/Command.cpp | 0 .../{Interpreter => DSL}/CommandContext.cpp | 53 +++++++--------- .../{Interpreter => DSL}/CommandFactory.cpp | 0 .../{Interpreter => DSL}/Commands/LoadCmd.cpp | 6 +- .../Commands/ParallelGroupCmd.cpp | 0 .../Commands/RotateByCmd.cpp | 0 .../Commands/RotateJointByCmd.cpp | 0 .../Commands/RotateJointToCmd.cpp | 0 .../Commands/RotateToCmd.cpp | 0 .../Commands/SelectCmd.cpp | 0 .../{Interpreter => DSL}/Commands/SetCmd.cpp | 0 .../Commands/SetOmegaCmd.cpp | 0 .../{Interpreter => DSL}/Commands/SpinCmd.cpp | 0 .../Commands/StartCmd.cpp | 0 .../{Interpreter => DSL}/Commands/StopCmd.cpp | 0 .../Commands/TrajClearCmd.cpp | 2 +- .../Commands/TrajSetCmd.cpp | 0 .../{Interpreter => DSL}/Commands/WaitCmd.cpp | 0 .../src/{Interpreter => DSL}/Parser.cpp | 6 +- .../{Interpreter => DSL}/RegisterCommand.cpp | 28 ++++----- .../src/{Interpreter => DSL}/RunWrapper.cpp | 0 .../{Interpreter => DSL}/StoredProgram.cpp | 0 .../src/{Interpreter => DSL}/UIContext.cpp | 0 .../src/{Interpreter => DSL}/Utils.cpp | 0 DSFE_App/DSFE_Core/src/EngineCore.cpp | 4 +- .../src/Physics/RigidBodyDynamics.cpp | 4 +- .../src/Physics/RigidBodyKinematics.cpp | 4 +- .../DSFE_Core/src/Platform/DataManager.cpp | 2 +- .../src/Systems/RigidBodySnapshot.cpp | 2 +- .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 3 +- 56 files changed, 223 insertions(+), 227 deletions(-) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Command.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/CommandContext.cpp (89%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/CommandFactory.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/LoadCmd.cpp (91%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/ParallelGroupCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/RotateByCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/RotateJointByCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/RotateJointToCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/RotateToCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/SelectCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/SetCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/SetOmegaCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/SpinCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/StartCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/StopCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/TrajClearCmd.cpp (95%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/TrajSetCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Commands/WaitCmd.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Parser.cpp (99%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/RegisterCommand.cpp (72%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/RunWrapper.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/StoredProgram.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/UIContext.cpp (100%) rename DSFE_App/DSFE_Core/src/{Interpreter => DSL}/Utils.cpp (100%) diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 05a5bcae..4e95fa79 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -40,7 +40,7 @@ set(SYSTEMS_SRC src/Systems/SystemLoader.cpp src/Systems/RigidBodySystem.cpp src/Systems/TrajectoryManager.cpp - src/Systems/RigidBodySimSnapshot.cpp + src/Systems/RigidBodySnapshot.cpp ) set(PHYSICS_SRC @@ -57,30 +57,30 @@ set(PLATFORM_SRC ) set(DSL_SRC - src/Interpreter/Command.cpp - src/Interpreter/CommandContext.cpp - src/Interpreter/CommandFactory.cpp - src/Interpreter/Parser.cpp - src/Interpreter/RegisterCommand.cpp - src/Interpreter/StoredProgram.cpp - src/Interpreter/Utils.cpp - src/Interpreter/RunWrapper.cpp - - src/Interpreter/Commands/LoadCmd.cpp - src/Interpreter/Commands/ParallelGroupCmd.cpp - src/Interpreter/Commands/RotateByCmd.cpp - src/Interpreter/Commands/RotateJointByCmd.cpp - src/Interpreter/Commands/RotateJointToCmd.cpp - src/Interpreter/Commands/RotateToCmd.cpp - src/Interpreter/Commands/SelectCmd.cpp - src/Interpreter/Commands/SetCmd.cpp - src/Interpreter/Commands/SetOmegaCmd.cpp - src/Interpreter/Commands/SpinCmd.cpp - src/Interpreter/Commands/StartCmd.cpp - src/Interpreter/Commands/StopCmd.cpp - src/Interpreter/Commands/TrajClearCmd.cpp - src/Interpreter/Commands/TrajSetCmd.cpp - src/Interpreter/Commands/WaitCmd.cpp + src/DSL/Command.cpp + src/DSL/CommandContext.cpp + src/DSL/CommandFactory.cpp + src/DSL/Parser.cpp + src/DSL/RegisterCommand.cpp + src/DSL/StoredProgram.cpp + src/DSL/Utils.cpp + src/DSL/RunWrapper.cpp + + src/DSL/Commands/LoadCmd.cpp + src/DSL/Commands/ParallelGroupCmd.cpp + src/DSL/Commands/RotateByCmd.cpp + src/DSL/Commands/RotateJointByCmd.cpp + src/DSL/Commands/RotateJointToCmd.cpp + src/DSL/Commands/RotateToCmd.cpp + src/DSL/Commands/SelectCmd.cpp + src/DSL/Commands/SetCmd.cpp + src/DSL/Commands/SetOmegaCmd.cpp + src/DSL/Commands/SpinCmd.cpp + src/DSL/Commands/StartCmd.cpp + src/DSL/Commands/StopCmd.cpp + src/DSL/Commands/TrajClearCmd.cpp + src/DSL/Commands/TrajSetCmd.cpp + src/DSL/Commands/WaitCmd.cpp ) set(SIM_CORE_SRC src/Scene/SimulationCore.cpp) diff --git a/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h b/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h index 1e501e6c..6f53de4a 100644 --- a/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h +++ b/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h @@ -7,7 +7,7 @@ #include #include -namespace robots { +namespace systems { // Struct for logging joint data each step (for later analysis) struct JointLogBuffer { // Sim Metadata diff --git a/DSFE_App/DSFE_Core/include/DSL/Command.h b/DSFE_App/DSFE_Core/include/DSL/Command.h index b3d87171..4a626e94 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Command.h +++ b/DSFE_App/DSFE_Core/include/DSL/Command.h @@ -14,7 +14,7 @@ namespace commands { // Set the command context void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult update(CommandContext& cntx, double dt) override; + dsl::CmdResult update(CommandContext& cntx, double dt) override; void execute() override; diff --git a/DSFE_App/DSFE_Core/include/DSL/CommandContext.h b/DSFE_App/DSFE_Core/include/DSL/CommandContext.h index 6e7c6616..8cd4ed0b 100644 --- a/DSFE_App/DSFE_Core/include/DSL/CommandContext.h +++ b/DSFE_App/DSFE_Core/include/DSL/CommandContext.h @@ -38,8 +38,7 @@ namespace commands { // --- INITIALISATION METHODS --- utils::OpResult startSim(); utils::OpResult setFixedDt(double dt); - utils::OpResult loadSingleBody(const std::string& bodyName); - utils::OpResult loadMultibody(const std::string& bodyName); + utils::OpResult loadRigidBody(const std::string& bodyName); // --- GLOBAL STATE METHODS --- @@ -61,7 +60,7 @@ namespace commands { // --- HELPER METHODS --- core::ISimulationCore* Core() const; - systems::RobotSystem& RigidBody() const; + systems::RigidBodySystem& RigidBody() const; // --- ROTATION COMMAND METHODS --- diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h index 7f155d5e..995eaf4a 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h @@ -20,7 +20,7 @@ namespace commands { std::string_view getName() const { return "rotateBy"; } void setContext(CommandContext& cntx) override { _cntx = &cntx; } - program_data::CmdResult getResult() const { return _result; } + CmdResult getResult() const { return _result; } void setResult(const CmdResult& result) { _result = result; } CmdResult currentResult() const override { return getResult(); } diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h index 49099b94..e07969c7 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h @@ -29,7 +29,7 @@ namespace commands { void execute() override; CommandContext* _cntx = nullptr; - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; + CmdResult _result = { CmdState::NotStarted, {}, "" }; protected: void markFailed(const std::string& message) override; diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h index 214c6936..e97ff188 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h @@ -7,8 +7,8 @@ #include "EngineCore.h" #include -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h index a3962db8..d42fd0f9 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h @@ -33,7 +33,7 @@ namespace commands { utils::AxisMask _axes{}; double _remainingTime = 0.0; bool _started = false; - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; + CmdResult _result = { CmdState::NotStarted, {}, "" }; protected: void markFailed(const std::string& message) override; diff --git a/DSFE_App/DSFE_Core/include/DSL/ICommand.h b/DSFE_App/DSFE_Core/include/DSL/ICommand.h index e8a1301a..562ad0a0 100644 --- a/DSFE_App/DSFE_Core/include/DSL/ICommand.h +++ b/DSFE_App/DSFE_Core/include/DSL/ICommand.h @@ -9,6 +9,8 @@ #include #include +using namespace dsl; + namespace commands { // Forward declaration of ICommand for use in IStoredProgram class CommandContext; @@ -23,10 +25,10 @@ namespace commands { virtual void setContext(CommandContext& cntx) = 0; // Update command - virtual program_data::CmdResult update(CommandContext& cntx, double dt) = 0; + virtual CmdResult update(CommandContext& cntx, double dt) = 0; // Get current result - virtual program_data::CmdResult currentResult() const = 0; + virtual CmdResult currentResult() const = 0; // Execute command virtual void execute() = 0; diff --git a/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h index d2c3ed9f..a9b038ab 100644 --- a/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h +++ b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h @@ -5,7 +5,7 @@ #pragma once #include "EngineCore.h" -#include "DSLProgramData.h" +#include "DSL/ProgramData.h" #include "DSL/Utils.h" #include #include diff --git a/DSFE_App/DSFE_Core/include/DSL/Parser.h b/DSFE_App/DSFE_Core/include/DSL/Parser.h index 23067122..81371f5b 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Parser.h +++ b/DSFE_App/DSFE_Core/include/DSL/Parser.h @@ -23,9 +23,9 @@ namespace dsl { private: IStoredProgram* _program = nullptr; - program_data::ProgramData _programData; + ProgramData _programData; Command _currentCmd; - + std::vector _tokens; std::vector lines; size_t pos = 0; diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h index 3d44d7e7..65ea579c 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h @@ -13,7 +13,7 @@ #include "Physics/SpatialDynamics.h" #include "Systems/SpatialModel.h" #include "Physics/RigidBodyKinematics.h" -#include "Systems/RigidBodySimSnapshot.h" +#include "Systems/RigidBodySnapshot.h" #include "Systems/TrajectoryManager.h" @@ -42,7 +42,7 @@ namespace physics { // Computes the inertia tensor of a body link template - mathlib::Mat3_T computeLinkInertiaTensor(const Link& link) const; + mathlib::Mat3_T computeLinkInertiaTensor(const systems::RigidBodyLink& link) const; // Computes the contribution of a single joint and its child link to the effective inertia I_eff of the joint template @@ -155,10 +155,10 @@ namespace physics { // References and pointers std::unique_ptr _kinematics = nullptr; - static bool isControlledJoint(eJointType t) { + static bool isControlledJoint(systems::eJointType t) { return - t == eJointType::REVOLUTE || - t == eJointType::PRISMATIC; + t == systems::eJointType::REVOLUTE || + t == systems::eJointType::PRISMATIC; } double _dt = 1.0 / 180.0; // default timestep for dynamics updates @@ -169,4 +169,4 @@ namespace physics { }; } // namespace physics -#include "Systems/RigidBodyDynamics.inl" \ No newline at end of file +#include "Physics/RigidBodyDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index 45a74558..7bf5edd1 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -7,7 +7,7 @@ namespace physics { // Computes the inertia tensor of a rigidbody link template - mathlib::Mat3_T RigidBodyDynamics::computeLinkInertiaTensor(const Link& link) const { + mathlib::Mat3_T RigidBodyDynamics::computeLinkInertiaTensor(const systems::RigidBodyLink& link) const { const systems::Inertia& I = link.inertial.inertia; // Construct the inertia tensor matrix @@ -63,7 +63,7 @@ namespace physics { // Computes the full mass matrix M(q) based on the current state and body configuration template void RigidBodyDynamics::computeMassMatrix( - const RigidBodyConstModel& body, + const systems::RigidBodyConstModel& body, const std::vector>& T_world, const std::vector>& jointWorldPoses, mathlib::MatX_T& M_out @@ -74,7 +74,7 @@ namespace physics { // Compute its contribution to the mass matrix for each link for (size_t k = 0; k < body.links.size(); ++k) { - const RigidBodyLink& link = body.links[k]; + const systems::RigidBodyLink& link = body.links[k]; const Scalar m = link.inertial.mass; if (m <= Scalar(0)) { continue; } @@ -88,8 +88,8 @@ namespace physics { // Compute Jacobian columns for each joint and accumulate mass matrix contributions for (size_t i = 0; i < n; ++i) { - const RigidBodyJoint& j_i = body.joints[i]; - if (j_i.type == eJointType::FIXED) { continue; } + const systems::RigidBodyJoint& j_i = body.joints[i]; + if (j_i.type == systems::eJointType::FIXED) { continue; } if (!body.jointAffectsLink(i, k)) { continue; } // skip if joint i does not affect link k const mathlib::Pose_T& T_joint_i = jointWorldPoses[i]; // pose of joint i in world frame @@ -104,8 +104,8 @@ namespace physics { // Computes the contribution to the mass matrix from this link for joints i and j for (size_t j = 0; j < n; ++j) { - const RigidBodyJoint& j_j = body.joints[j]; - if (j_j.type == eJointType::FIXED) { continue; } + const systems::RigidBodyJoint& j_j = body.joints[j]; + if (j_j.type == systems::eJointType::FIXED) { continue; } if (!body.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k @@ -127,7 +127,7 @@ namespace physics { // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and body configuration template mathlib::VecX_T RigidBodyDynamics::computeCoriolisVector( - const RigidBodyConstModel& body, + const systems::RigidBodyConstModel& body, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, const std::vector>& T_world, @@ -186,7 +186,7 @@ namespace physics { // Computes the gravity torque for a joint based on the current state and body configuration template mathlib::VecX_T RigidBodyDynamics::computeGravityTorque( - const RigidBodyConstModel& body, + const systems::RigidBodyConstModel& body, const std::vector>& T_world, const std::vector>& jointWorldPoses ) const { @@ -196,8 +196,8 @@ namespace physics { // For each joint, sum the gravity contributions from all links for (size_t i = 0; i < n; ++i) { - const RigidBodyJoint& j = body.joints[i]; - if (j.type == eJointType::FIXED) { continue; } + const systems::RigidBodyJoint& j = body.joints[i]; + if (j.type == systems::eJointType::FIXED) { continue; } Scalar tau_g_i = Scalar(0); // [Nm], gravity torque contribution for joint i @@ -209,7 +209,7 @@ namespace physics { // For each link, compute the gravitational force and its torque contribution about joint i for (size_t k = 0; k < body.links.size(); ++k) { - const RigidBodyLink& link = body.links[k]; + const systems::RigidBodyLink& link = body.links[k]; const Scalar m = (Scalar)link.inertial.mass; if (m <= Scalar(0)) { continue; } @@ -236,7 +236,7 @@ namespace physics { // Computes the analytical Jacobian matrix J(q) for the body based on the current state and body configuration template void RigidBodyDynamics::analyticalJacobian( - const RigidBodyConstModel& body, + const systems::RigidBodyConstModel& body, const mathlib::VecX_T& x, mathlib::MatX_T& J_out, DenseDynamicsScratch& scratch @@ -257,8 +257,8 @@ namespace physics { mathlib::MatX_T dTau_dv = mathlib::MatX::Zero(n, n); for (size_t i = 0; i < n; ++i) { - const RigidBodyJoint& joint = body.joints[i]; - if (joint.type == eJointType::FIXED) { continue; } + const systems::RigidBodyJoint& joint = body.joints[i]; + if (joint.type == systems::eJointType::FIXED) { continue; } const Scalar wn = static_cast(joint.wn_target); // [rad/s], natural frequency const Scalar z = static_cast(joint.zeta_target); // damping ratio @@ -294,7 +294,7 @@ namespace physics { mathlib::VecX_T RigidBodyDynamics::derivative_dense( Scalar t, const mathlib::VecX_T& x, - const RigidBodySimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ) { @@ -316,14 +316,14 @@ namespace physics { if (!scratch.dense.M.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } + if (snap.torqueMode != systems::eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } scratch.dense.tau.setZero(); for (size_t i = 0; i < n; ++i) { - const RigidBodyJoint& joint = snap.model->joints[i]; + const systems::RigidBodyJoint& joint = snap.model->joints[i]; // Fixed joints - if (joint.type == eJointType::FIXED) { + if (joint.type == systems::eJointType::FIXED) { out.metrics.q[i] = q[i]; out.metrics.qd[i] = qd[i]; out.metrics.qdd[i] = 0.0; @@ -396,7 +396,7 @@ namespace physics { const systems::SpatialModel& model, Scalar t, const mathlib::VecX_T& x, - const RigidBodySimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ) { @@ -433,7 +433,7 @@ namespace physics { scratch.dense.tau.setZero(); for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; + const systems::SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { scratch.dense.tau[i] = Scalar(0); continue; @@ -487,7 +487,7 @@ namespace physics { void RigidBodyDynamics::jacobian_spatial( const systems::SpatialModel& model, const mathlib::VecX_T& x, - const RigidBodySimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, @@ -514,7 +514,7 @@ namespace physics { mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); for (size_t i = 0; i < n; ++i) { - const SpatialJoint& joint = model.joints[i]; + const systems::SpatialJoint& joint = model.joints[i]; if (!isControlledJoint(joint.type)) { continue; } dTau_dq(i, i) = -kp[i]; const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient @@ -549,7 +549,7 @@ namespace physics { mathlib::VecX_T RigidBodyDynamics::derivative_with_gains( Scalar t, const mathlib::VecX_T& x, - const RigidBodySimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, DynamicsScratch& scratch, @@ -568,14 +568,14 @@ namespace physics { scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); scratch.g.setZero(); - if (snap.torqueMode != eTorqueMode::NONE) { + if (snap.torqueMode != systems::eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } scratch.dense.tau.setZero(); for (size_t i = 0; i < n; ++i) { - const RigidBodyJoint& joint = snap.model->joints[i]; - if (joint.type == eJointType::FIXED) continue; + const systems::RigidBodyJoint& joint = snap.model->joints[i]; + if (joint.type == systems::eJointType::FIXED) continue; const Scalar eps = static_cast < Scalar>(1e-6); const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient @@ -603,7 +603,7 @@ namespace physics { template void RigidBodyDynamics::jacobian_with_gains( const mathlib::VecX_T& x, - const RigidBodySimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, @@ -626,8 +626,8 @@ namespace physics { mathlib::MatX_T dTau_dv = mathlib::MatX_T::Zero(n, n); for (size_t i = 0; i < n; ++i) { - const RigidBodyJoint& joint = snap.model->joints[i]; - if (joint.type == eJointType::FIXED) continue; + const systems::RigidBodyJoint& joint = snap.model->joints[i]; + if (joint.type == systems::eJointType::FIXED) continue; dTau_dq(i, i) = -kp[i]; diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h index d2150642..4e500dfc 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h @@ -4,7 +4,7 @@ #include "EngineCore.h" #include #include -#include "Systems/RigidBodySimSnapshot.h" +#include "Systems/RigidBodySnapshot.h" #include "EngineLib/LogMacros.h" diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl index 1b3b2a59..f5a7f9fd 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl @@ -39,10 +39,10 @@ namespace physics { // Compute joint motion transform based on joint axis and angle Pose_T T_motion = mathlib::Pose_T::Identity(); - if (joint.type == eJointType::REVOLUTE) { + if (joint.type == systems::eJointType::REVOLUTE) { T_motion = jointMotionTransform(joint.axis.template cast(), q); // rotation about joint axis } - else if (joint.type == eJointType::PRISMATIC) { + else if (joint.type == systems::eJointType::PRISMATIC) { T_motion.template block<3, 1>(0, 3) = mathlib::safeNormalised(joint.axis) * q; // translation along joint axis } @@ -68,7 +68,7 @@ namespace physics { std::vector> jointWorldPoses(body.joints.size()); for (size_t i = 0; i < body.joints.size(); ++i) { - const RigidBodyJoint& joints = body.joints[i]; + const systems::RigidBodyJoint& joints = body.joints[i]; int childIdx = body.linkIndex(joints.child); if (childIdx < 0 || childIdx >= T_world.size()) { diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h index 939ca0aa..910849bb 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h @@ -5,6 +5,7 @@ #pragma once #include "EngineCore.h" +#include #include "Systems/SpatialModel.h" #include "Physics/DynamicsTypes.h" @@ -23,7 +24,7 @@ namespace physics { template static void computeAccelerations_RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& qdd, const std::vector>& Xup, const std::vector>& c, @@ -33,7 +34,7 @@ namespace physics { template static void computeBackwardForces_RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& a, @@ -42,7 +43,7 @@ namespace physics { template static mathlib::VecX_T RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, const mathlib::VecX_T& qdd, @@ -51,14 +52,14 @@ namespace physics { template static mathlib::MatX_T CRBA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, DynamicsScratch& scratch ); template static void computeArticulatedBodies_ABA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& c, @@ -73,7 +74,7 @@ namespace physics { template static void computeAccelerations_ABA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& c, const mathlib::VecX_T& u_out, @@ -86,7 +87,7 @@ namespace physics { template static mathlib::VecX_T ABA( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, const mathlib::VecX_T& tau, @@ -95,4 +96,4 @@ namespace physics { }; } // namespace physics -#include "Systems/SpatialDynamics.inl" \ No newline at end of file +#include "Physics/SpatialDynamics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 73d44adc..ed154684 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -7,7 +7,7 @@ namespace physics { template void SpatialDynamics::computeSpatialKinematicsAndBias( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, std::vector>& Xup_out, @@ -20,18 +20,18 @@ namespace physics { c_out.resize(n); for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; + const systems::SpatialJoint& j = model.joints[i]; // Joint Transform XJ mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); - if (j.type == eJointType::REVOLUTE) { + if (j.type == systems::eJointType::REVOLUTE) { mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.angular()); mathlib::Mat3_T R = mathlib::AngleAxis(q[i], axis); mathlib::Vec3_T r = mathlib::Vec3_T::Zero(); XJ = mathlib::spatialTransform(R, r); } - else if (j.type == eJointType::PRISMATIC) { + else if (j.type == systems::eJointType::PRISMATIC) { mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.linear()); mathlib::Vec3_T r = q[i] * axis; mathlib::Mat3_T R = mathlib::Mat3_T::Identity(); @@ -54,7 +54,7 @@ namespace physics { template void SpatialDynamics::computeAccelerations_RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& qdd, const std::vector>& Xup, const std::vector>& c, @@ -70,7 +70,7 @@ namespace physics { g.template segment<3>(3); for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; + const systems::SpatialJoint& j = model.joints[i]; mathlib::SpatialVec_T aJ = j.S * qdd[i]; // Joint Acceleration // Root Link @@ -85,7 +85,7 @@ namespace physics { template void SpatialDynamics::computeBackwardForces_RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& a, @@ -98,7 +98,7 @@ namespace physics { // Forward Force Computation for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; + const systems::SpatialJoint& j = model.joints[i]; mathlib::SpatialVec_T I_v = j.inertia * v[i]; mathlib::SpatialVec_T coriolis = crossForce(v[i], I_v); f[i].v = j.inertia * a[i].v + coriolis.v; @@ -106,7 +106,7 @@ namespace physics { // Backward Recursion Computation for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; + const systems::SpatialJoint& j = model.joints[i]; tau_out[i] = j.S.dot(f[i]); mathlib::SpatialMat_T XupT = Xup[i].transpose(); if (j.parent >= 0) { f[j.parent] += XupT * f[i]; } @@ -115,7 +115,7 @@ namespace physics { template mathlib::VecX_T SpatialDynamics::RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, const mathlib::VecX_T& qdd, @@ -142,7 +142,7 @@ namespace physics { template mathlib::MatX_T SpatialDynamics::CRBA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, DynamicsScratch& scratch ) { @@ -155,8 +155,8 @@ namespace physics { // Upward pass: propagate spatial inertia from child links to parent joints for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } + const systems::SpatialJoint& j = model.joints[i]; + if (j.type == systems::eJointType::FIXED) { continue; } int p = j.parent; if (p >= 0) { mathlib::MatX_T XupT = Xup[i].transpose(); @@ -166,8 +166,8 @@ namespace physics { // Downward pass: compute mass matrix contributions for each joint for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { continue; } + const systems::SpatialJoint& j = model.joints[i]; + if (j.type == systems::eJointType::FIXED) { continue; } mathlib::SpatialVec_T F = Ic[i] * j.S; scratch.dense.M(i, i) = j.S.dot(F); @@ -186,7 +186,7 @@ namespace physics { template void SpatialDynamics::computeArticulatedBodies_ABA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& c, @@ -210,9 +210,9 @@ namespace physics { // Upward pass: compute articulated body inertias and bias forces for (int i = (int)n - 1; i >= 0; --i) { - const SpatialJoint& j = model.joints[i]; + const systems::SpatialJoint& j = model.joints[i]; - if (j.type == eJointType::FIXED) { + if (j.type == systems::eJointType::FIXED) { Ia_out[i] = IA_out[i]; if (j.parent >= 0) { mathlib::SpatialMat_T XupT = Xup[i].transpose(); @@ -244,7 +244,7 @@ namespace physics { template void SpatialDynamics::computeAccelerations_ABA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& c, const mathlib::VecX_T& u_out, @@ -259,12 +259,12 @@ namespace physics { qdd_out.resize(n); for (size_t i = 0; i < n; ++i) { - const SpatialJoint& j = model.joints[i]; + const systems::SpatialJoint& j = model.joints[i]; if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } - if (j.type == eJointType::FIXED) { + if (j.type == systems::eJointType::FIXED) { qdd_out[i] = Scalar(0); continue; } @@ -276,7 +276,7 @@ namespace physics { template mathlib::VecX_T SpatialDynamics::ABA( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, const mathlib::VecX_T& tau, diff --git a/DSFE_App/DSFE_Core/include/Platform/DataManager.h b/DSFE_App/DSFE_Core/include/Platform/DataManager.h index 2498937c..b5497297 100644 --- a/DSFE_App/DSFE_Core/include/Platform/DataManager.h +++ b/DSFE_App/DSFE_Core/include/Platform/DataManager.h @@ -17,7 +17,7 @@ typedef int64_t hid_t; // Placeholder for HDF5 type, compiled file will include the actual HDF5 headers -namespace robots { struct JointLogBuffer; } +namespace systems { struct JointLogBuffer; } namespace data { // Variant type to hold different data types @@ -107,7 +107,7 @@ namespace data { void captureJointBuffer( Stream s, std::string_view topic, - const robots::JointLogBuffer& buf + const systems::JointLogBuffer& buf ); bool enabled() const { return _enabled; } diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 5ff1029c..9c2a869e 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -8,11 +8,11 @@ // Forward Declarations namespace integration { enum class eIntegrationMethod; enum class eAutoDiffIntegrationMethod; } -namespace robots { class RobotSystem; } +namespace systems { class RigidBodySystem; } namespace single_body_system { class SingleBodySystem; } namespace control { class TrajectoryManager; } namespace diagnostics { class TelemetryRecorder; } -namespace interpreter { class IStoredProgram; } +namespace dsl { class IStoredProgram; } enum class eSimulationBackend; @@ -54,31 +54,31 @@ namespace core { virtual void enableAutoDiff(bool enable) = 0; virtual bool autoDiffEnabled() const = 0; // Subsystems - virtual robots::RobotSystem& robotSystem() = 0; + virtual systems::RigidBodySystem& rigidBodySystem() = 0; virtual single_body_system::SingleBodySystem& singleBodySystem() = 0; virtual control::TrajectoryManager& trajectoryManager() = 0; // Body management virtual bool hasSingleBody() const = 0; virtual void loadSingleBody(const std::string& name) = 0; - // Robot management - virtual bool hasRobot() const = 0; - virtual void loadRobot(const std::string& name) = 0; - virtual bool robotPresentationDirty() const = 0; - virtual void clearRobotPresentationDirty() = 0; + // RigidBody management + virtual bool hasRigidBody() const = 0; + virtual void loadRigidBody(const std::string& name) = 0; + virtual bool rigidBodyPresentationDirty() const = 0; + virtual void clearRigidBodyPresentationDirty() = 0; // Script execution virtual void setRunTag(const std::string& tag) = 0; virtual void setScriptRunning(bool running) = 0; virtual bool isScriptRunning() const = 0; virtual void setLastScriptText(const std::string& text) = 0; virtual std::string& lastScriptText() = 0; - virtual bool runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) = 0; + virtual bool runScriptToCompletion(dsl::IStoredProgram* program, integration::eIntegrationMethod method) = 0; // Telemetry access virtual void setTelemetryHz(double hz) = 0; virtual double telemetryHz() const = 0; virtual diagnostics::TelemetryRecorder& telemetry() = 0; virtual size_t telemetrySampleCount() const = 0; // Access to the active program (if any) - virtual void setActiveProgram(interpreter::IStoredProgram* program) = 0; - virtual interpreter::IStoredProgram* activeProgram() const = 0; + virtual void setActiveProgram(dsl::IStoredProgram* program) = 0; + virtual dsl::IStoredProgram* activeProgram() const = 0; }; } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Platform/StudyRunner.h b/DSFE_App/DSFE_Core/include/Platform/StudyRunner.h index 7ae5b8dd..378e50ad 100644 --- a/DSFE_App/DSFE_Core/include/Platform/StudyRunner.h +++ b/DSFE_App/DSFE_Core/include/Platform/StudyRunner.h @@ -9,7 +9,7 @@ #include #include "Platform/ISimulationCore.h" -#include "Interpreter/StoredProgram.h" +#include "DSL/StoredProgram.h" // Type alias for a unique_ptr to ISimulationCore with a custom deleter (using std::function for flexibility) using CorePtr = std::unique_ptr>; diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index fa363942..81f4d29b 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -26,7 +26,7 @@ namespace control { class TrajectoryManager; } namespace systems { class RigidBodySystem; } namespace single_body_system { class SingleBodySystem; } -namespace interpreter { class IStoredProgram; } +namespace dsl { class IStoredProgram; } namespace core { // configurable defaults (not part of class to allow tuning without recompilation) @@ -80,7 +80,7 @@ namespace core { void setRunTag(const std::string& tag) override { _runTag = tag; } // Subsystems access - systems::RigidBodySystem& rigidBoySystem() override; + systems::RigidBodySystem& rigidBodySystem() override; single_body_system::SingleBodySystem& singleBodySystem() override; control::TrajectoryManager& trajectoryManager() override; @@ -95,7 +95,7 @@ namespace core { void loadRigidBodyInternal(const std::string& name); // Internal method that assumes ownership // Run a script to completion synchronously with a specific integrator - bool runScriptToCompletion(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) override; + bool runScriptToCompletion(dsl::IStoredProgram* program, integration::eIntegrationMethod method) override; // Telemetry diagnostics::TelemetryRecorder& telemetry() override; @@ -135,8 +135,8 @@ namespace core { std::string& lastScriptText() override { return _lastScriptText; } // Accesors for the active script program - void setActiveProgram(interpreter::IStoredProgram* p) override; - interpreter::IStoredProgram* activeProgram() const override; + void setActiveProgram(dsl::IStoredProgram* p) override; + dsl::IStoredProgram* activeProgram() const override; bool rigidBodyPresentationDirty() const override { return _rigidBodyPresentationDirty; } void clearRigidBodyPresentationDirty() override { _rigidBodyPresentationDirty = false; } @@ -144,7 +144,7 @@ namespace core { private: // Export thread management void exportThreadMain(); - void scriptParallelisation(interpreter::IStoredProgram* program); + void scriptParallelisation(dsl::IStoredProgram* program); std::thread _expThread; std::mutex _expMutex; @@ -184,7 +184,7 @@ namespace core { std::string _runTag; // Active Script Program - interpreter::IStoredProgram* _activeProgram = nullptr; + dsl::IStoredProgram* _activeProgram = nullptr; bool _rigidBodyPresentationDirty = false; bool _singleBodyPresentationDirty = false; diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h index 29347f69..fe1fbf9c 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h @@ -10,6 +10,6 @@ namespace systems { class DSFE_API RigidBodyLoader { public: - static RigidBodyLoader loadFromJSON(const std::string& filepath); + static RigidBodyModel loadFromJSON(const std::string& filepath); }; } // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h index 4d1eb8d1..55bbed62 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h @@ -53,7 +53,7 @@ namespace systems { using RigidBodySnapshot = RigidBodySnapshot_T; template - inline RigidBdoySnapshot_T castSnapshot( + inline RigidBodySnapshot_T castSnapshot( const RigidBodySnapshot_T& src ) { RigidBodySnapshot_T dst; diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 91f78ff9..2c282186 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -7,8 +7,8 @@ #include "EngineCore.h" #include "Systems/RigidBodyModel.h" -#include "Physics/SpatialModel.h" -#include "System/RigidBodySimSnapshot.h" +#include "Systems/SpatialModel.h" +#include "Systems/RigidBodySnapshot.h" #include "Physics/DynamicsTypes.h" #include @@ -43,7 +43,7 @@ namespace systems { struct RigidBodyStepResult_T { integration::StepOut_T stepOut; RigidBodySnapshot_T snap; - DynamicsResult dynamics; + physics::DynamicsResult dynamics; mathlib::VecX_T tau_rnea; }; @@ -75,9 +75,9 @@ namespace systems { std::string findRootLink() const; - bool hasLinkName(const std::string& linkName) const { return _linkIndex.find(linkName) != _linkIndex.end(); } - const std::string& rigidbodyName() const { return _body.name; } - bool hasRigidBody() const { return _hasRigidBody; } + bool hasLinkName(const std::string& linkName) const { return _link_idx.find(linkName) != _link_idx.end(); } + const std::string& rigidBodyName() const { return _body.name; } + bool hasRigidBody() const { return _hasBody; } void setGravity(double g); const double getGravity() const { return _gravity; } @@ -93,7 +93,7 @@ namespace systems { } // Get pointer to this Systemsystem - const Systemsystem& getRigidBody() const { return *this; } + const RigidBodySystem& getRigidBody() const { return *this; } // ---- Joint State Methods --- @@ -145,10 +145,10 @@ namespace systems { // --- RIGIDBODY LINK AND ROOT POSE METHODS --- - bool setLinkRotation(const std::string& childLinkName, double angleDeg); - mathlib::Mat4 setRoot(const mathlib::Vec3& pos, const mathlib::Quat& rot); - void setRootPose(const mathlib::Vec3& pos, const mathlib::Quat& rot); - void setRootHome(const mathlib::Vec3& pos, const mathlib::Quat& rot); + bool setRigidBodyLinkRotation(const std::string& childLinkName, double angleDeg); + mathlib::Mat4 setRigidBodyRoot(const mathlib::Vec3& pos, const mathlib::Quat& rot); + void setRigidBodyRootPose(const mathlib::Vec3& pos, const mathlib::Quat& rot); + void setRigidBodyRootHome(const mathlib::Vec3& pos, const mathlib::Quat& rot); bool setDefaultPoseDeg(); void setCurrentJointIndex(int index) { _currentJointIndex = index; } @@ -197,17 +197,17 @@ namespace systems { void buildSpatialModel(); template - SystemstepResult_T step_impl( + RigidBodyStepResult_T step_impl( const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator, - DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + physics::DynamicsScratch& dynamicScratch, physics::DynamicsResult& dynamicResult ); template - void postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& scratch, const RigidBodyStepResult_T& result); + void postStepUpdate(const mathlib::VecX& x, const physics::DynamicsScratch& scratch, const RigidBodyStepResult_T& result); - std::unique_ptr _kinematics; - std::unique_ptr _dynamics; + std::unique_ptr _kinematics; + std::unique_ptr _dynamics; std::unique_ptr _integrator; integration::eIntegrationMethod _curIntMethod{}; @@ -245,7 +245,7 @@ namespace systems { void unpackRefState(const mathlib::VecX& xr); // Enforce joint limits after integration - void enforceJointLimits(Joint& j); + void enforceJointLimits(RigidBodyJoint& j); // Simulation time double _simTime = 0.0; @@ -257,11 +257,11 @@ namespace systems { SpatialModel _spatialModel; RigidBodyConstModel _constModel; - DynamicsScratch _dynScratch; - DynamicsResult _dynResult; + physics::DynamicsScratch _dynScratch; + physics::DynamicsResult _dynResult; - DynamicsScratch> _dynScratch_AD; - DynamicsResult> _dynResult_AD; + physics::DynamicsScratch> _dynScratch_AD; + physics::DynamicsResult> _dynResult_AD; // World to rigidbody base transform (meters) std::vector _worldTransforms; @@ -325,4 +325,4 @@ namespace systems { }; } // namespace rigidbody -#include "SystemsystemStep.inl" \ No newline at end of file +#include "Systems/RigidBodySystemStep.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index e924dde9..fc640418 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -6,7 +6,7 @@ namespace systems { template - T RigidBodySystem::clampJointAngle_T(const Joint& joint, T angleRad) { + T RigidBodySystem::clampJointAngle_T(const RigidBodyJoint& joint, T angleRad) { if (joint.limits.continuous) { return mathlib::wrapRad(angleRad); } else { return std::clamp(angleRad, T(joint.limits.minAngle), T(joint.limits.maxAngle)); } } @@ -14,7 +14,7 @@ namespace systems { // Method to take a snapshot of the current rigidbody state template RigidBodySnapshot_T RigidBodySystem::takeSnapshot(T simTime) const { - SystemsimSnapshot_T snap; + RigidBodySnapshot_T snap; snap.model = &_constModel; const size_t n = (size_t)_body.joints.size(); @@ -53,9 +53,9 @@ namespace systems { RigidBodyStepResult_T RigidBodySystem::step_impl( const mathlib::VecX_T& x, Scalar dt, Scalar t, IntegratorT& integrator, - DynamicsScratch& dynamicScratch, DynamicsResult& dynamicResult + physics::DynamicsScratch& dynamicScratch, physics::DynamicsResult& dynamicResult ) { - SystemstepResult_T result; + RigidBodyStepResult_T result; result.snap = takeSnapshot(t); auto& snap = result.snap; const size_t n = snap.model->joints.size(); @@ -74,7 +74,7 @@ namespace systems { auto& dynScratch = dynamicScratch; auto& dynResult = dynamicResult; - SpatialDynamics::computeSpatialKinematicsAndBias( + physics::SpatialDynamics::computeSpatialKinematicsAndBias( spatialModel, q, qd, dynScratch.spatial.Xup, @@ -82,7 +82,7 @@ namespace systems { ); // CRBA only for controller inertia scaling - mathlib::MatX_T M_start = SpatialDynamics::CRBA( + mathlib::MatX_T M_start = physics::SpatialDynamics::CRBA( spatialModel, dynScratch.spatial.Xup, dynScratch @@ -103,7 +103,7 @@ namespace systems { } // Compute RNEA torques for feedforward control - mathlib::VecX_T tau_rnea = SpatialDynamics::RNEA( + mathlib::VecX_T tau_rnea = physics::SpatialDynamics::RNEA( spatialModel, q, qd, qdd, dynScratch @@ -153,7 +153,7 @@ namespace systems { } template - void RigidBodySystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const SystemstepResult_T& result) { + void RigidBodySystem::postStepUpdate(const mathlib::VecX& x, const physics::DynamicsScratch& dynScratch, const RigidBodyStepResult_T& result) { const size_t n = result.snap.model->joints.size(); Eigen::Map q_next(x.data(), n); @@ -186,10 +186,10 @@ namespace systems { double g = _dynamics->getGravity(); for (size_t k = 0; k < _body.links.size(); ++k) { - const Link& link = _body.links[k]; + const RigidBodyLink& link = _body.links[k]; const double m = link.inertial.mass; if (m <= 0.0) { continue; } - Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); + mathlib::Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); sys_PE += m * g * com_world.z(); } @@ -203,7 +203,7 @@ namespace systems { if (buf) { auto dynResult = result.dynamics; for (size_t i = 0; i < n; ++i) { - const Joint& j = _body.joints[i]; + const RigidBodyJoint& j = _body.joints[i]; const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); const double err = q_ref_real[i] - q_real[i]; diff --git a/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h index 7e3e17ea..f03ca3a2 100644 --- a/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h @@ -33,4 +33,4 @@ namespace systems { SpatialModel cast() const; }; } // namespace systems -#include "SpatialModelCast.inl" \ No newline at end of file +#include "Systems/SpatialModelCast.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Command.cpp b/DSFE_App/DSFE_Core/src/DSL/Command.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Command.cpp rename to DSFE_App/DSFE_Core/src/DSL/Command.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp b/DSFE_App/DSFE_Core/src/DSL/CommandContext.cpp similarity index 89% rename from DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp rename to DSFE_App/DSFE_Core/src/DSL/CommandContext.cpp index 5676e936..035123a1 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/CommandContext.cpp @@ -40,19 +40,12 @@ namespace commands { return OpResult::Success(true); } - OpResult CommandContext::loadSingleBody(const std::string& bodyName) { - if (!_core) { return OpResult::Failure("Simulation manager is null."); } - if (bodyName.empty()) return OpResult::Failure("Body name is empty."); - /* Need to add logic here for single bodies since I removed physicsSystem. */ - return OpResult::Success(true); - } - // Loads a rigidBody by name and updates the context with the new rigidBody system - OpResult CommandContext::loadMultibody(const std::string& bodyName) { - if (!_core) { return OpResult::Failure("Simulation manager is null."); } + OpResult CommandContext::loadRigidBody(const std::string& bodyName) { + if (!_core) { return OpResult::Failure("SimulationCore is null."); } if (bodyName.empty()) return OpResult::Failure("RigidBody name is empty."); - _core->loadRigidBody(bodyName); // only load rigidBody for now, as multibody is not finished - auto& rs = _core->rigidBodySystem(); + _core->loadRigidBody(bodyName); + auto& rb = _core->rigidBodySystem(); return OpResult::Success(true); } @@ -65,8 +58,8 @@ namespace commands { utils::OpResult CommandContext::setJointOmega(const std::string& childLink, double omegaDegPerSec) { double omegaRadPerSec = degToRad(omegaDegPerSec); - auto& rs = _core->rigidBodySystem(); - rs.trySetJointOmegaRad(childLink, omegaRadPerSec); + auto& rb = _core->rigidBodySystem(); + rb.trySetJointOmegaRad(childLink, omegaRadPerSec); return OpResult::Success(true); } @@ -89,9 +82,9 @@ namespace commands { } double CommandContext::getJointAngleRad(const std::string& link) const { - auto& rs = _core->rigidBodySystem(); + auto& rb = _core->rigidBodySystem(); double a = 0.0f; - if (rs.tryGetJointAngleRad(link, a)) { return (double)a; } + if (rb.tryGetJointAngleRad(link, a)) { return (double)a; } else { LOG_WARN("Failed to get joint angle for link '%s'", link.c_str()); } return 0.0; } @@ -99,17 +92,17 @@ namespace commands { // --- JOINT ANGLE METHODS --- utils::OpResult CommandContext::setJointTargetRad(const std::string& link, double thetaTargetRad) { - auto& rs = _core->rigidBodySystem(); - if (!rs.trySetJointTargetRad(link, thetaTargetRad)) { + auto& rb = _core->rigidBodySystem(); + if (!rb.trySetJointTargetRad(link, thetaTargetRad)) { return OpResult::Failure("Failed to set joint target -> Joint not found or target rejected."); } return OpResult::Success(true); } utils::OpResult CommandContext::setJointTargetDeltaRad(const std::string& link, double deltaRad) { - auto& rs = _core->rigidBodySystem(); + auto& rb = _core->rigidBodySystem(); double refRad = 0.0f; - if (!rs.tryGetJointTargetRad(link, refRad)) { + if (!rb.tryGetJointTargetRad(link, refRad)) { return OpResult::Failure("Failed to get joint angle -> Joint not found."); } const double targetRad = refRad + deltaRad; @@ -117,9 +110,9 @@ namespace commands { } utils::OpResult CommandContext::setJointMaxOmegaRad(const std::string& link, double maxqd) { - auto& rs = _core->rigidBodySystem(); + auto& rb = _core->rigidBodySystem(); if (maxqd <= 0.0) { return OpResult::Failure("Max omega must be positive."); } - if (!rs.trySetJointOmegaMaxRad(link, maxqd)) { + if (!rb.trySetJointOmegaMaxRad(link, maxqd)) { return OpResult::Failure("Failed to set joint max omega -> Joint not found or invalid value."); } return OpResult::Success(true); @@ -127,8 +120,8 @@ namespace commands { // Sets the reference angular velocity for a joint (rad/s) utils::OpResult CommandContext::setJointOmegaRefRad(const std::string& link, double qd_ref) { - auto& rs = _core->rigidBodySystem(); - if (!rs.trySetJointOmegaRefRad(link, qd_ref)) { + auto& rb = _core->rigidBodySystem(); + if (!rb.trySetJointOmegaRefRad(link, qd_ref)) { return OpResult::Failure("Failed to set joint omega ref -> Joint not found or invalid value."); } return OpResult::Success(true); @@ -136,8 +129,8 @@ namespace commands { // Sets the reference angular acceleration for a joint (rad/s^2) utils::OpResult CommandContext::setJointAlphaRefRad(const std::string& link, double qdd_ref) { - auto& rs = _core->rigidBodySystem(); - if (!rs.trySetJointAlphaRefRad(link, qdd_ref)) { + auto& rb = _core->rigidBodySystem(); + if (!rb.trySetJointAlphaRefRad(link, qdd_ref)) { return OpResult::Failure("Failed to set joint alpha ref -> Joint not found or invalid value."); } return OpResult::Success(true); @@ -146,8 +139,8 @@ namespace commands { utils::OpResult CommandContext::updateJointRotateTo(double /*dt*/) { if (!_jnt.active) { return OpResult::Success(true); } - auto& rs = _core->rigidBodySystem(); - const bool done = rs.isJointAtTargetRad(_jnt.link, _jnt.epsAngle); + auto& rb = _core->rigidBodySystem(); + const bool done = rb.isJointAtTargetRad(_jnt.link, _jnt.epsAngle); if (done) { _jnt.active = false; return OpResult::Success(true); } SIM_ROTATE("Updating joint rotate to link='%s'", _jnt.link.c_str()); @@ -156,7 +149,7 @@ namespace commands { } utils::OpResult CommandContext::beginJointRotateTo(const std::string& link, double maxOmegaDegPerSec, double angleDeg) { - auto& rs = _core->rigidBodySystem(); + auto& rb = _core->rigidBodySystem(); if (link.empty()) return OpResult::Failure("beginJointRotateTo -> empty link."); const double current = getJointAngleRad(link); @@ -361,8 +354,8 @@ namespace commands { // Checks if the current context has a valid rigidBody and if the specified link index is within bounds bool CommandContext::hasLink(std::size_t linkIndex) const { - auto& rs = _core->rigidBodySystem(); - return linkIndex < rs.links().size(); + auto& rb = _core->rigidBodySystem(); + return linkIndex < rb.links().size(); } // --- PRIVATE METHODS --- diff --git a/DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp b/DSFE_App/DSFE_Core/src/DSL/CommandFactory.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp rename to DSFE_App/DSFE_Core/src/DSL/CommandFactory.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp similarity index 91% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp index 36feabf7..9f997964 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp @@ -19,7 +19,7 @@ namespace commands { // constructor LoadCmd::LoadCmd(const std::string& id, const std::vector& tokens) { - if (id == "rigidbody") { _target.type = LoadTargetType::rigidBody ; } + if (id == "rigidbody") { _target.type = LoadTargetType::RigidBody ; } else { std::string errMsg = "Invalid load(,...) identifier -> " + id; markFailed(errMsg); @@ -60,7 +60,7 @@ namespace commands { D_FAIL(errMsg.c_str()); return nullptr; } - id = std::tolower(id); - return std::make_unique(id, tokens); + std::string id_lower = toLower(id); + return std::make_unique(id_lower, tokens); } } // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/ParallelGroupCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/ParallelGroupCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/RotateByCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateByCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/RotateJointByCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateJointByCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/RotateJointToCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateJointToCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/RotateToCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateToCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SelectCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SelectCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SetCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetOmegaCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SetOmegaCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SpinCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SpinCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/StartCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/StartCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/StopCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/StopCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/TrajClearCmd.cpp similarity index 95% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/TrajClearCmd.cpp index dd78e317..3c2a4198 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/TrajClearCmd.cpp @@ -27,7 +27,7 @@ namespace commands { // --- TrajClearCmd Implementation --- // Update method for TrajClearCmd - program_data::CmdResult TrajClearCmd::update(CommandContext& cntx, double /*dt*/) { + CmdResult TrajClearCmd::update(CommandContext& cntx, double /*dt*/) { if (_done) return { CmdState::Executed, {}, "" }; core::ISimulationCore* core = cntx.Core(); diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/WaitCmd.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/WaitCmd.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp b/DSFE_App/DSFE_Core/src/DSL/Parser.cpp similarity index 99% rename from DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp rename to DSFE_App/DSFE_Core/src/DSL/Parser.cpp index 4cababd7..ebae5ab3 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Parser.cpp @@ -202,7 +202,7 @@ namespace dsl { } // inner commands - std::vector innerCmds; + std::vector innerCmds; int braceDepth = 1; // consume subsequent lines until matching '}' @@ -224,7 +224,7 @@ namespace dsl { if (t == "}") { braceDepth--; if (braceDepth == 0) break; continue; } // parse inner command line - program_data::Command cmd; + dsl::Command cmd; cmd.rawLine = std::string(innerLine); cmd.lineNumber = _program->getCurrentLineNumber(); @@ -264,7 +264,7 @@ namespace dsl { return; } - program_data::Command par; + dsl::Command par; par.cmdName = "parallel"; par.rawLine = std::string(line); par.lineNumber = _program->getCurrentLineNumber(); diff --git a/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp b/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp similarity index 72% rename from DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp rename to DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp index 6b2c434c..82679d16 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp @@ -7,22 +7,22 @@ #include "DSL/RegisterCommand.h" // Motion commands -#include "Interpreter/Commands/SpinCmd.h" -#include "Interpreter/Commands/RotateToCmd.h" -#include "Interpreter/Commands/RotateByCmd.h" -#include "Interpreter/Commands/RotateJointToCmd.h" -#include "Interpreter/Commands/RotateJointByCmd.h" -#include "Interpreter/Commands/TrajSetCmd.h" -#include "Interpreter/Commands/TrajClearCmd.h" -#include "Interpreter/Commands/SetOmegaCmd.h" +#include "DSL/Commands/SpinCmd.h" +#include "DSL/Commands/RotateToCmd.h" +#include "DSL/Commands/RotateByCmd.h" +#include "DSL/Commands/RotateJointToCmd.h" +#include "DSL/Commands/RotateJointByCmd.h" +#include "DSL/Commands/TrajSetCmd.h" +#include "DSL/Commands/TrajClearCmd.h" +#include "DSL/Commands/SetOmegaCmd.h" // Primary function commands -#include "Interpreter/Commands/StartCmd.h" -#include "Interpreter/Commands/StopCmd.h" -#include "Interpreter/Commands/WaitCmd.h" -#include "Interpreter/Commands/SelectCmd.h" -#include "Interpreter/Commands/LoadCmd.h" -#include "Interpreter/Commands/SetCmd.h" +#include "DSL/Commands/StartCmd.h" +#include "DSL/Commands/StopCmd.h" +#include "DSL/Commands/WaitCmd.h" +#include "DSL/Commands/SelectCmd.h" +#include "DSL/Commands/LoadCmd.h" +#include "DSL/Commands/SetCmd.h" namespace commands { // Register all commands with the factory diff --git a/DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp b/DSFE_App/DSFE_Core/src/DSL/RunWrapper.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp rename to DSFE_App/DSFE_Core/src/DSL/RunWrapper.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp b/DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp rename to DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp b/DSFE_App/DSFE_Core/src/DSL/UIContext.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp rename to DSFE_App/DSFE_Core/src/DSL/UIContext.cpp diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp b/DSFE_App/DSFE_Core/src/DSL/Utils.cpp similarity index 100% rename from DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp rename to DSFE_App/DSFE_Core/src/DSL/Utils.cpp diff --git a/DSFE_App/DSFE_Core/src/EngineCore.cpp b/DSFE_App/DSFE_Core/src/EngineCore.cpp index e184ba73..ef5f65e7 100644 --- a/DSFE_App/DSFE_Core/src/EngineCore.cpp +++ b/DSFE_App/DSFE_Core/src/EngineCore.cpp @@ -3,8 +3,8 @@ #include "EngineCore.h" #include "Scene/SimulationCore.h" -#include "Robots/RobotSystem.h" -#include "Robots/TrajectoryManager.h" +#include "Systems/RigidBodySystem.h" +#include "Systems/TrajectoryManager.h" #include #include diff --git a/DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp b/DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp index 5cadadf6..b9123ece 100644 --- a/DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp +++ b/DSFE_App/DSFE_Core/src/Physics/RigidBodyDynamics.cpp @@ -3,9 +3,9 @@ * Created by: Joss Salton, 26-07-2026 */ #include "pch.h" -#include "Systems/RigidBodyDynamics.h" +#include "Physics/RigidBodyDynamics.h" -namespace systems { +namespace physics { // Constructor RigidBodyDynamics::RigidBodyDynamics() : _kinematics(std::make_unique()) { diff --git a/DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp b/DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp index 84200e3c..47e4c203 100644 --- a/DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp +++ b/DSFE_App/DSFE_Core/src/Physics/RigidBodyKinematics.cpp @@ -4,11 +4,11 @@ */ #include "pch.h" -#include "Systems/RigidBodyKinematics.h" +#include "Physics/RigidBodyKinematics.h" using namespace mathlib; using namespace constants; -namespace systems { +namespace physics { RigidBodyKinematics::RigidBodyKinematics() {} } // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp b/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp index 5fa6cbf1..a164d614 100644 --- a/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp +++ b/DSFE_App/DSFE_Core/src/Platform/DataManager.cpp @@ -697,7 +697,7 @@ namespace data { void DataManager::captureJointBuffer( Stream s, std::string_view topic, - const robots::JointLogBuffer& buf + const systems::JointLogBuffer& buf ) { HDF5StreamWriter* writer = nullptr; diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp index ff79e576..1974f36b 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp @@ -4,7 +4,7 @@ */ #include "pch.h" -#include "Systems/RigidBodySimSnapshot.h" +#include "Systems/RigidBodySnapshot.h" namespace systems { // Method to check if a joint affects a link diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 14b370ca..1a6fe205 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -20,13 +20,14 @@ using namespace mathlib; using namespace constants; +using namespace physics; namespace systems { // Constructor RigidBodySystem::RigidBodySystem() : _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), _AD_integrator(std::make_unique()), _curIntMethod_AD(integration::eAutoDiffIntegrationMethod::AD_ImplicitEuler), - _kinematics(std::make_unique()), _dynamics(std::make_unique()), + _kinematics(std::make_unique()), _dynamics(std::make_unique()), _torqueMode(eTorqueMode::CONTROLLED) { if (!_integrator ) { LOG_WARN("RigidBodySystem got null IntegrationService*"); } } From 626ad4d8265114c0a259ecd96ab27b33773cae68 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 19:44:15 +0100 Subject: [PATCH 007/114] fixes: Further corrections for GUI and Core changeover (Succesfully Builds) --- .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 2 +- .../MainWindow/Widgets/DSLEditorWidget.h | 6 +-- .../include/MainWindow/Workspace/Workspace.h | 3 +- .../include/Simulation/SimulationManager.h | 52 +++++++++---------- .../include/Systems/MultiBodySystem.h | 8 +-- .../src/MainWindow/DSFE_MainWindow.cpp | 3 +- .../MainWindow/Widgets/DSLEditorWidget.cpp | 6 +-- .../Widgets/RobotSelectorWidget.cpp | 2 +- .../src/Simulation/SimulationManager.cpp | 16 +++--- .../DSFE_GUI/src/Systems/MultiBodySystem.cpp | 4 +- .../Systems/RigidBodyPresentationBuilder.cpp | 6 +-- 11 files changed, 54 insertions(+), 54 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 1a6fe205..948ac87a 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -398,7 +398,7 @@ namespace systems { resetDampingRatioToTarget(); // Construct path to rigidBody JSON file - const std::filesystem::path jsonPath = paths::assets() / "objects" / "RigidBodyic_Arm_Models" / name / (name + ".json"); + const std::filesystem::path jsonPath = paths::assets() / "objects" / "Robotic_Arm_Models" / name / (name + ".json"); if (!std::filesystem::exists(jsonPath)) { LOG_ERROR("RigidBody JSON file not found -> %s", jsonPath.string().c_str()); D_ERROR("RigidBody JSON file not found -> %s", jsonPath.string().c_str()); diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h index 15256f57..7b4ef76a 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/DSLEditorWidget.h @@ -57,9 +57,9 @@ namespace widgets { std::vector _activeRuns; // Vector to hold active runs and their futures gui::SimulationManager* _sim = nullptr; - DSL::Parser* _parser = nullptr; - DSL::IStoredProgram* _program = nullptr; - DSL::RunWrapper* _wrapper = nullptr; + dsl::Parser* _parser = nullptr; + dsl::IStoredProgram* _program = nullptr; + dsl::RunWrapper* _wrapper = nullptr; ConsoleOutputWidget* _log = nullptr; DSLSyntaxHighlighter* _highlighter = nullptr; diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h index ff8736dc..b964754d 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h @@ -12,8 +12,7 @@ namespace gui { struct WorkspaceData { int version = 1; QString name; - - QString rigidBodyName; // empty = no rigid body loaded + QString rigidBodyName; // empty = no rigid body loaded QString scriptText; // DSL script embedded — file is self-contained QString scriptPath; // original script file if one was opened (informational) diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h index 3d315774..88d1db34 100644 --- a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h @@ -40,7 +40,7 @@ namespace scene { // Forward Declarations for Simulation Core namespace core { class ISimulationCore; } -// Forward Declarations for Physics, Robots, Control, and Integration +// Forward Declarations for Physics, RigidBodys, Control, and Integration namespace dsl { class IStoredProgram; } namespace systems { class RigidBodySystem; struct RigidBodyModel; } namespace control { class TrajectoryManager; } @@ -131,11 +131,11 @@ namespace gui { void setViewFollowTarget(ViewID view, scene::Object* obj, const glm::vec3& offset = glm::vec3(0.0f, 0.25f, 1.0f)); void clearViewFollowTarget(ViewID view); - // Follow a robot joint by name (binds the view to that joint's child link object) - bool setViewFollowRobotJoint(ViewID view, const std::string& jointName, const glm::vec3& offset); + // Follow a rigidBody joint by name (binds the view to that joint's child link object) + bool setViewFollowRigidBodyJoint(ViewID view, const std::string& jointName, const glm::vec3& offset); // Convenience: follow in the Follow view - bool followRobotJoint(const std::string& jointName, const glm::vec3& offset = glm::vec3(0.0f, 0.2f, 0.6f)); + bool followRigidBodyJoint(const std::string& jointName, const glm::vec3& offset = glm::vec3(0.0f, 0.2f, 0.6f)); // Mesh loading & Management void loadMesh(const std::string& filepath); @@ -152,7 +152,7 @@ namespace gui { void tick(double dt); void setDisplaySize(uint32_t w, uint32_t h); - void syncRobotToScene(); + void syncRigidBodyToScene(); void syncBodyToScene(); // Scene Objects Management @@ -166,21 +166,21 @@ namespace gui { scene::Object* getObject(); scene::Object* getObjectByID(scene::ObjectID id); - // Robot System loading and management - void load_robot(const std::string& name); - void resetRobot(); - void clearRobot(); - const bool hasRobot() const; + // RigidBody System loading and management + void load_rigidBody(const std::string& name); + void resetRigidBody(); + void clearRigidBody(); + const bool hasRigidBody() const; const bool hasBody() const; - // Setters for robot joint states (angle in radians) - void setRobotLinkRotation(const std::string& linkName, double angle); - void setRobotRootPose(const mathlib::Vec3& pos, mathlib::Quat& rot); - void setRobotRootHome(const mathlib::Vec3& pos, mathlib::Quat& rot); + // Setters for rigidBody joint states (angle in radians) + void setRigidBodyLinkRotation(const std::string& linkName, double angle); + void setRigidBodyRootPose(const mathlib::Vec3& pos, mathlib::Quat& rot); + void setRigidBodyRootHome(const mathlib::Vec3& pos, mathlib::Quat& rot); - // Accesors for the robot system (non-const and const versions) - robots::RobotSystem& robotSystem(); - const robots::RobotSystem& robotSystem() const; + // Accesors for the rigidBody system (non-const and const versions) + systems::RigidBodySystem& rigidBodySystem(); + const systems::RigidBodySystem& rigidBodySystem() const; single_body_system::SingleBodySystem& singleBodySystem(); const single_body_system::SingleBodySystem& singleBodySystem() const; @@ -217,9 +217,9 @@ namespace gui { std::string& lastScriptText() const; // Accessors for the last script text - void setActiveProgram(interpreter::IStoredProgram* program); - interpreter::IStoredProgram* activeProgram(); - const interpreter::IStoredProgram* activeProgram() const; + void setActiveProgram(dsl::IStoredProgram* program); + dsl::IStoredProgram* activeProgram(); + const dsl::IStoredProgram* activeProgram() const; // Run a script to completion synchronously with a specific integrator bool runScriptToCompletion(const std::string& scriptText, integration::eIntegrationMethod method); @@ -262,9 +262,9 @@ namespace gui { // Workspace Management void closeWorkspace(); // Tear down the current workspace: systems, scene, all CPU+GPU meshes. void applyWorkspace(const gui::WorkspaceData& w); // Populate a fresh state from saved data (call after closeWorkspace). - void gatherWorkspace(gui::WorkspaceData& w) const; // Fill the manager-owned parts of a workspace (robot, camera). + void gatherWorkspace(gui::WorkspaceData& w) const; // Fill the manager-owned parts of a workspace (rigidBody, camera). - const std::string& currentRobotName() const { return _currentRobotName; } + const std::string& currentRigidBodyName() const { return _currentRigidBodyName; } private: std::unique_ptr _core = nullptr; @@ -307,8 +307,8 @@ namespace gui { // 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 + systems::JointLogBuffer _jointLogBuffer; // Buffer for logging joint data each step + systems::TrajRefBuffer _trajRefBuffer; // Buffer for logging trajectory reference data each step bool _telemetryBegun = false; // Environment & Lighting @@ -337,12 +337,12 @@ namespace gui { double _simTime = 0.0; double _fixedDt = 1.0 / 180.0; double _telemetryHz = 100.0; - std::string _currentRobotName; + std::string _currentRigidBodyName; scene::Object* _selectedObject = nullptr; std::vector> _objects; - interpreter::IStoredProgram* _activeProgram = nullptr; + dsl::IStoredProgram* _activeProgram = nullptr; integration::eIntegrationMethod _integrationMethod{}; integration::eAutoDiffIntegrationMethod _adIntegrationMethod{}; diff --git a/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h b/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h index 3abc37ac..78c019f5 100644 --- a/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h +++ b/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h @@ -19,9 +19,9 @@ namespace gui { class MeshStore; class SimulationRenderer; - class RigidBodySystem : public ISimulationSystem { + class MultiBodySystem : public ISimulationSystem { public: - RigidBodySystem(const robots::RigidBodyModel& model, + MultiBodySystem(const systems::RigidBodyModel& model, std::function&()> world_src, MeshStore& mesh_store, SimulationRenderer& renderer); @@ -30,11 +30,11 @@ namespace gui { void clear(SimulationScene& scene) override; private: - const robots::RigidBodyModel& _model; + const systems::RigidBodyModel& _model; std::function&()> _world_src; MeshStore& _meshStore; SimulationRenderer& _renderer; - RobotBinding _binding; + RigidBodyBinding _binding; }; } // namespace gui diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 49051c2b..4b2e81bb 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -194,6 +194,7 @@ namespace window { }); } + // Build the robot menu dynamically based on the available robotic systems, using the general RigidBody System interface void DSFE_MainWindow::buildRobotMenu(QMenu* projectMenu) { const auto& robotMap = platform::getRobotSystemMap(); std::unordered_map familyMenus; @@ -207,7 +208,7 @@ namespace window { connect(robotAction, &QAction::triggered, this, [this, robotName]() { LOG_INFO("Menu clicked: Project -> Load Robot -> %s", robotName.toStdString().c_str()); showProjectPage(); // renderer init if we're still on the home page - _sim->load_robot(robotName.toStdString()); + _sim->load_rigidBody(robotName.toStdString()); }); } } diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index cd27ef30..91022caf 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -153,9 +153,9 @@ namespace widgets { delete _parser; _parser = nullptr; delete _program; _program = nullptr; - _program = new interpreter::StoredProgram(_sim->simCore()); - _parser = new interpreter::Parser(_program); - _wrapper = new interpreter::RunWrapper(_parser, _program); + _program = new dsl::StoredProgram(_sim->simCore()); + _parser = new dsl::Parser(_program); + _wrapper = new dsl::RunWrapper(_parser, _program); _scriptText = _scriptEditor->toPlainText().toStdString(); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp index 70b646b7..c0693a0d 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp @@ -36,7 +36,7 @@ namespace widgets { layout()->addWidget(button); connect(button, &QPushButton::clicked, this, [this, robotName]() { LOG_INFO("Selected robot: %s", robotName.toStdString().c_str()); - if (_sim) { _sim->load_robot(robotName.toStdString()); } + if (_sim) { _sim->load_rigidBody(robotName.toStdString()); } }); } } // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index 43a3fed3..3f761dd8 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -13,9 +13,9 @@ #include "SingleBodySystems/SingleBodySystem.h" #include "Platform/ISimulationCore.h" -#include "Interpreter/IStoredProgram.h" -#include "Interpreter/StoredProgram.h" -#include "Interpreter/Parser.h" +#include "DSL/IStoredProgram.h" +#include "DSL/StoredProgram.h" +#include "DSL/Parser.h" #include #include @@ -249,9 +249,9 @@ namespace gui { const diagnostics::TelemetryRecorder& SimulationManager::telemetry() const { return _core->telemetry(); } // Accesors for the active program (if any) - void SimulationManager::setActiveProgram(interpreter::IStoredProgram* program) { _core->setActiveProgram(program); } - interpreter::IStoredProgram* SimulationManager::activeProgram() { return _core->activeProgram(); } - const interpreter::IStoredProgram* SimulationManager::activeProgram() const { return _core->activeProgram(); } + void SimulationManager::setActiveProgram(dsl::IStoredProgram* program) { _core->setActiveProgram(program); } + dsl::IStoredProgram* SimulationManager::activeProgram() { return _core->activeProgram(); } + const dsl::IStoredProgram* SimulationManager::activeProgram() const { return _core->activeProgram(); } // Access the simulation core interface (non-const and const versions) core::ISimulationCore* SimulationManager::simCore() { return _core.get(); } @@ -306,9 +306,9 @@ namespace gui { std::string modifiedScript = replaceIntegratorInScript(scriptText, methodName); // Create program and parser (bound to headless core) - auto program = std::make_unique(_core.get()); + auto program = std::make_unique(_core.get()); //if (scene::Object* o = getObject()) program->setDefaultObject(o); - auto parser = std::make_unique(program.get()); + auto parser = std::make_unique(program.get()); // Parse the modified script and start the program parser->parse(modifiedScript); diff --git a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp index dc7abb3c..295081f7 100644 --- a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp @@ -1,5 +1,5 @@ // DSFE_GUI Systems/MultiBodySystem.cpp -#include "Systems/MutliBodySystem.h" +#include "Systems/MultiBodySystem.h" #include "Simulation/SimulationScene.h" #include "Simulation/MeshStore.h" #include "Simulation/SimulationRenderer.h" @@ -32,7 +32,7 @@ namespace gui { for (const auto& link : _model.links) { auto& renderables = _binding.link_to_renderables[link.name]; for (const auto& entry : link.visual.meshEntries) { - fs::path full = paths::assets() / "objects" / "RigidBodyic_Arm_Models" / entry.meshFile; + fs::path full = paths::assets() / "objects" / "Robotic_Arm_Models" / entry.meshFile; auto meshes = loader.load(full.string()); if (meshes.empty()) { LOG_ERROR("No meshes in %s", full.string().c_str()); diff --git a/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp index 73c9652a..e31406c4 100644 --- a/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp @@ -1,7 +1,7 @@ // DSFE_GUI RobotPresentationBuilder.cpp -#include "Robots/RobotPresentationBuilder.h" +#include "Systems/RigidBodyPresentationBuilder.h" -#include "Robots/RobotModel.h" +#include "Systems/RigidBodyModel.h" #include "Assets/MeshLoader.h" #include "Scene/Object.h" @@ -14,7 +14,7 @@ using namespace systems; namespace fs = std::filesystem; // Build a RobotRenderBinding from a RobotModel by loading the visual meshes for each link -RobotRenderBinding RobotPresentationBuilder::build(const systems::RigidBodyModel& model) { +RigidBodyRenderBinding RigidBodyPresentationBuilder::build(const systems::RigidBodyModel& model) { RigidBodyRenderBinding binding; assets::MeshLoader loader; for (const auto& link : model.links) { From 4daf958dca498fd948b031741c33f18924aa5bf2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 19:44:24 +0100 Subject: [PATCH 008/114] chore: Updated Templates --- DSFE_App/DSFE_GUI/assets/templates/empty.dsfe | 25 +++++++++++++++++++ DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe | 12 ++++----- 2 files changed, 31 insertions(+), 6 deletions(-) create mode 100644 DSFE_App/DSFE_GUI/assets/templates/empty.dsfe diff --git a/DSFE_App/DSFE_GUI/assets/templates/empty.dsfe b/DSFE_App/DSFE_GUI/assets/templates/empty.dsfe new file mode 100644 index 00000000..b56d91b9 --- /dev/null +++ b/DSFE_App/DSFE_GUI/assets/templates/empty.dsfe @@ -0,0 +1,25 @@ +{ + "camera": { + "pitch": -0.06599894911050797, + "pos": [ + 2.5499985218048096, + 1.1508084535598755, + 3.396512269973755 + ], + "yaw": -2.1787872314453125 + }, + "content": { + "rigid_body": "", + "script_path": "", + "script_text": "" + }, + "name": "empty", + "simulation": { + "ad_integration_method": 0, + "auto_diff": false, + "integration_method": 4, + "sim_dt": 0.005555555555555556, + "telemetry_dt": 0.008333333333333333 + }, + "version": 1 +} diff --git a/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe b/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe index 3a145688..f036e5bb 100644 --- a/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe +++ b/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe @@ -1,15 +1,15 @@ { "camera": { - "pitch": -0.06799895316362381, + "pitch": -0.06199825182557106, "pos": [ - 2.5499985218048096, - 1.1508084535598755, - 3.396512269973755 + 3.1640207767486572, + 1.1873780488967896, + 2.983812093734741 ], - "yaw": -2.219787359237671 + "yaw": -8.675688743591309 }, "content": { - "robot": "VISPA", + "rigid_body": "VISPA", "script_path": "", "script_text": "# --------------------------------------------\n# TEMPLATE PROJECT: Airbus VISPA Ready-State Observer Positoion\n# --------------------------------------------\n#\n# MISSION PROFILE\n# ---------------\n# 1. System Initialization & Calibration Check (0.0s - 5.0s) - Before start\n# 2. Nominal Deployment to Ready-State Observer Position (5.0s - 35.0s)\n#\n# JOINT LIMITS\n# ------------\n# j1-j6: +/- ~180 deg (+/-3.14149 rad)\n# v_max: 5.38 deg/s (0.0940 rad/s) hardware limit\n# v_operating: < 1.5 deg/s\n# Q_max: 50 Nm per joint\n# Damping / Friction: 0.2 / 0.05 (all joints)\n#\n# LINK MASSES (kg)\n# ----------------\n# link00 0.627 base adapter (gold coloured)\n# link01 2.328 shoulder yaw\n# link02 3.995 upper arm (0.8m, heaviest link)\n# link03 2.328 elbow\n# link04 3.157 forearm (0.65m)\n# link05 2.695 wrist roll\n# link06 0.924 end-effector flange\n#\n# Create By: Joss Salton\n# GitHub: SaltyJoss\n#\n# --------------------------------------------\n\nload(robot, VISPA)\n\nwait(4.0)\ntrajClear()\nwait(1.0)\n\n# Begins sim run and logging.\nstart()\n\n# Nominal Deployment\nparallel(30.0) {\n trajSet(link01, TRAP, -30.0, 1.5, 5.0)\n trajSet(link02, TRAP, 45.0, 2.0, 5.0)\n trajSet(link03, TRAP, -90.0, 2.0, 5.0)\n trajSet(link04, TRAP, 0.0, 1.0, 3.0)\n trajSet(link05, TRAP, 45.0, 1.5, 4.0)\n trajSet(link06, TRAP, 0.0, 1.0, 3.0)\n}\n\nwait(30.0)\n\n# --------------------------------------------\n# END: ~30 seconds\n# --------------------------------------------" }, From a047e9e81a4d0183924e93fa134a6766e3b568f7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 20:01:03 +0100 Subject: [PATCH 009/114] feat: Added external force application methods to RigidBodySystem --- DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h | 6 ++++++ DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp | 12 ++++++++++++ 2 files changed, 18 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 2c282186..925d3b56 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -95,6 +95,10 @@ namespace systems { // Get pointer to this Systemsystem const RigidBodySystem& getRigidBody() const { return *this; } + // --- External Force Application Methods --- + bool setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce); + void clearExtForces(); + // ---- Joint State Methods --- void computeRigidBodyKinematics(std::vector& world); @@ -256,6 +260,8 @@ namespace systems { SpatialModel _spatialModel; RigidBodyConstModel _constModel; + + std::vector> _pendingExtForces; physics::DynamicsScratch _dynScratch; physics::DynamicsResult _dynResult; diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 948ac87a..fdc78855 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -542,6 +542,18 @@ namespace systems { return _body.links.empty() ? "" : _body.links.front().name; // fallback } + // Method to apply an external force to a specific rigidBody link at a given world point + bool RigidBodySystem::setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) { + if (!_hasBody) { return false; } + auto it = _link_idx.find(linkName); + if (it == _link_idx.end()) { return false; } + _pendingExtForces.emplace_back(it->second, worldPoint, worldForce); + return true; + } + + // Method to clear all pending external forces applied to rigidBody links + void RigidBodySystem::clearExtForces() { _pendingExtForces.clear(); } + // Method to update the pose of each rigidBody link based on current joint angles using forward kinematics void RigidBodySystem::computeRigidBodyKinematics(std::vector& world) { if (!_hasBody) { From 26060dc4695d2266a371a9c1ac092491f7960845 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 21:11:00 +0100 Subject: [PATCH 010/114] feat: Implemented external force assembly methods in RigidBodySystem --- .../include/Systems/RigidBodySystem.h | 8 ++++++- .../include/Systems/RigidBodySystemStep.inl | 24 +++++++++++++++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 925d3b56..a4770905 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -96,7 +96,10 @@ namespace systems { const RigidBodySystem& getRigidBody() const { return *this; } // --- External Force Application Methods --- + + bool linkWorldOrigin(const std::string& linkName, mathlib::Vec3& out) const; bool setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce); + bool setLinkExternalForce(const std::string& linkName, const mathlib::Vec3& worldForce); void clearExtForces(); // ---- Joint State Methods --- @@ -210,6 +213,9 @@ namespace systems { template void postStepUpdate(const mathlib::VecX& x, const physics::DynamicsScratch& scratch, const RigidBodyStepResult_T& result); + template + void assembleExtForces(physics::DynamicsScratch& scratch) const; + std::unique_ptr _kinematics; std::unique_ptr _dynamics; @@ -261,7 +267,7 @@ namespace systems { SpatialModel _spatialModel; RigidBodyConstModel _constModel; - std::vector> _pendingExtForces; + std::vector> _pendingExtForces; physics::DynamicsScratch _dynScratch; physics::DynamicsResult _dynResult; diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index fc640418..1602a26f 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -226,6 +226,30 @@ namespace systems { } } + template + void RigidBodySystem::assembleExtForces(physics::DynamicsScratch& dynScratch) { + const size_t n = _body.joints.size(); + scratch.spatial.f_ext.assign(n, mathlib::SpatialVec_T()); // reset external forces + if (_pendingExtForces.empty()) { return; } + for (const auto& [jointIdx, linkIdx, worldPoint, worldForce] : _pendingExtForces) { + // Link world pose from last kinematics update + const Mat4& T = _worldTransforms[linkIdx]; + const Mat3 R = T.block<3, 3>(0, 0); + const Vec3 o = T.block<3, 1>(0, 3); + // Transform world force to link frame + const Vec3 F_link = R.transpose() * worldForce.template cast(); + const Vec3 r_world = worldPoint.template cast() - o; + const Vec3 moment_world = r_world.cross(worldForce.template cast()); + const Vec3 M_link = R.transpose() * moment_world; + // Compute spatial force in link frame + mathlib::SpatialVec_T fs( + M_link.template cast(), // angular slot (moment) + F_link.template cast() // linear slot (force) + ); + scratch.spatial.f_ext[jointIdx] += fs; // accumulate external force for this joint + } + } + template void RigidBodySystem::step_AD(double dt, double simTime) { if (!hasRigidBody()) { return; } From 02862fd22ed96592922503d229371510f3540e0e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 21:11:10 +0100 Subject: [PATCH 011/114] feat: Enhanced RigidBodySystem with external force handling and link origin retrieval --- .../include/Physics/SpatialDynamics.inl | 2 ++ .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 28 +++++++++++++++++-- 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index ed154684..360cc90e 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -297,9 +297,11 @@ namespace physics { scratch.spatial.c ); + const bool hasExt = (scratch.spatial.f_ext.size() == n); for (size_t i = 0; i < n; ++i) { scratch.spatial.IA[i] = model.joints[i].inertia; // Articulated Body Inertia scratch.spatial.pA[i] = crossForce(scratch.spatial.v[i], (scratch.spatial.IA[i] * scratch.spatial.v[i])); + if (hasExt) { scratch.spatial.pA[i] += scratch.spatial.f_ext[i]; } } // Compute articulated body inertias and bias forces diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index fdc78855..b286e26d 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -326,7 +326,9 @@ namespace systems { const size_t n = _body.joints.size(); mathlib::VecX x = packState(); + assembleExtForces(_dynScratch); auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); + clearExtForces(); unpackState(result.stepOut.x_next); _dynamics->setDt(result.stepOut.dt_taken); @@ -542,15 +544,37 @@ namespace systems { return _body.links.empty() ? "" : _body.links.front().name; // fallback } + // Method to get the world origin of a specific rigidBody link by name + bool RigidBodySystem::linkWorldOrigin(const std::string& linkName, mathlib::Vec3& outOrigin) const { + auto it = _linkIndex.find(linkName); + if (it == _linkIndex.end()) { return false; } + out = _worldTransforms[it->second].block<3,1>(0,3); + return true; + } // Method to apply an external force to a specific rigidBody link at a given world point bool RigidBodySystem::setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) { if (!_hasBody) { return false; } auto it = _link_idx.find(linkName); if (it == _link_idx.end()) { return false; } - _pendingExtForces.emplace_back(it->second, worldPoint, worldForce); + const int linkIdx = it->second; + + // Find the joint whose child is this link — that's the body ABA indexes. + int jointIdx = -1; + for (size_t j = 0; j < _robot.joints.size(); ++j) { + auto cit = _linkIndex.find(_robot.joints[j].child); + if (cit != _linkIndex.end() && cit->second == linkIdx) { jointIdx = (int)j; break; } + } + if (jointIdx < 0) { return false; } // root/base link: no governing joint (see note) + + _pendingExtForces.emplace_back(jointIdx, linkIdx, worldPoint, worldForce); return true; } - + // Overload to apply an external force to a specific rigidBody link at its world origin + bool RigidBodySystem::setLinkExternalForce(const std::string& linkName, const mathlib::Vec3& worldForce) { + mathlib::Vec3 o; + if (!linkWorldOrigin(linkName, o)) { return false; } + return setLinkExternalForce(linkName, o, worldForce); + } // Method to clear all pending external forces applied to rigidBody links void RigidBodySystem::clearExtForces() { _pendingExtForces.clear(); } From 3c963aad5dc6ddd5e99f3a90d73b7b6dff82e7ea Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 21:11:16 +0100 Subject: [PATCH 012/114] feat: Added manipulation control and external force handling to SimulationCore --- .../include/Platform/ISimulationCore.h | 6 +++++ .../DSFE_Core/include/Scene/SimulationCore.h | 7 ++++++ .../DSFE_Core/src/Scene/SimulationCore.cpp | 23 ++++++++++++++++++- 3 files changed, 35 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 9c2a869e..8284a167 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -80,5 +80,11 @@ namespace core { // Access to the active program (if any) virtual void setActiveProgram(dsl::IStoredProgram* program) = 0; virtual dsl::IStoredProgram* activeProgram() const = 0; + // Access the Free-Dynamics manipulation state (for external control of the rigidBody) + virtual void setManipulating(bool on) = 0; + virtual bool isManipulating() const = 0; + virtual bool setLinkExternalForce(const std::string& link, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) = 0; + virtual const std::vector& linkWorldTransforms() const = 0; + virtual std::vector linkNames() const = 0; }; } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 81f4d29b..298f891e 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -141,6 +141,12 @@ namespace core { bool rigidBodyPresentationDirty() const override { return _rigidBodyPresentationDirty; } void clearRigidBodyPresentationDirty() override { _rigidBodyPresentationDirty = false; } + void setManipulating(bool on) override; + bool isManipulating() const override { return _manipulating.load(); } + bool setLinkExternalForce(const std::string& link, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) override; + const std::vector& linkWorldTransforms() const override; + std::vector linkNames() const override; + private: // Export thread management void exportThreadMain(); @@ -175,6 +181,7 @@ namespace core { std::atomic _simRunning{ false }; // Whether the simulation loop is currently running std::atomic _scriptRunning{ false }; // Whether a script is currently running std::atomic _exportsInFlight = 0; + std::atomic _manipulating{ false }; // Run mode eRunMode _runMode = eRunMode::Interactive; diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index af558ebd..35c3587c 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -148,7 +148,6 @@ namespace core { // Update physics and rigidBody system if sim is running if (_simRunning.load()) { simTime += _dt; - // Update rigidBody trajectory inputs and step the rigidBody forward in time if (hasRigidBody()) { _rigidBody->updateTrajectoryInputs(*_traj, simTime); @@ -163,6 +162,10 @@ namespace core { } if (hasSingleBody()) { _singleBody->step(_dt, simTime); } } + else if (_manipulating.load() && hasRigidBody()) { + _rigidBody->step(_dt, simTime); + } + _accum -= _dt; // decrease accumulator by fixed timestep until we catch up to the current frame time } @@ -446,6 +449,14 @@ namespace core { if (!_rigidBody) { LOG_ERROR("Cannot load rigidBody: RigidBodySystem not set"); return; } _rigidBody->loadRigidBody(name); } + // Sets an external force on a specific link of the rigidBody system at a given world point + bool SimulationCore::setLinkExternalForce(const std::string& link, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) { + return _rigidBody->setLinkExtForce(link, worldPoint, worldForce); + } + // Accessor for the world transforms of the rigidBody links (const version) + const std::vector& SimulationCore::linkWorldTransforms() const { return _rigidBody->worldTransforms(); } + // Accessor for the names of the rigidBody links (const version) + std::vector SimulationCore::linkNames() const { return _rigidBody->linkNames(); } // Accessor for the single body system (non-const and const versions) single_body_system::SingleBodySystem& SimulationCore::singleBodySystem() { return *_singleBody; } @@ -540,4 +551,14 @@ namespace core { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } + + void SimulationCore::setManipulating(bool on) { + if (on == _manipulating.load()) { return; } + if (on) { + setupSimulationIntegrator(); + _accum = 0.0; + } + _manipulating.store(on); + if (!on) { _rigidBody->clearExternalForces(); } // drop any residual drag force + } } \ No newline at end of file From 76d836a184a3d90b2b3fb356edd75a011fd9b530 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 21:43:37 +0100 Subject: [PATCH 013/114] fixes: Corrected link name retrieval and refactored external force methods in RigidBodySystem --- .../include/Systems/RigidBodySystem.h | 4 +++- .../include/Systems/RigidBodySystemStep.inl | 2 +- .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 21 ++++++++++++------- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index a4770905..200c330c 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -70,6 +70,8 @@ namespace systems { const std::vector& joints() const { return _body.joints; } std::vector& joints() { return _body.joints; } + std::vector linkNames() const; + std::size_t linkCount() const { return _body.links.size(); } std::size_t jointCount() const { return _body.joints.size(); } @@ -99,7 +101,7 @@ namespace systems { bool linkWorldOrigin(const std::string& linkName, mathlib::Vec3& out) const; bool setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce); - bool setLinkExternalForce(const std::string& linkName, const mathlib::Vec3& worldForce); + bool setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldForce); void clearExtForces(); // ---- Joint State Methods --- diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index 1602a26f..99af62a5 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -227,7 +227,7 @@ namespace systems { } template - void RigidBodySystem::assembleExtForces(physics::DynamicsScratch& dynScratch) { + void RigidBodySystem::assembleExtForces(physics::DynamicsScratch& scratch) const { const size_t n = _body.joints.size(); scratch.spatial.f_ext.assign(n, mathlib::SpatialVec_T()); // reset external forces if (_pendingExtForces.empty()) { return; } diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index b286e26d..e3df4401 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -35,6 +35,11 @@ namespace systems { RigidBodySystem::~RigidBodySystem() = default; const systems::RigidBodyModel& RigidBodySystem::model() const { return _body; } + std::vector RigidBodySystem::linkNames() const { + std::vector names; + for (const auto& link : _body.links) { names.push_back(link.name); } + return names; + } // Helper function to convert std::vector to Eigen::VectorXd static VecX toVecX(const std::vector& a) { @@ -546,9 +551,9 @@ namespace systems { // Method to get the world origin of a specific rigidBody link by name bool RigidBodySystem::linkWorldOrigin(const std::string& linkName, mathlib::Vec3& outOrigin) const { - auto it = _linkIndex.find(linkName); - if (it == _linkIndex.end()) { return false; } - out = _worldTransforms[it->second].block<3,1>(0,3); + auto it = _link_idx.find(linkName); + if (it == _link_idx.end()) { return false; } + outOrigin = _worldTransforms[it->second].block<3,1>(0,3); return true; } // Method to apply an external force to a specific rigidBody link at a given world point @@ -560,9 +565,9 @@ namespace systems { // Find the joint whose child is this link — that's the body ABA indexes. int jointIdx = -1; - for (size_t j = 0; j < _robot.joints.size(); ++j) { - auto cit = _linkIndex.find(_robot.joints[j].child); - if (cit != _linkIndex.end() && cit->second == linkIdx) { jointIdx = (int)j; break; } + for (size_t j = 0; j < _body.joints.size(); ++j) { + auto cit = _link_idx.find(_body.joints[j].child); + if (cit != _link_idx.end() && cit->second == linkIdx) { jointIdx = (int)j; break; } } if (jointIdx < 0) { return false; } // root/base link: no governing joint (see note) @@ -570,10 +575,10 @@ namespace systems { return true; } // Overload to apply an external force to a specific rigidBody link at its world origin - bool RigidBodySystem::setLinkExternalForce(const std::string& linkName, const mathlib::Vec3& worldForce) { + bool RigidBodySystem::setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldForce) { mathlib::Vec3 o; if (!linkWorldOrigin(linkName, o)) { return false; } - return setLinkExternalForce(linkName, o, worldForce); + return setLinkExtForce(linkName, o, worldForce); } // Method to clear all pending external forces applied to rigidBody links void RigidBodySystem::clearExtForces() { _pendingExtForces.clear(); } From e08d5764db9b28a6107f66f4b906c5756595db69 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 21:44:04 +0100 Subject: [PATCH 014/114] fixes: Updated include directive for MathLib and modified `clearExternalForces` method to `clearExtForces` in SimulationCore --- DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h | 2 +- DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 8284a167..a267fafd 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -2,7 +2,7 @@ #pragma once #include "EngineCore.h" - +#include #include #include diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 35c3587c..a64410a1 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -559,6 +559,6 @@ namespace core { _accum = 0.0; } _manipulating.store(on); - if (!on) { _rigidBody->clearExternalForces(); } // drop any residual drag force + if (!on) { _rigidBody->clearExtForces(); } // drop any residual drag force } } \ No newline at end of file From 451cb0096f867b2685eaee934662f8607431201e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 21:44:15 +0100 Subject: [PATCH 015/114] feat: Add manipulation control and link external force handling in SimulationManager --- DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h | 7 +++++++ DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp | 8 ++++++++ 2 files changed, 15 insertions(+) diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h index 88d1db34..6b3e109e 100644 --- a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h @@ -266,6 +266,13 @@ namespace gui { const std::string& currentRigidBodyName() const { return _currentRigidBodyName; } + void setManipulating(bool on); + bool isManipulating() const; + bool setLinkExternalForce(const std::string& link, const glm::vec3& worldPoint, const glm::vec3& worldForce); + const std::vector& linkWorldTransforms() const; + std::vector linkNames() const; + scene::Camera& camera() { return _camera; } + private: std::unique_ptr _core = nullptr; diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index 3f761dd8..42a6b1a0 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -391,4 +391,12 @@ namespace gui { w.cameraYaw = _camera.getYaw(); w.cameraPitch = _camera.getPitch(); } + + void SimulationManager::setManipulating(bool on) { _core->setManipulating(on); } + bool SimulationManager::isManipulating() const { return _core->isManipulating(); } + bool SimulationManager::setLinkExternalForce(const std::string& link, const glm::vec3& p, const glm::vec3& f) { + return _core->setLinkExternalForce(link, mathlib::Vec3(p.x, p.y, p.z), mathlib::Vec3(f.x, f.y, f.z)); + } + const std::vector& SimulationManager::linkWorldTransforms() const { return _core->linkWorldTransforms(); } + std::vector SimulationManager::linkNames() const { return _core->linkNames(); } } From b62242fd6861bf96c2f574fad7d5323a1c96eb57 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 21:44:43 +0100 Subject: [PATCH 016/114] feat: Implement link picking and dragging functionality in ViewportWidget * Needs work, is not where I want it --- .../MainWindow/Widgets/ViewportWidget.h | 8 ++ .../src/MainWindow/Widgets/ViewportWidget.cpp | 87 +++++++++++++++++-- 2 files changed, 90 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h index ec3327dc..c53fb1f7 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ViewportWidget.h @@ -1,6 +1,7 @@ // DSFE_GUI ViewportWidget.h #pragma once +#include #include #include #include @@ -11,6 +12,7 @@ #include #include #include +#include #include @@ -40,12 +42,18 @@ namespace widgets { void initialise_renderer(); private: + std::string pickLink(float sx, float sy) const; + glm::vec3 cursorToDragPlane(float sx, float sy) const; + glm::vec3 linkOrigin(const std::string& name) const; + gui::SimulationManager* _sim = nullptr; QElapsedTimer _frameTimer; qint64 _lastNs = 0; QTimer _updateTimer; bool _renderer_initialised = false; bool _mouse_captured = false; + bool _dragging = false; + std::string _dragLink; QPoint _screenCenter; std::unordered_set _pressedKeys; }; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp index 9428e611..57acac0b 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -134,6 +134,11 @@ namespace widgets { grabMouse(); if (_sim) { _sim->resetMouseDelta(); } } + else if (event->button() == Qt::LeftButton && !_mouse_captured && _sim) { + // Pick the link whose world origin is nearest the cursor ray. + _dragLink = pickLink(event->position().x(), event->position().y()); + if (!_dragLink.empty()) { _dragging = true; _sim->setManipulating(true); } + } } void ViewportWidget::mouseReleaseEvent(QMouseEvent* event) { @@ -142,14 +147,29 @@ namespace widgets { releaseMouse(); unsetCursor(); } + else if (event->button() == Qt::LeftButton && _dragging) { + _dragging = false; + _dragLink.clear(); + if (_sim) { _sim->setManipulating(false); } + } } void ViewportWidget::mouseMoveEvent(QMouseEvent* event) { - if (!_sim || !_mouse_captured) { return; } - QPoint current = QCursor::pos(); - QPoint delta = current - _screenCenter; - _sim->handleMouseLook(delta.x(), -delta.y(), true); - QCursor::setPos(_screenCenter); + if (!_sim) { return; } + if (_mouse_captured) { + QPoint current = QCursor::pos(); + QPoint delta = current - _screenCenter; + _sim->handleMouseLook(delta.x(), -delta.y(), true); + QCursor::setPos(_screenCenter); + return; + } + if (_dragging && !_dragLink.empty()) { + // Unproject the cursor onto a view-parallel plane through the grab point, then push the link toward it with a spring. + const glm::vec3 target = cursorToDragPlane(event->position().x(), event->position().y()); + const glm::vec3 grab = linkOrigin(_dragLink); + const glm::vec3 force = 800.0f * (target - grab); // spring; tune stiffness + _sim->setLinkExternalForce(_dragLink, grab, force); + } } void ViewportWidget::wheelEvent(QWheelEvent* event) { @@ -157,4 +177,61 @@ namespace widgets { _sim->onMouseWheel(event->angleDelta().y() / 120.0); } + glm::vec3 ViewportWidget::linkOrigin(const std::string& name) const { + const auto& xf = _sim->linkWorldTransforms(); + const auto names = _sim->linkNames(); + for (size_t i = 0; i < names.size() && i < xf.size(); ++i) { + if (names[i] == name) { + const mathlib::Mat4& m = xf[i]; + return glm::vec3((float)m(0,3), (float)m(1,3), (float)m(2,3)); + } + } + return glm::vec3(0.0f); + } + + // Build a world-space ray from the cursor, return the nearest link name. + std::string ViewportWidget::pickLink(float sx, float sy) const { + scene::Camera& cam = _sim->camera(); + const glm::mat4 invVP = glm::inverse(cam.getProjection() * cam.getViewMatrix()); + // NDC (Vulkan Y-down: flip y). Near/far points -> ray. + const float ndcX = 2.0f * (sx / (float)width()) - 1.0f; + const float ndcY = 2.0f * (sy / (float)height()) - 1.0f; // no extra flip: screen y-down matches + glm::vec4 pNear = invVP * glm::vec4(ndcX, ndcY, 0.0f, 1.0f); + glm::vec4 pFar = invVP * glm::vec4(ndcX, ndcY, 1.0f, 1.0f); + pNear /= pNear.w; pFar /= pFar.w; + const glm::vec3 o(pNear); + const glm::vec3 d = glm::normalize(glm::vec3(pFar) - glm::vec3(pNear)); + const auto& xf = _sim->linkWorldTransforms(); + const auto names = _sim->linkNames(); + std::string best; float bestDist = 1e9f; + for (size_t i = 0; i < names.size() && i < xf.size(); ++i) { + const glm::vec3 p((float)xf[i](0,3), (float)xf[i](1,3), (float)xf[i](2,3)); + // distance from link origin to the ray + const float t = glm::dot(p - o, d); + if (t < 0.0f) { continue; } + const float perp = glm::length((o + d * t) - p); + if (perp < bestDist && perp < 0.5f) { bestDist = perp; best = names[i]; } // 0.5m pick radius + } + return best; + } + + // Project the cursor onto a plane through the grab point, normal = camera forward. + glm::vec3 ViewportWidget::cursorToDragPlane(float sx, float sy) const { + scene::Camera& cam = _sim->camera(); + const glm::mat4 invVP = glm::inverse(cam.getProjection() * cam.getViewMatrix()); + const float ndcX = 2.0f * (sx / (float)width()) - 1.0f; + const float ndcY = 2.0f * (sy / (float)height()) - 1.0f; + glm::vec4 pNear = invVP * glm::vec4(ndcX, ndcY, 0.0f, 1.0f); + glm::vec4 pFar = invVP * glm::vec4(ndcX, ndcY, 1.0f, 1.0f); + pNear /= pNear.w; pFar /= pFar.w; + const glm::vec3 o(pNear); + const glm::vec3 d = glm::normalize(glm::vec3(pFar) - glm::vec3(pNear)); + const glm::vec3 planePt = linkOrigin(_dragLink); + const glm::vec3 n = -cam.getForward(); // plane faces the camera + const float denom = glm::dot(d, n); + if (std::abs(denom) < 1e-6f) { return planePt; } + const float t = glm::dot(planePt - o, n) / denom; + return o + d * t; + } + } // namespace widgets \ No newline at end of file From 0415b64eeb01e7ac07ca8fa9d6029ea86c0a2d6c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 23:00:34 +0100 Subject: [PATCH 017/114] feat: Added `clearExternalForces` method to ISimulationCore and SimulationCore --- DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h | 1 + DSFE_App/DSFE_Core/include/Scene/SimulationCore.h | 1 + 2 files changed, 2 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index a267fafd..20cac370 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -84,6 +84,7 @@ namespace core { virtual void setManipulating(bool on) = 0; virtual bool isManipulating() const = 0; virtual bool setLinkExternalForce(const std::string& link, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) = 0; + virtual void clearExternalForces() = 0; virtual const std::vector& linkWorldTransforms() const = 0; virtual std::vector linkNames() const = 0; }; diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 298f891e..32fa4342 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -144,6 +144,7 @@ namespace core { void setManipulating(bool on) override; bool isManipulating() const override { return _manipulating.load(); } bool setLinkExternalForce(const std::string& link, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) override; + void clearExternalForces() override; const std::vector& linkWorldTransforms() const override; std::vector linkNames() const override; From ab3a76581d5018ce5080db6ae6fc193c4c03efe8 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 23:01:00 +0100 Subject: [PATCH 018/114] feat: Added `clearExternalForces` method to SimulationCore for resetting external forces --- DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index a64410a1..d9a811ac 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -457,6 +457,8 @@ namespace core { const std::vector& SimulationCore::linkWorldTransforms() const { return _rigidBody->worldTransforms(); } // Accessor for the names of the rigidBody links (const version) std::vector SimulationCore::linkNames() const { return _rigidBody->linkNames(); } + // Clears all external forces applied to the rigidBody system + void SimulationCore::clearExternalForces() { _rigidBody->clearExtForces(); } // Accessor for the single body system (non-const and const versions) single_body_system::SingleBodySystem& SimulationCore::singleBodySystem() { return *_singleBody; } From f50e9d7e9013dba42512069e3edafd7cdea8fa1e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 23:01:21 +0100 Subject: [PATCH 019/114] feat: Added AABB properties and `linkWorldMinY` method for rigid body links --- .../include/Systems/RigidBodyModel.h | 6 +++ .../include/Systems/RigidBodySystem.h | 3 ++ .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 37 ++++++++++++++++++- 3 files changed, 45 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h index d9b52e50..be4b6bda 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h @@ -95,6 +95,12 @@ namespace systems { Visual visual{}; std::vector collisions; Inertial inertial{}; + + // Axis-aligned bounding box (AABB) for the link, in world coordinates + // Placeholders, kind of, if using GUI these are filled at loadtime + mathlib::Vec3 aabbMin{ 0,0,0 }; + mathlib::Vec3 aabbMax{ 0,0,0 }; + bool hasBounds = false; }; // --- RigidBody Model Joints --- diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 200c330c..19d2e25c 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -270,6 +270,9 @@ namespace systems { RigidBodyConstModel _constModel; std::vector> _pendingExtForces; + std::vector _prevLinkY; // last-step link heights for floor damping + + double linkWorldMinY(size_t linkIdx, const Mat4& T) const; physics::DynamicsScratch _dynScratch; physics::DynamicsResult _dynResult; diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index e3df4401..0b3770d9 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -318,6 +318,20 @@ namespace systems { if (j.q > hi) { j.q = hi; if (j.qd > 0.0f) { j.qd = 0.0f; }} } + double RigidBodySystem::linkWorldMinY(size_t linkIdx, const Mat4& T) const { + const RigidBodyLink& L = _body.links[linkIdx]; + if (!L.hasBounds) { return T(1,3); } // no geometry -> fall back to origin + double minY = 1e30; + for (int c = 0; c < 8; ++c) { + const double x = (c & 1) ? L.aabbMax.x() : L.aabbMin.x(); + const double y = (c & 2) ? L.aabbMax.y() : L.aabbMin.y(); + const double z = (c & 4) ? L.aabbMax.z() : L.aabbMin.z(); + const double wy = T(1,0)*x + T(1,1)*y + T(1,2)*z + T(1,3); + if (wy < minY) { minY = wy; } + } + return minY; + } + // Method to advance the rigidBody state by dt using the selected integrator void RigidBodySystem::step(double dt, double simTime) { if (!_hasBody) { return; } @@ -330,7 +344,28 @@ namespace systems { _simTime = simTime; const size_t n = _body.joints.size(); mathlib::VecX x = packState(); - + // Apply floor contact forces if enabled (Bit crude but yeah) + { + constexpr double k_floor = 400000.0; + constexpr double c_floor = 2000.0; + const size_t nl = _body.links.size(); + if (_prevLinkY.size() != nl) { _prevLinkY.assign(nl, 0.0); } + for (size_t i = 0; i < nl; ++i) { + const Mat4& T = _worldTransforms[i]; + const double lowY = linkWorldMinY(i, T); + const double vy = (lowY - _prevLinkY[i]) / (_dynamics->dt() > 0 ? _dynamics->dt() : (1.0/180.0)); + _prevLinkY[i] = lowY; + if (lowY < 0.0) { + double Fy = -k_floor * lowY - c_floor * vy; + if (Fy < 0.0) { Fy = 0.0; } // floor only pushes, never pulls + setLinkExtForce( + _body.links[i].name, + Vec3(T(0,3), lowY, T(2,3)), + Vec3(0.0, Fy, 0.0) + ); + } + } + } assembleExtForces(_dynScratch); auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); clearExtForces(); From 6bd2d96184a7665a0384539a16cacbf0bf10f184 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 23:01:35 +0100 Subject: [PATCH 020/114] feat: Added renderable retrieval method to SimulationScene --- DSFE_App/DSFE_GUI/include/Simulation/SimulationScene.h | 1 + 1 file changed, 1 insertion(+) diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationScene.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationScene.h index ccfb498f..a97e14cd 100644 --- a/DSFE_App/DSFE_GUI/include/Simulation/SimulationScene.h +++ b/DSFE_App/DSFE_GUI/include/Simulation/SimulationScene.h @@ -18,6 +18,7 @@ namespace gui { uint32_t add_renderable(uint32_t mesh_id, const glm::mat4& transform); void set_material(uint32_t idx, const glm::vec3& albedo, float metallic, float roughness, float ao=1.0f); void set_transform(uint32_t idx, const glm::mat4& transform); + const Renderable* renderable(uint32_t idx) const { return idx < _renderables.size() ? &_renderables[idx] : nullptr; } const std::vector& renderables() const { return _renderables; } void clear() { _renderables.clear(); } From c1ce07d825dd5d61241de60544a8198201c80fbc Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 23:01:42 +0100 Subject: [PATCH 021/114] feat: Calculated AABB bounds for links in MultiBodySystem during build --- .../DSFE_GUI/src/Systems/MultiBodySystem.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp index 295081f7..7ec73830 100644 --- a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp @@ -31,17 +31,20 @@ namespace gui { namespace fs = std::filesystem; for (const auto& link : _model.links) { auto& renderables = _binding.link_to_renderables[link.name]; + glm::vec3 lo(1e30f), hi(-1e30f); + bool anyVerts = false; for (const auto& entry : link.visual.meshEntries) { fs::path full = paths::assets() / "objects" / "Robotic_Arm_Models" / entry.meshFile; auto meshes = loader.load(full.string()); - if (meshes.empty()) { - LOG_ERROR("No meshes in %s", full.string().c_str()); - continue; - } + if (meshes.empty()) { LOG_ERROR("No meshes in %s", full.string().c_str()); continue; } for (auto& mptr : meshes) { scene::Mesh& src = *mptr; if (src._vertices.empty()) { continue; } - + for (const auto& v : src._vertices) { + lo = glm::min(lo, glm::vec3(v._pos.x, v._pos.y, v._pos.z)); + hi = glm::max(hi, glm::vec3(v._pos.x, v._pos.y, v._pos.z)); + anyVerts = true; + } std::vector indices(src._indices.begin(), src._indices.end()); const uint32_t cpu_id = _meshStore.add(src); const uint32_t gpu_id = _renderer.upload(_meshStore.get(cpu_id)->_vertices, indices); @@ -59,6 +62,12 @@ namespace gui { renderables.push_back(r_idx); } } + if (anyVerts) { + auto& L = const_cast(link); + L.aabbMin = mathlib::Vec3(lo.x, lo.y, lo.z); + L.aabbMax = mathlib::Vec3(hi.x, hi.y, hi.z); + L.hasBounds = true; + } } } From e8d2685a19001c21086d95e2f63825406b6d07c1 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 23:01:51 +0100 Subject: [PATCH 022/114] feat: Implemented link highlighting and clear external forces methods in SimulationManager --- .../include/Simulation/SimulationManager.h | 8 +++++ .../src/Simulation/SimulationManager.cpp | 29 +++++++++++++++++++ 2 files changed, 37 insertions(+) diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h index 6b3e109e..74c54b24 100644 --- a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h @@ -269,10 +269,13 @@ namespace gui { void setManipulating(bool on); bool isManipulating() const; bool setLinkExternalForce(const std::string& link, const glm::vec3& worldPoint, const glm::vec3& worldForce); + void clearExternalForces(); const std::vector& linkWorldTransforms() const; std::vector linkNames() const; scene::Camera& camera() { return _camera; } + void setLinkHighlight(const std::string& link, bool on); + private: std::unique_ptr _core = nullptr; @@ -305,6 +308,11 @@ namespace gui { float planeY = 2.5f; glm::vec3 planeNormal{ 0.0f, 1.0f, 0.0f }; + // Highlighting + int _highlightIdx = -1; + glm::vec3 _highlightAlbedo0{1.0f}; + glm::vec4 _highlightMat0{0.0f}; + // Objects & Scene Management scene::ObjectID _nextObjectID = scene::FIRST_VALID_OBJECT_ID; // Next available ObjectID diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index 42a6b1a0..dcb26255 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -397,6 +397,35 @@ namespace gui { bool SimulationManager::setLinkExternalForce(const std::string& link, const glm::vec3& p, const glm::vec3& f) { return _core->setLinkExternalForce(link, mathlib::Vec3(p.x, p.y, p.z), mathlib::Vec3(f.x, f.y, f.z)); } + void SimulationManager::clearExternalForces() { _core->clearExternalForces(); } const std::vector& SimulationManager::linkWorldTransforms() const { return _core->linkWorldTransforms(); } std::vector SimulationManager::linkNames() const { return _core->linkNames(); } + + // Set a highlight color for a specific link in the rigidBody system. This is typically used to visually indicate selection or focus on a particular link in the GUI. + void SimulationManager::setLinkHighlight(const std::string& link, bool on) { + const auto names = _core->linkNames(); + int idx = -1; + for (size_t i = 0; i < names.size(); ++i) { if (names[i] == link) { idx = (int)i; break; } } + if (idx < 0) { return; } + if (on) { + // Cache original, then tint faint blue. + if (const auto* r = _scene.renderable((uint32_t)idx)) { + _highlightIdx = idx; + _highlightAlbedo0 = glm::vec3(r->albedo); + _highlightMat0 = r->material; + } + _scene.set_material( + (uint32_t)idx, glm::vec3(0.6f, 1.0f, 0.3f), + _highlightMat0.x, _highlightMat0.y, _highlightMat0.z + ); + } + else if (_highlightIdx == idx) { + // Restore. + _scene.set_material( + (uint32_t)idx, + _highlightAlbedo0, _highlightMat0.x, _highlightMat0.y, _highlightMat0.z + ); + _highlightIdx = -1; + } + } } From 312a906ebdba8fa6accaeb021b180b5d27e3ca95 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sun, 26 Jul 2026 23:01:58 +0100 Subject: [PATCH 023/114] feat: Enhanced link manipulation with highlighting and force limits in ViewportWidget --- .../src/MainWindow/Widgets/ViewportWidget.cpp | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp index 57acac0b..1cda4023 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -137,7 +137,11 @@ namespace widgets { else if (event->button() == Qt::LeftButton && !_mouse_captured && _sim) { // Pick the link whose world origin is nearest the cursor ray. _dragLink = pickLink(event->position().x(), event->position().y()); - if (!_dragLink.empty()) { _dragging = true; _sim->setManipulating(true); } + if (!_dragLink.empty()) { + _dragging = true; + _sim->setManipulating(true); + _sim->setLinkHighlight(_dragLink, true); + } } } @@ -149,8 +153,9 @@ namespace widgets { } else if (event->button() == Qt::LeftButton && _dragging) { _dragging = false; + if (_sim && !_dragLink.empty()) { _sim->setLinkHighlight(_dragLink, false); } _dragLink.clear(); - if (_sim) { _sim->setManipulating(false); } + if (_sim) { _sim->clearExternalForces(); } } } @@ -165,9 +170,12 @@ namespace widgets { } if (_dragging && !_dragLink.empty()) { // Unproject the cursor onto a view-parallel plane through the grab point, then push the link toward it with a spring. - const glm::vec3 target = cursorToDragPlane(event->position().x(), event->position().y()); - const glm::vec3 grab = linkOrigin(_dragLink); - const glm::vec3 force = 800.0f * (target - grab); // spring; tune stiffness + glm::vec3 target = cursorToDragPlane(event->position().x(), event->position().y()); + if (target.y < 0.0f) { target.y = 0.0f; } // never drag a link below the floor plane + const glm::vec3 grab = linkOrigin(_dragLink); + glm::vec3 force = 400.0f * (target - grab); // spring; tune stiffness + const float fmax = 3000.0f; + if (glm::length(force) > fmax) { force = glm::normalize(force) * fmax; } _sim->setLinkExternalForce(_dragLink, grab, force); } } @@ -194,7 +202,7 @@ namespace widgets { scene::Camera& cam = _sim->camera(); const glm::mat4 invVP = glm::inverse(cam.getProjection() * cam.getViewMatrix()); // NDC (Vulkan Y-down: flip y). Near/far points -> ray. - const float ndcX = 2.0f * (sx / (float)width()) - 1.0f; + const float ndcX = 2.0f * (sx / (float)width()) - 1.0f; const float ndcY = 2.0f * (sy / (float)height()) - 1.0f; // no extra flip: screen y-down matches glm::vec4 pNear = invVP * glm::vec4(ndcX, ndcY, 0.0f, 1.0f); glm::vec4 pFar = invVP * glm::vec4(ndcX, ndcY, 1.0f, 1.0f); @@ -219,7 +227,7 @@ namespace widgets { glm::vec3 ViewportWidget::cursorToDragPlane(float sx, float sy) const { scene::Camera& cam = _sim->camera(); const glm::mat4 invVP = glm::inverse(cam.getProjection() * cam.getViewMatrix()); - const float ndcX = 2.0f * (sx / (float)width()) - 1.0f; + const float ndcX = 2.0f * (sx / (float)width()) - 1.0f; const float ndcY = 2.0f * (sy / (float)height()) - 1.0f; glm::vec4 pNear = invVP * glm::vec4(ndcX, ndcY, 0.0f, 1.0f); glm::vec4 pFar = invVP * glm::vec4(ndcX, ndcY, 1.0f, 1.0f); From 9d5b9ed968dc23349a1647a59dd46f1fb20457ce Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 09:09:21 +0100 Subject: [PATCH 024/114] feat: Refactored gravity handling in RigidBodySystem to use Vec3 for 3D representation --- .../include/Systems/RigidBodySystem.h | 6 +++-- .../include/Systems/RigidBodySystemStep.inl | 24 ++++++++++++++++++- .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 9 +++++-- 3 files changed, 34 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 19d2e25c..4e4a44fb 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -82,7 +82,9 @@ namespace systems { bool hasRigidBody() const { return _hasBody; } void setGravity(double g); - const double getGravity() const { return _gravity; } + void setGravityVec(const mathlib::Vec3& g); + const mathlib::Vec3& getGravityVec() const { return _gravity; } + double getGravity() const { return _gravity.norm(); } void setNaturalFrequency(double wn) { _wn = wn; } double getNaturalFrequency() const { return _wn; } @@ -309,7 +311,7 @@ namespace systems { mutable std::vector _clampOmega; // Gravity acceleration (m/s^2) - double _gravity = 0.0; + mathlib::Vec3 _gravity{ 0.0, 0.0, 0.0 }; // FLoating base state bool _baseIsFree = false; diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index 99af62a5..037b948e 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -39,7 +39,7 @@ namespace systems { snap.root_pose = _root_pose.template cast(); snap.baseIsFree = _baseIsFree; snap.lastBaseForwardForce = T(_lastBaseForwardForce); - snap.gravity = T(_gravity); + snap.gravity = T(_gravity.z()); snap.torqueMode = _body.torqueMode; @@ -257,6 +257,28 @@ namespace systems { _simTime = simTime; const size_t n = _body.joints.size(); mathlib::VecX_T x = packState_AD(); + // Apply floor contact forces if enabled (Bit crude but yeah) + { + constexpr double k_floor = 400000.0; + constexpr double c_floor = 2000.0; + const size_t nl = _body.links.size(); + if (_prevLinkY.size() != nl) { _prevLinkY.assign(nl, 0.0); } + for (size_t i = 0; i < nl; ++i) { + const Mat4& T = _worldTransforms[i]; + const double lowY = linkWorldMinY(i, T); + const double vy = (lowY - _prevLinkY[i]) / (_dynamics->dt() > 0 ? _dynamics->dt() : (1.0/180.0)); + _prevLinkY[i] = lowY; + if (lowY < 0.0) { + double Fy = -k_floor * lowY - c_floor * vy; + if (Fy < 0.0) { Fy = 0.0; } // floor only pushes, never pulls + setLinkExtForce( + _body.links[i].name, + Vec3(T(0,3), lowY, T(2,3)), + Vec3(0.0, Fy, 0.0) + ); + } + } + } assert((size_t)x.size() <= NVar && "State size exceeds the number of dual variables."); // Checks state vector size is within the dual variable limit for (size_t i = 0; i < (size_t)x.size(); ++i) { x[i].dual[i] = 1.0; } diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 0b3770d9..da57023d 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -533,7 +533,7 @@ namespace systems { _dynResult.resize(_body.joints.size()); _dynResult_AD.resize(_body.joints.size()); - _dynScratch.g.setConstant(_gravity); + _dynScratch.g.setConstant(_gravity.z()); // Reset adaptive integrator so it doesn't carry a stale step size _integrator->resetAdaptiveState(); @@ -1023,9 +1023,14 @@ namespace systems { // Method to set the gravity strength for the rigidBody system void RigidBodySystem::setGravity(double g) { - _gravity = g; + _gravity = mathlib::Vec3(0.0, 0.0, -g); _dynamics->setGravity(g); } + // + void RigidBodySystem::setGravityVec(const mathlib::Vec3& g) { + _gravity = g; + _dynamics->setGravityVec(g); + } // Set the torque mode for the rigidBody system void RigidBodySystem::setTorqueMode(eTorqueMode mode) { _body.torqueMode = mode; } From d406284355a8424fc5950027655df04e392a88b8 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 09:09:34 +0100 Subject: [PATCH 025/114] feat: Refactored gravity handling to use Vec3 for 3D representation in RigidBodyDynamics --- DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h | 9 ++++++--- DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl | 4 +--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h index 65ea579c..6005b5bc 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h @@ -144,8 +144,11 @@ namespace physics { ); // Set the gravity strength for the body system - void setGravity(double gravity) { _gravity = gravity; } - const double getGravity() const { return _gravity; } + void setGravity(double g) { _gravity = mathlib::Vec3(0,0,-g); } + void setGravityVec(const mathlib::Vec3& g) { _gravity = g; } + const mathlib::Vec3& getGravityVec() const { return _gravity; } + double getGravity() const { return _gravity.norm(); } + // Set the timestep for dynamics updates (used for energy calculations and integration) void setDt(double dt) { _dt = dt; } @@ -163,7 +166,7 @@ namespace physics { double _dt = 1.0 / 180.0; // default timestep for dynamics updates - double _gravity{ 0.0 }; + mathlib::Vec3 _gravity{ 0.0, 0.0, 0.0 }; bool _baseIsFree = false; double _lastBaseForwardForce{ 0.0 }; }; diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index 7bf5edd1..87cb9c2f 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -192,7 +192,6 @@ namespace physics { ) const { const size_t n = body.joints.size(); mathlib::VecX_T tau_G = mathlib::VecX_T::Zero(n); - Scalar g{ _gravity }; // [m/s^2], gravity acceleration magnitude // For each joint, sum the gravity contributions from all links for (size_t i = 0; i < n; ++i) { @@ -220,8 +219,7 @@ namespace physics { const mathlib::Vec3_T com_world = R_k * link.inertial.com_xyz + T_world[k].template block<3, 1>(0, 3); // Gravitational force on the link - mathlib::Vec3_T g_world; - g_world = mathlib::Vec3_T(0.0, 0.0, -g); // [m/s^2], gravity vector in world frame + mathlib::Vec3_T g_world = _gravity.template cast(); // [m/s^2], gravity vector in world frame const mathlib::Vec3_T F_g = m * g_world; // [N], gravitational force on the link in world frame const mathlib::Vec3_T r = com_world - p_i; // [m] From 90392e0863d4548f02e18f4df5979d6975d572b0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 09:09:45 +0100 Subject: [PATCH 026/114] feat: Added gravity handling methods to ISimulationCore and SimulationCore for physics dynamics --- DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h | 3 +++ DSFE_App/DSFE_Core/include/Scene/SimulationCore.h | 5 +++++ DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp | 4 ++++ 3 files changed, 12 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 20cac370..0d3de299 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -53,6 +53,9 @@ namespace core { virtual integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const = 0; virtual void enableAutoDiff(bool enable) = 0; virtual bool autoDiffEnabled() const = 0; + // Physics and dynamics + virtual void setGravity(const mathlib::Vec3& g) = 0; + virtual mathlib::Vec3 gravity() const = 0; // Subsystems virtual systems::RigidBodySystem& rigidBodySystem() = 0; virtual single_body_system::SingleBodySystem& singleBodySystem() = 0; diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 32fa4342..1018f9f2 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -77,6 +77,11 @@ namespace core { void enableAutoDiff(bool enable) override; bool autoDiffEnabled() const override; + // Physics and dynamics + void setGravity(const mathlib::Vec3& g) override; + mathlib::Vec3 gravity() const override; + + // Simulation run tag (used for logging and data management) void setRunTag(const std::string& tag) override { _runTag = tag; } // Subsystems access diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index d9a811ac..4797e72f 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -110,6 +110,10 @@ namespace core { return _rigidBody->autoDiffEnabled(); } + // SimulationCore + void SimulationCore::setGravity(const mathlib::Vec3& g) { _rigidBody->setGravityVec(g); } + mathlib::Vec3 SimulationCore::gravity() const { return _rigidBody->getGravityVec(); } + // Fixed timestep loop for physics and rigidBody updates, called from the main render loop with the frame delta time void SimulationCore::stepFixed(double frame_dt) { double simTime = _simTime.load(); From 1f31beedcc92abfbae0221506aac6fd6b8d8cbb5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 09:10:01 +0100 Subject: [PATCH 027/114] feat: Added gravity accessors and conversion utility in SimulationManager for improved physics handling --- .../include/Simulation/SimulationManager.h | 12 ++++++--- .../src/Simulation/SimulationManager.cpp | 25 +++++++++++-------- 2 files changed, 23 insertions(+), 14 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h index 74c54b24..1bc213e2 100644 --- a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h @@ -1,4 +1,8 @@ -// DSFE_GUI SimulationManager.h +/* + * Project: DSFE_GUI + * File: Simulation/SimulationManager.h + * Created by: Joss Salton, 27-07-2026 + */ #pragma once #include "Renderer/NativeWindow.h" @@ -237,12 +241,14 @@ namespace gui { const integration::eIntegrationMethod integrationMethod() const; void setADIntegrationMethod(integration::eAutoDiffIntegrationMethod method); const integration::eAutoDiffIntegrationMethod autoDiffIntegrationMethod() const; - std::string integrationMethodName() const; - void enableAutoDiff(bool enable); bool autoDiffEnabled() const; + // Accessors for Physics and Dynamics state + void setGravity(const glm::vec3& g); + glm::vec3 gravity() const; + // Access to the underlying StudyRunner for running batch studies from the GUI StudyRunner* studyRunner() { return _studyRunner.get(); } diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index dcb26255..506a6251 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -43,6 +43,14 @@ namespace gui { static const glm::quat q_corr = glm::angleAxis(glm::radians(90.0f), glm::vec3(1, 0, 0)); + static glm::vec3 toGlm(const mathlib::Vec3& v) { + return glm::vec3( + static_cast(v.x()), + static_cast(v.y()), + static_cast(v.z()) + ); + } + static glm::mat4 toGlm(const mathlib::Mat4& m) { glm::mat4 g(1.0f); for (int c = 0; c < 4; ++c) { @@ -274,18 +282,13 @@ namespace gui { LOG_INFO("DEBUG -> Current AutoDiff integration method: %d", static_cast(_core->autoDiffIntegrationMethod())); return _core->autoDiffIntegrationMethod(); } + std::string SimulationManager::integrationMethodName() const { return _core->integrationMethodName(); } + void SimulationManager::enableAutoDiff(bool enable) { _core->enableAutoDiff(enable); } + bool SimulationManager::autoDiffEnabled() const { return _core->autoDiffEnabled(); } - std::string SimulationManager::integrationMethodName() const { - return _core->integrationMethodName(); - } - - void SimulationManager::enableAutoDiff(bool enable) { - _core->enableAutoDiff(enable); - } - - bool SimulationManager::autoDiffEnabled() const { - return _core->autoDiffEnabled(); - } + // Accessors for Physics and Dynamics state + void SimulationManager::setGravity(const glm::vec3& g) { _core->setGravity(mathlib::Vec3(g.x, g.y, g.z)); } + glm::vec3 SimulationManager::gravity() const { mathlib::Vec3 g = _core->gravity(); return toGlm(g); } // This seems to be the better solution? static std::string replaceIntegratorInScript(const std::string& script, const std::string& methodName) { From 85bfeb8f64d688a9f7ed2c8eea16167e5104357c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 09:10:24 +0100 Subject: [PATCH 028/114] feat: Added GravityVectorWidget for 3D gravity vector manipulation in GUI --- DSFE_App/DSFE_GUI/CMakeLists.txt | 2 + .../MainWindow/Widgets/GravityVectorWidget.h | 30 ++++++++ .../Widgets/GravityVectorWidget.cpp | 76 +++++++++++++++++++ 3 files changed, 108 insertions(+) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/Widgets/GravityVectorWidget.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index 5c78cd93..d23528b8 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -140,6 +140,8 @@ set(WIDGETS_SRC include/MainWindow/Widgets/FractionSelectorWidget.h src/MainWindow/DSL/DSLSyntaxHighlighter.cpp include/MainWindow/DSL/DSLSyntaxHighlighter.h + src/MainWindow/Widgets/GravityVectorWidget.cpp + include/MainWindow/Widgets/GravityVectorWidget.h ) set(OBJECT_SRC diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/GravityVectorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/GravityVectorWidget.h new file mode 100644 index 00000000..437e4023 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/GravityVectorWidget.h @@ -0,0 +1,30 @@ +/* + * Project: DSFE_GUI + * File: MainWindow/Widgets/GravityVectorWidget.h + * Created by: Joss Salton, 27-07-2026 + */ +#pragma once + +#include +#include +#include + +class QDoubleSpinBox; +class QLabel; + +namespace widgets { + class GravityVectorWidget : public QWidget { + Q_OBJECT + public: + explicit GravityVectorWidget(QWidget* parent = nullptr); + void setValue(const glm::vec3& g); + glm::vec3 value() const; + std::function onChanged; + + private: + void emitChanged(); + void refreshLabel(); + QLabel* _tex; + QDoubleSpinBox* _x; QDoubleSpinBox* _y; QDoubleSpinBox* _z; + }; +} // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp new file mode 100644 index 00000000..b94bbb4d --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp @@ -0,0 +1,76 @@ +/* + * Project: DSFE_GUI + * File: MainWindow/Widgets/GravityVectorWidget.cpp + * Created by: Joss Salton, 27-07-2026 + */ +#include "Widgets/GravityVectorWidget.h" + +#include +#include +#include +#include + +namespace widgets { + static QDoubleSpinBox* makeBox() { + auto* b = new QDoubleSpinBox(); + b->setRange(-100.0, 100.0); // Set a reasonable range for gravity values + b->setDecimals(3); + b->setSingleStep(0.1); + b->setMinimumWidth(25); + b->setSuffix(" m/s\u00B2"); // Unicode for squared symbol + return b; + } + + GravityVectorWidget::GravityVectorWidget(QWidget* parent) : QWidget(parent) { + _tex = new QLabel(this); + _tex->setTextFormat(Qt::RichText); + _tex->setAlignment(Qt::AlignCenter); + _x = makeBox(); _y = makeBox(); _z = makeBox(); + + auto* row = new QHBoxLayout(); + auto add_labelled = [&](const char* axis, QDoubleSpinBox* b) { + auto* col = new QVBoxLayout(); + auto* l = new QLabel(QString("%1").arg(axis)); + l->setAlignment(Qt::AlignCenter); + col->addWidget(l); col->addWidget(b); + row->addLayout(col); + }; + add_labelled("x", _x); add_labelled("y", _y); add_labelled("z", _z); + + auto* root = new QVBoxLayout(this); + root->addWidget(_tex); + root->addLayout(row); + + for (auto* b : {_x, _y, _z}) { + connect(b, QOverload::of(&QDoubleSpinBox::valueChanged), this, [this]() { refreshLabel(); emitChanged(); }); + } + refreshLabel(); + } + + void GravityVectorWidget::refreshLabel() { + _tex->setText( + QString( + "" + "g = [ %1, %2, %3 ]T " + "m/s2" + ) + .arg(_x->value(), 0, 'f', 3) + .arg(_y->value(), 0, 'f', 3) + .arg(_z->value(), 0, 'f', 3) + ); + } + + void GravityVectorWidget::setValue(const glm::vec3& g) { + QSignalBlocker bx(_x), by(_y), bz(_z); + _x->setValue(g.x); _y->setValue(g.y); _z->setValue(g.z); + refreshLabel(); + } + + glm::vec3 GravityVectorWidget::value() const { + return glm::vec3(static_cast(_x->value()), static_cast(_y->value()), static_cast(_z->value())); + } + + void GravityVectorWidget::emitChanged() { + if (onChanged) { onChanged(value()); } + } +} // namespace widgets \ No newline at end of file From 96ea6da0c640b6160d91ae810566a657e35feb52 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 09:10:33 +0100 Subject: [PATCH 029/114] feat: Added world properties panel to ControlPanelWidget for gravity manipulation --- .../MainWindow/Widgets/ControlPanelWidget.h | 4 ++++ .../MainWindow/Widgets/ControlPanelWidget.cpp | 17 +++++++++++++++++ 2 files changed, 21 insertions(+) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 17a89d23..455a79b9 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -88,6 +88,8 @@ namespace widgets { void simPropertiesPanel(); void buildIntegratorCombos(); + void worldPropertiesPanel(); + void jointInfoPanel(); void updateTelemetryInfo(const diagnostics::JointTelemetry& j); void buildTelemetryWidgets(QVBoxLayout* layout); @@ -110,6 +112,8 @@ namespace widgets { FractionSelectorWidget* _simDtSelector = nullptr; FractionSelectorWidget* _telemetryDtSelector = nullptr; + QGroupBox* _worldPropertiesGroup = nullptr; + QGroupBox* _jointInfoGroup = nullptr; QSlider* _jointIdxSlider = nullptr; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index c8083377..078dc370 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -16,6 +16,7 @@ #include #include "Widgets/FractionSelectorWidget.h" +#include "Widgets/GravityVectorWidget.h" #include "Scene/Camera.h" @@ -41,6 +42,7 @@ namespace widgets { rootLayout->addWidget(scrollArea); simPropertiesPanel(); + worldPropertiesPanel(); jointInfoPanel(); auto* timer = new QTimer(this); @@ -149,6 +151,21 @@ namespace widgets { } } + void ControlPanelWidget::worldPropertiesPanel() { + _worldPropertiesGroup = new QGroupBox("World Properties"); + auto* layout = new QVBoxLayout(_worldPropertiesGroup); + _worldPropertiesGroup->setLayout(layout); + + auto* grav = new GravityVectorWidget(_worldPropertiesGroup); + grav->setValue(_sim->gravity()); + grav->onChanged = [this](const glm::vec3& g) { _sim->setGravity(g); }; + + layout->addWidget(grav); + layout->addSpacing(8); + + _contentLayout->addWidget(_worldPropertiesGroup); + } + void ControlPanelWidget::jointInfoPanel() { if (!_jointInfoGroup) { _jointInfoGroup = new QGroupBox("RigidBody Joint Information"); From 6ea7d5c13d5ddddd98a3c5770a391c0d492edfd4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 10:29:20 +0100 Subject: [PATCH 030/114] feat: Implemented QLabel methods for displaying variable names and decimal values in FractionSelectorWidget --- .../Widgets/FractionSelectorWidget.h | 4 ++++ .../Widgets/FractionSelectorWidget.cpp | 19 ++++++++++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h index 895c20c7..daa89842 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/FractionSelectorWidget.h @@ -3,6 +3,7 @@ #include +class QLabel; namespace widgets { class FractionSelectorWidget : public QWidget { Q_OBJECT @@ -10,6 +11,9 @@ namespace widgets { explicit FractionSelectorWidget(bool telemetryMode = false, QWidget* parent = nullptr); double dt() const; void setDt(double dt); + + QLabel* setDtVarName(const QString& subscript); + QLabel* setDtVarDecValue(double dt); signals: void valueChanged(double denom); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp index ad7f7f17..e86c3265 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/FractionSelectorWidget.cpp @@ -7,12 +7,13 @@ #include #include #include +#include namespace widgets { FractionSelectorWidget::FractionSelectorWidget(bool telemetryMode, QWidget* parent) : QWidget(parent), _telemetryMode(telemetryMode) { - setMinimumSize(100, 70); + setMinimumSize(50, 50); } double FractionSelectorWidget::dt() const { @@ -62,4 +63,20 @@ namespace widgets { _k = std::clamp(static_cast(std::round(1.0 / (10.0 * dt))), _minK, max); update(); } + + QLabel* FractionSelectorWidget::setDtVarName(const QString& subscript) { + auto* label = new QLabel(this); + label->setTextFormat(Qt::RichText); + label->setText("\u0394t" + subscript + "\u2009=\u2009"); + label->setAlignment(Qt::AlignCenter); + return label; + } + + QLabel* FractionSelectorWidget::setDtVarDecValue(double dt) { + auto* label = new QLabel(this); + label->setTextFormat(Qt::RichText); + label->setText(QString("\u2192 %1").arg(dt, 0, 'g', 3)); + label->setAlignment(Qt::AlignCenter); + return label; + } } \ No newline at end of file From 23042fce4c3bca1432d36e8115964ab46b6c62a4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 10:29:29 +0100 Subject: [PATCH 031/114] fix: Enhanced label formatting in GravityVectorWidget for improved readability --- .../DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp index b94bbb4d..94632780 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp @@ -30,7 +30,7 @@ namespace widgets { auto* row = new QHBoxLayout(); auto add_labelled = [&](const char* axis, QDoubleSpinBox* b) { auto* col = new QVBoxLayout(); - auto* l = new QLabel(QString("%1").arg(axis)); + auto* l = new QLabel(QString("%1").arg(axis)); l->setAlignment(Qt::AlignCenter); col->addWidget(l); col->addWidget(b); row->addLayout(col); @@ -51,7 +51,7 @@ namespace widgets { _tex->setText( QString( "" - "g = [ %1, %2, %3 ]T " + "g\u2009=\u2009[ %1, %2, %3 ]T " "m/s2" ) .arg(_x->value(), 0, 'f', 3) @@ -73,4 +73,6 @@ namespace widgets { void GravityVectorWidget::emitChanged() { if (onChanged) { onChanged(value()); } } + + } // namespace widgets \ No newline at end of file From bc59e4630509e780f92f75b20d8f4c2105997b7f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 10:29:40 +0100 Subject: [PATCH 032/114] feat: Refactored ControlPanelWidget to add timestep selectors and improve layout for simulation parameters --- .../MainWindow/Widgets/ControlPanelWidget.h | 9 +- .../MainWindow/Widgets/ControlPanelWidget.cpp | 87 +++++++++++++------ .../src/MainWindow/Widgets/ViewportWidget.cpp | 4 +- 3 files changed, 71 insertions(+), 29 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 455a79b9..ab1d560c 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -87,6 +87,7 @@ namespace widgets { void simPropertiesPanel(); void buildIntegratorCombos(); + void buildTimestepSelectors(QVBoxLayout* layout); void worldPropertiesPanel(); @@ -109,8 +110,14 @@ namespace widgets { QComboBox* _integratorCombo = nullptr; QLabel* _currentIntegratorLabel = nullptr; QLabel* _simTimeLabel = nullptr; + + QLabel* _simDtLabel = nullptr; FractionSelectorWidget* _simDtSelector = nullptr; - FractionSelectorWidget* _telemetryDtSelector = nullptr; + QLabel* _simDtValue = nullptr; + + QLabel* _telDtLabel = nullptr; + FractionSelectorWidget* _telDtSelector = nullptr; + QLabel* _telDtValue = nullptr; QGroupBox* _worldPropertiesGroup = nullptr; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 078dc370..960f7e80 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -68,7 +68,9 @@ namespace widgets { _integratorCombo->setMaximumWidth(175); _simDtSelector = new FractionSelectorWidget(false); - _telemetryDtSelector = new FractionSelectorWidget(true); + _telDtSelector = new FractionSelectorWidget(true); + _simDtLabel = new QLabel(); + _telDtLabel = new QLabel(); _simTimeLabel = new QLabel(); _simTimeLabel->setWordWrap(true); @@ -79,32 +81,13 @@ namespace widgets { form->addRow("Integrator", _integratorCombo); layout->addLayout(form); - layout->addSpacing(8); - - auto* dtHeaderRow = new QHBoxLayout(); - - auto* simDtLabel = new QLabel("Simulation dt"); - simDtLabel->setAlignment(Qt::AlignCenter); - - auto* telemetryDtLabel = new QLabel("Telemetry dt"); - telemetryDtLabel->setAlignment(Qt::AlignCenter); - _simDtSelector->setDt(_sim->fixedDt()); - _telemetryDtSelector->setDt(1.0 / _sim->telemetryHz()); - - dtHeaderRow->addWidget(simDtLabel); - dtHeaderRow->addWidget(telemetryDtLabel); - - layout->addLayout(dtHeaderRow); + layout->addSpacing(8); - auto* dtValueRow = new QHBoxLayout(); + buildTimestepSelectors(layout); - dtValueRow->addWidget(_simDtSelector); - dtValueRow->addWidget(_telemetryDtSelector); - layout->addLayout(dtValueRow); layout->addSpacing(8); layout->addWidget(_simTimeLabel); - _contentLayout->addWidget(_simPropertiesGroup); buildIntegratorCombos(); @@ -126,8 +109,14 @@ namespace widgets { } }); - connect(_simDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { _sim->setFixedDt(dt); }); - connect(_telemetryDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { _sim->setTelemetryHz(1.0 / dt); }); + connect(_simDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { + _sim->setFixedDt(dt); + _simDtValue = _simDtSelector->setDtVarDecValue(dt); + }); + connect(_telDtSelector, &FractionSelectorWidget::valueChanged, this, [this](double dt) { + _sim->setTelemetryHz(1.0 / dt); + _telDtValue = _telDtSelector->setDtVarDecValue(1.0/dt); + }); } void ControlPanelWidget::buildIntegratorCombos() { @@ -151,16 +140,58 @@ namespace widgets { } } + void ControlPanelWidget::buildTimestepSelectors(QVBoxLayout* layout) { + _simDtLabel = _simDtSelector->setDtVarName("sim"); + _telDtLabel = _telDtSelector->setDtVarName("tel"); + + double simDt = _sim->fixedDt(); + _simDtSelector->setDt(simDt); + _simDtSelector->setFixedWidth(50); + _simDtValue = _simDtSelector->setDtVarDecValue(simDt); + + double telDt = 1.0 / _sim->telemetryHz(); + _telDtSelector->setDt(telDt); + _telDtSelector->setFixedWidth(50); + _telDtValue = _telDtSelector->setDtVarDecValue(telDt); + + auto* dtGrid = new QGridLayout(); + dtGrid->setHorizontalSpacing(2); // label hugs fraction + dtGrid->setVerticalSpacing(8); + + dtGrid->setColumnStretch(0, 0); + dtGrid->setColumnStretch(1, 0); + dtGrid->setColumnStretch(2, 1); + + dtGrid->addWidget(_simDtLabel, 0, 0, Qt::AlignRight | Qt::AlignVCenter); + dtGrid->addWidget(_simDtSelector, 0, 1, Qt::AlignLeft); + dtGrid->addWidget(_simDtValue, 0, 2, Qt::AlignLeft | Qt::AlignVCenter); + dtGrid->addWidget(_telDtLabel, 1, 0, Qt::AlignRight | Qt::AlignVCenter); + dtGrid->addWidget(_telDtSelector, 1, 1, Qt::AlignLeft); + dtGrid->addWidget(_telDtValue, 1, 2, Qt::AlignLeft | Qt::AlignVCenter); + + dtGrid->setColumnStretch(0, 0); + dtGrid->setColumnStretch(1, 0); + dtGrid->setColumnStretch(2, 1); + + layout->addLayout(dtGrid); + } + void ControlPanelWidget::worldPropertiesPanel() { _worldPropertiesGroup = new QGroupBox("World Properties"); auto* layout = new QVBoxLayout(_worldPropertiesGroup); _worldPropertiesGroup->setLayout(layout); + auto* gravityLabel = new QLabel(this); + gravityLabel->setTextFormat(Qt::RichText); + gravityLabel->setText("Gravity"); + gravityLabel->setAlignment(Qt::AlignLeft); auto* grav = new GravityVectorWidget(_worldPropertiesGroup); grav->setValue(_sim->gravity()); grav->onChanged = [this](const glm::vec3& g) { _sim->setGravity(g); }; + layout->addWidget(gravityLabel); layout->addWidget(grav); + layout->addSpacing(8); _contentLayout->addWidget(_worldPropertiesGroup); @@ -421,7 +452,11 @@ namespace widgets { _useAutoDiffCheck->setChecked(_useAutoDiff); } buildIntegratorCombos(); // already reads back from _sim - _simDtSelector->setDt(_sim->fixedDt()); - _telemetryDtSelector->setDt(1.0 / _sim->telemetryHz()); + double simDt = _sim->fixedDt(); + double telDt = 1.0 / _sim->telemetryHz(); + _simDtSelector->setDt(simDt); + _simDtValue = _simDtSelector->setDtVarDecValue(simDt); + _telDtSelector->setDt(telDt); + _telDtValue = _telDtSelector->setDtVarDecValue(telDt); } } // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp index 1cda4023..0050ca2c 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -173,8 +173,8 @@ namespace widgets { glm::vec3 target = cursorToDragPlane(event->position().x(), event->position().y()); if (target.y < 0.0f) { target.y = 0.0f; } // never drag a link below the floor plane const glm::vec3 grab = linkOrigin(_dragLink); - glm::vec3 force = 400.0f * (target - grab); // spring; tune stiffness - const float fmax = 3000.0f; + glm::vec3 force = 450.0f * (target - grab); // spring; tune stiffness + const float fmax = 3000.0f; // cap the force to avoid instability if (glm::length(force) > fmax) { force = glm::normalize(force) * fmax; } _sim->setLinkExternalForce(_dragLink, grab, force); } From 71511c967f1493321212a5a251f50b9cb615dc80 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 10:36:38 +0100 Subject: [PATCH 033/114] feat: Added gravity property to WorkspaceData and updated serialisation methods --- DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h | 3 ++- DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp | 4 ++++ DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp | 2 ++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h index b964754d..990118ff 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h @@ -20,8 +20,9 @@ namespace gui { int integrationMethod = 0; // integration::eIntegrationMethod as int int adIntegrationMethod = 0; // integration::eAutoDiffIntegrationMethod as int double simDt = 1.0 / 180.0; - double telemetryDt = 1.0 / 180.0; + double telemetryDt = 1.0 / 120.0; bool autoDiff = false; + glm::vec3 gravity{ 0.0f, 0.0f, 0.0f }; // Camera glm::vec3 cameraPos{ 0.0f, 1.5f, 4.0f }; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp index 017e7c4f..c193a620 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp @@ -50,6 +50,10 @@ namespace gui { w.simDt = sim["sim_dt"].toDouble(1.0 / 180.0); w.telemetryDt = sim["telemetry_dt"].toDouble(1.0 / 180.0); w.autoDiff = sim["auto_diff"].toBool(false); + const QJsonArray gravity = sim["gravity"].toArray(); + if (gravity.size() == 3) { + w.gravity = { (float)gravity[0].toDouble(), (float)gravity[1].toDouble(), (float)gravity[2].toDouble() }; + } const QJsonObject cam = o["camera"].toObject(); const QJsonArray pos = cam["pos"].toArray(); diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index 506a6251..e67cd6aa 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -373,6 +373,7 @@ namespace gui { setADIntegrationMethod(static_cast(w.adIntegrationMethod)); setFixedDt(w.simDt); setTelemetryHz(1.0 / w.telemetryDt); + setGravity(w.gravity); _camera.setPosition(w.cameraPos); _camera.setYaw(w.cameraYaw); @@ -390,6 +391,7 @@ namespace gui { w.autoDiff = autoDiffEnabled(); w.simDt = fixedDt(); w.telemetryDt = 1.0 / telemetryHz(); + w.gravity = gravity(); w.cameraPos = _camera.getPosition(); w.cameraYaw = _camera.getYaw(); w.cameraPitch = _camera.getPitch(); From 8aee75cd2c21f84cfa8495eadfef561959182e67 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 11:06:57 +0100 Subject: [PATCH 034/114] fixes: Correctly integrated `GravityVectorWidget` into ControlPanelWidget and update gravity serialisation in WorkspaceData --- .../MainWindow/Widgets/ControlPanelWidget.h | 4 ++++ .../MainWindow/Widgets/ControlPanelWidget.cpp | 22 ++++++++++--------- .../src/MainWindow/Workspace/Workspace.cpp | 1 + 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index ab1d560c..7d2b6b5d 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -35,6 +35,7 @@ class QSlider; namespace widgets { class FractionSelectorWidget; + class GravityVectorWidget; class ControlPanelWidget : public QWidget { public: @@ -121,6 +122,9 @@ namespace widgets { QGroupBox* _worldPropertiesGroup = nullptr; + QLabel* _gravityLabel = nullptr; + GravityVectorWidget* _grav = nullptr; + QGroupBox* _jointInfoGroup = nullptr; QSlider* _jointIdxSlider = nullptr; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 960f7e80..2e6c61c9 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -181,16 +181,16 @@ namespace widgets { auto* layout = new QVBoxLayout(_worldPropertiesGroup); _worldPropertiesGroup->setLayout(layout); - auto* gravityLabel = new QLabel(this); - gravityLabel->setTextFormat(Qt::RichText); - gravityLabel->setText("Gravity"); - gravityLabel->setAlignment(Qt::AlignLeft); - auto* grav = new GravityVectorWidget(_worldPropertiesGroup); - grav->setValue(_sim->gravity()); - grav->onChanged = [this](const glm::vec3& g) { _sim->setGravity(g); }; - - layout->addWidget(gravityLabel); - layout->addWidget(grav); + _gravityLabel = new QLabel(this); + _gravityLabel->setTextFormat(Qt::RichText); + _gravityLabel->setText("Gravity"); + _gravityLabel->setAlignment(Qt::AlignLeft); + _grav = new GravityVectorWidget(_worldPropertiesGroup); + _grav->setValue(_sim->gravity()); + _grav->onChanged = [this](const glm::vec3& g) { _sim->setGravity(g); }; + + layout->addWidget(_gravityLabel); + layout->addWidget(_grav); layout->addSpacing(8); @@ -458,5 +458,7 @@ namespace widgets { _simDtValue = _simDtSelector->setDtVarDecValue(simDt); _telDtSelector->setDt(telDt); _telDtValue = _telDtSelector->setDtVarDecValue(telDt); + _grav->setValue(_sim->gravity()); + } } // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp index c193a620..773230b3 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp @@ -23,6 +23,7 @@ namespace gui { sim["sim_dt"] = simDt; sim["telemetry_dt"] = telemetryDt; sim["auto_diff"] = autoDiff; + sim["gravity"] = QJsonArray{ gravity.x, gravity.y, gravity.z }; o["simulation"] = sim; QJsonObject cam; From 8c20aa03941d90624867942f69b4eb571bfc7ffd Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:17:26 +0100 Subject: [PATCH 035/114] feat: Added gravity vector to RigidBodySnapshot_T and updated casting methods --- DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h index 55bbed62..7b19dda8 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h @@ -37,13 +37,13 @@ namespace systems { mathlib::VecX_T q_ref; // reference joint angles mathlib::VecX_T qd_ref; // reference joint velocities mathlib::VecX_T qdd_ref; // reference joint accelerations + mathlib::VecX_T gravity; mathlib::Mat4_T root_pose = mathlib::Mat4_T::Identity(); bool baseIsFree = false; Scalar lastBaseForwardForce = Scalar(0); - Scalar gravity = Scalar(0); eTorqueMode torqueMode = eTorqueMode::CONTROLLED; @@ -66,16 +66,13 @@ namespace systems { dst.q_ref = src.q_ref.template cast(); dst.qd_ref = src.qd_ref.template cast(); dst.qdd_ref = src.qdd_ref.template cast(); + dst.gravity = src.gravity.template cast(); dst.root_pose = src.root_pose.template cast(); dst.baseIsFree = src.baseIsFree; - dst.lastBaseForwardForce = ToScalar(src.lastBaseForwardForce); - dst.gravity = ToScalar(src.gravity); - dst.torqueMode = src.torqueMode; - dst.dt = ToScalar(src.dt); dst.simTime = ToScalar(src.simTime); From 7c5b4cfdf9163806b66cf7c123a852e27a826d3c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:17:40 +0100 Subject: [PATCH 036/114] fix: Added missing tau_g vector initialisation in RigidBodyMetrics resize method --- DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h index e0841f7d..d22cd3f0 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h @@ -19,6 +19,7 @@ namespace systems { // Dynamics mathlib::VecX_T I_eff; mathlib::VecX_T tau; + mathlib::VecX_T tau_g; // Constraints / realism mathlib::VecX_T tau_barrier; @@ -36,7 +37,7 @@ namespace systems { void resize(size_t n) { q.resize(n); qd.resize(n); qdd.resize(n); err.resize(n); errd.resize(n); - I_eff.resize(n); tau.resize(n); + I_eff.resize(n); tau.resize(n); tau_g.resize(n); tau_barrier.resize(n); tau_sat.resize(n); KE.resize(n); PE.resize(n); E_total.resize(n); W_actuator.resize(n); sat_flag.resize(n, 0); From a1158dd8e227f4259c4f2ef1f0bb58e401ed4959 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:17:49 +0100 Subject: [PATCH 037/114] feat: Added gravity torque vector to DenseDynamicsScratch and updated related methods --- DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h index cfb09f58..f2d75ba8 100644 --- a/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h @@ -18,6 +18,7 @@ namespace physics { mathlib::VecX_T rhs; // right-hand side vector for dynamics equations (Coriolis, gravity, control torques) mathlib::VecX_T h; // Coriolis and centrifugal bias vector mathlib::VecX_T tau; // control torque vector + mathlib::VecX_T tau_g; // gravity torque vector mathlib::VecX_T I_eff_controller; // effective inertia vector for controller design (e.g., for inverse dynamics control) std::vector> T_world; @@ -39,6 +40,7 @@ namespace physics { rhs.resize(nJoints); h.resize(nJoints); tau.resize(nJoints); + tau_g.resize(nJoints); I_eff_controller.resize(nJoints); T_world.resize(nLinks); jointWorldPoses.resize(nJoints); @@ -55,6 +57,7 @@ namespace physics { rhs.setZero(); h.setZero(); tau.setZero(); + tau_g.setZero(); I_eff_controller.setZero(); for (auto& T : T_world) T.setIdentity(); for (auto& T : jointWorldPoses) T.setIdentity(); @@ -66,6 +69,7 @@ namespace physics { rhs.resize(0); h.resize(0); tau.resize(0); + tau_g.resize(0); I_eff_controller.resize(0); T_world.clear(); jointWorldPoses.clear(); @@ -88,6 +92,7 @@ namespace physics { std::vector> U; // articulated body force std::vector> f_ext; // spatial force + mathlib::VecX_T g; // gravity vector in spatial coordinates (6D) mathlib::VecX_T u; // joint force contribution mathlib::VecX_T d; // joint inertia contribution @@ -116,6 +121,7 @@ namespace physics { U.resize(nJoints); f_ext.resize(nJoints); + g.resize(6); // gravity vector is always 6D u.resize(nJoints); d.resize(nJoints); @@ -139,6 +145,7 @@ namespace physics { U.clear(); f_ext.clear(); + g.resize(0); u.resize(0); d.resize(0); @@ -181,7 +188,7 @@ namespace physics { void resize(size_t nJoints, size_t nLinks) { dense.resize(nJoints, nLinks); spatial.resize(nJoints); - g.resize(nJoints); + g.resize(6); } // Clears all scratch buffers From 7e4a575a89c963a906eab6d67b92c92cc617d70e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:18:00 +0100 Subject: [PATCH 038/114] fixes: Updated gravity vector reference in computeAccelerations_RNEA and log base acceleration --- .../include/Physics/RigidBodyDynamics.h | 2 +- .../include/Physics/RigidBodyDynamics.inl | 16 +++++++--------- .../include/Physics/SpatialDynamics.inl | 10 +++++++--- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h index 6005b5bc..b73275f4 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h @@ -144,7 +144,7 @@ namespace physics { ); // Set the gravity strength for the body system - void setGravity(double g) { _gravity = mathlib::Vec3(0,0,-g); } + void setGravity(double g) { _gravity = mathlib::Vec3(0,0,g); } void setGravityVec(const mathlib::Vec3& g) { _gravity = g; } const mathlib::Vec3& getGravityVec() const { return _gravity; } double getGravity() const { return _gravity.norm(); } diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index 87cb9c2f..07dca9a8 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -313,8 +313,7 @@ namespace physics { if (!scratch.dense.M.allFinite()) { throw std::runtime_error("Mass matrix contains non-finite values"); } - scratch.g.setZero(); - if (snap.torqueMode != systems::eTorqueMode::NONE) { scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } + if (snap.torqueMode != systems::eTorqueMode::NONE) { scratch.dense.tau_g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } scratch.dense.tau.setZero(); for (size_t i = 0; i < n; ++i) { @@ -345,7 +344,7 @@ namespace physics { const Scalar eps_f = static_cast(1e-2); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; // [Nm], control torque for joint i - tau_i += scratch.g[i]; // Gravity compensation + tau_i += scratch.dense.tau_g[i]; // Gravity compensation tau_i += scratch.dense.h[i]; // add Coriolis and centrifugal bias // Scalar tau_f = dynamics::computeKarnoppFriction(qd[i], tau_i, b, c); // add friction compensation // tau_i += tau_f; @@ -363,7 +362,7 @@ namespace physics { } // Solve Forward Dynamics: M(q) qdd = tau - h(q, qd) - g(q) - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.dense.tau_g; // [Nm], right-hand side of the dynamics equation M*qdd = tau - h - g // Solve for Accelerations Eigen::LDLT> solver(scratch.dense.M); @@ -565,9 +564,8 @@ namespace physics { computeMassMatrix(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses, scratch.dense.M); scratch.dense.h = computeCoriolisVector(*snap.model, q, qd, scratch.dense.T_world, scratch.dense.M); - scratch.g.setZero(); if (snap.torqueMode != systems::eTorqueMode::NONE) { - scratch.g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); + scratch.dense.tau_g = computeGravityTorque(*snap.model, scratch.dense.T_world, scratch.dense.jointWorldPoses); } scratch.dense.tau.setZero(); @@ -575,20 +573,20 @@ namespace physics { const systems::RigidBodyJoint& joint = snap.model->joints[i]; if (joint.type == systems::eJointType::FIXED) continue; - const Scalar eps = static_cast < Scalar>(1e-6); + const Scalar eps = static_cast(1e-6); const Scalar b = static_cast(joint.dynamics.damping); // viscous damping coefficient const Scalar c = static_cast(joint.dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-2); Scalar tau_i = kp[i] * (snap.q_ref[i] - q[i]) + kd[i] * (snap.qd_ref[i] - qd[i]) + mathlib::LSE_smoothMax(scratch.dense.M(i, i), eps) * snap.qdd_ref[i]; - tau_i += scratch.g[i] + scratch.dense.h[i]; + tau_i += scratch.dense.tau_g[i] + scratch.dense.h[i]; // is this meant to be tau_i -= b * qd[i]; tau_i -= c * mathlib::tanh(qd[i] / eps_f); scratch.dense.tau[i] = tau_i; } - scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.g; + scratch.dense.rhs.noalias() = scratch.dense.tau - scratch.dense.h - scratch.dense.tau_g; out.qdd = scratch.dense.M.ldlt().solve(scratch.dense.rhs); out.metrics.qdd = out.qdd; diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 360cc90e..cb0ca3e5 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -133,7 +133,7 @@ namespace physics { // Compute spatial velocities and transforms computeSpatialKinematicsAndBias(model, q, qd, Xup, v, c); // Compute spatial accelerations - computeAccelerations_RNEA(model, qdd, Xup, c, scratch.g, a); + computeAccelerations_RNEA(model, qdd, Xup, c, scratch.spatial.g, a); // Compute inverse dynamics (joint torques) computeBackwardForces_RNEA(model, Xup, v, a, tau); @@ -287,8 +287,12 @@ namespace physics { mathlib::SpatialVec_T a0; // base acceleration (gravity) a0.v << - scratch.g.template segment<3>(0), - scratch.g.template segment<3>(3); + scratch.spatial.g.template segment<3>(0), + scratch.spatial.g.template segment<3>(3); + + LOG_INFO_ONCE("a0 = [%.3f %.3f %.3f | %.3f %.3f %.3f], g.size=%d", + (double)a0.v(0),(double)a0.v(1),(double)a0.v(2), + (double)a0.v(3),(double)a0.v(4),(double)a0.v(5),(int)scratch.spatial.g.size()); computeSpatialKinematicsAndBias( model, q, qd, From 831e21be938e0bd31d5c810b1782e8fc30e622d8 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:19:54 +0100 Subject: [PATCH 039/114] fixes: Updated `ABA` to use new spatial scratch gravity vector --- DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl | 4 ---- 1 file changed, 4 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index cb0ca3e5..14679270 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -290,10 +290,6 @@ namespace physics { scratch.spatial.g.template segment<3>(0), scratch.spatial.g.template segment<3>(3); - LOG_INFO_ONCE("a0 = [%.3f %.3f %.3f | %.3f %.3f %.3f], g.size=%d", - (double)a0.v(0),(double)a0.v(1),(double)a0.v(2), - (double)a0.v(3),(double)a0.v(4),(double)a0.v(5),(int)scratch.spatial.g.size()); - computeSpatialKinematicsAndBias( model, q, qd, scratch.spatial.Xup, From 4385c66bbff8af6141b67ebaf68745b297d2d901 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:20:04 +0100 Subject: [PATCH 040/114] fix: Refactored gravity handling in RigidBodySystem to use updated gravity vector and adjust related calculations --- .../DSFE_Core/include/Systems/RigidBodySystemStep.inl | 8 ++++---- DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp | 8 ++++++-- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index 037b948e..73a2d8e8 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -39,7 +39,7 @@ namespace systems { snap.root_pose = _root_pose.template cast(); snap.baseIsFree = _baseIsFree; snap.lastBaseForwardForce = T(_lastBaseForwardForce); - snap.gravity = T(_gravity.z()); + snap.gravity = _gravity.template cast(); snap.torqueMode = _body.torqueMode; @@ -183,14 +183,14 @@ namespace systems { // Compute system potential energy at configuration q (relative to gravity) double sys_PE = 0.0; - double g = _dynamics->getGravity(); + auto g = _dynamics->getGravityVec(); for (size_t k = 0; k < _body.links.size(); ++k) { const RigidBodyLink& link = _body.links[k]; const double m = link.inertial.mass; if (m <= 0.0) { continue; } mathlib::Vec3 com_world = (T_world[k].block<3, 3>(0, 0) * link.inertial.com_xyz) + T_world[k].block<3, 1>(0, 3); - sys_PE += m * g * com_world.z(); + sys_PE += -m * g.dot(com_world); } const double sys_E = sys_KE + sys_PE; // total mechanical energy of the system @@ -216,7 +216,7 @@ namespace systems { e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = mathlib::real(dynResult.metrics.qdd[i]); e.err = err; e.err_d = err_d; e.I_eff = I_eff; - e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = 0.0; + e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = mathlib::real(dynResult.metrics.tau_g[i]); e.tau_sat = mathlib::real(dynResult.metrics.tau_sat[i]); e.KE = sys_KE; e.PE = sys_PE; e.E_total = sys_E; e.clamp_theta = mathlib::real(_clampTheta[i]); e.clamp_omega = mathlib::real(_clampOmega[i]); diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index da57023d..ac40260a 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -533,7 +533,11 @@ namespace systems { _dynResult.resize(_body.joints.size()); _dynResult_AD.resize(_body.joints.size()); - _dynScratch.g.setConstant(_gravity.z()); + _dynScratch.spatial.g = mathlib::VecX::Zero(6); + _dynScratch.spatial.g.segment<3>(3) = _gravity; + + _dynScratch_AD.spatial.g = mathlib::VecX_T>::Zero(6); + _dynScratch_AD.spatial.g.segment<3>(3) = _gravity.template cast>(); // Reset adaptive integrator so it doesn't carry a stale step size _integrator->resetAdaptiveState(); @@ -1023,7 +1027,7 @@ namespace systems { // Method to set the gravity strength for the rigidBody system void RigidBodySystem::setGravity(double g) { - _gravity = mathlib::Vec3(0.0, 0.0, -g); + _gravity = mathlib::Vec3(0.0, 0.0, g); _dynamics->setGravity(g); } // From 2f2c682cacf6c24e114698ac42b6762fda117575 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:31:14 +0100 Subject: [PATCH 041/114] feat: Added `RigidBodyLoaderJSON` and `RigidBodyLoaderURDF` for JSON and URDF parsing --- DSFE_App/DSFE_Core/CMakeLists.txt | 9 +++++---- .../{SystemLoader.cpp => RigidBodyLoaderJSON.cpp} | 2 +- .../DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp | 14 ++++++++++++++ 3 files changed, 20 insertions(+), 5 deletions(-) rename DSFE_App/DSFE_Core/src/Systems/{SystemLoader.cpp => RigidBodyLoaderJSON.cpp} (99%) create mode 100644 DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 4e95fa79..dd7ae98d 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -32,15 +32,16 @@ set(DEP_CORE_SRC src/Numerics/IntegrationService.cpp ) -set(SINGLE_BODY_SYS_SRC - src/SingleBodySystem/Body.cpp +set(LOADING_SRC + src/Systems/RigidBodyLoaderJSON.cpp + src/Systems/RigidBodyLoaderURDF.cpp ) set(SYSTEMS_SRC - src/Systems/SystemLoader.cpp src/Systems/RigidBodySystem.cpp src/Systems/TrajectoryManager.cpp src/Systems/RigidBodySnapshot.cpp + src/SingleBodySystem/Body.cpp ) set(PHYSICS_SRC @@ -88,8 +89,8 @@ set(SIM_CORE_SRC src/Scene/SimulationCore.cpp) # Add sources for DSFE_Core, including ImGui files target_sources(DSFE_Core PRIVATE ${DEP_CORE_SRC} - ${SINGLE_BODY_SYS_SRC} ${SYSTEMS_SRC} + ${LOADING_SRC} ${PHYSICS_SRC} ${PLATFORM_SRC} ${DSL_SRC} diff --git a/DSFE_App/DSFE_Core/src/Systems/SystemLoader.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp similarity index 99% rename from DSFE_App/DSFE_Core/src/Systems/SystemLoader.cpp rename to DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp index 07c776cb..c6268aeb 100644 --- a/DSFE_App/DSFE_Core/src/Systems/SystemLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp @@ -1,5 +1,5 @@ /* - * File: Systems/SystemLoader.cpp + * File: Systems/RigidBodyLoaderJSON.cpp * Created by: Joss Salton, 26-07-2026 */ #include "pch.h" diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp new file mode 100644 index 00000000..1934bb14 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -0,0 +1,14 @@ +/* + * File: Systems/RigidBodyLoaderURDF.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "Systems/RigidBodyLoader.h" +#include + +#include "EngineLib/LogMacros.h" + +namespace systems { + // --- URDF Loader --- +} \ No newline at end of file From c1dfc87e512e1817c16e65289d26344a9d8aec6d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:31:34 +0100 Subject: [PATCH 042/114] feat: Added `loadFromURDF` method to RigidBodyLoader for URDF file parsing --- DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h | 1 + 1 file changed, 1 insertion(+) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h index fe1fbf9c..5bab89be 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h @@ -11,5 +11,6 @@ namespace systems { class DSFE_API RigidBodyLoader { public: static RigidBodyModel loadFromJSON(const std::string& filepath); + static RigidBodyModel loadFromURDF(const std::string& filepath); }; } // namespace systems \ No newline at end of file From d0ecb59ecc2853bce7adfe5dae600bef89bae249 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 13:52:17 +0100 Subject: [PATCH 043/114] feat: Integrated tinyxml2 library using FetchContent in CMakeLists.txt --- DSFE_App/DSFE_Core/CMakeLists.txt | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index dd7ae98d..00c3bd2e 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -2,8 +2,24 @@ cmake_minimum_required(VERSION 3.17) project(DSFE_Core LANGUAGES C CXX) +include(FetchContent) + +set(tinyxml2_BUILD_TESTING OFF CACHE BOOL "" FORCE) +set(TINYXML2_BUILD_TESTING OFF CACHE BOOL "" FORCE) +FetchContent_Declare( + tinyxml2 + GIT_REPOSITORY https://github.com/leethomason/tinyxml2.git + GIT_TAGE 11.0.0 + SOURCE_SUBDIR nonexistent-dir-to-skip-their-cmake +) +FetchContent_MakeAvailable(tinyxml2) + add_library(DSFE_Core SHARED) +# Compile tinyxml2.cpp straight into DSFE_Core — no subproject, no .pc, no tests +target_sources(DSFE_Core PRIVATE ${tinyxml2_SOURCE_DIR}/tinyxml2.cpp) +target_include_directories(DSFE_Core PRIVATE ${tinyxml2_SOURCE_DIR}) + find_package(Threads REQUIRED) find_package(HDF5 REQUIRED) From acecc19d88d0927e0107752e13fb03c5cdd64d82 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 15:36:17 +0100 Subject: [PATCH 044/114] refactor: Added a dispatach method for loading bodies --- DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h index 5bab89be..458dd949 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h @@ -10,7 +10,8 @@ namespace systems { class DSFE_API RigidBodyLoader { public: - static RigidBodyModel loadFromJSON(const std::string& filepath); - static RigidBodyModel loadFromURDF(const std::string& filepath); + static RigidBodyModel load(const std::string& fp); + static RigidBodyModel loadFromJSON(const std::string& fp); + static RigidBodyModel loadFromURDF(const std::string& fp); }; } // namespace systems \ No newline at end of file From e23db6929d79044362f23433127d4ecdfc95f0c7 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 15:36:33 +0100 Subject: [PATCH 045/114] refactor: Simplified quaternion conversion by removing redundant variable assignments in `rpyRadToQuat` function --- .../DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp index c6268aeb..6a8441fd 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp @@ -21,14 +21,9 @@ namespace systems { // 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))); - + const Quat qx(Eigen::AngleAxisd(rpyRad.x(), Vec3(1.0, 0.0, 0.0))); + const Quat qy(Eigen::AngleAxisd(rpyRad.y(), Vec3(0.0, 1.0, 0.0))); + const Quat qz(Eigen::AngleAxisd(rpyRad.z(), Vec3(0.0, 0.0, 1.0))); return (qz * qy * qx).normalized(); } From 4abdc0b9d5eb3bd791a0e043d33aeab5bc6be6b3 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 15:36:44 +0100 Subject: [PATCH 046/114] feat: Implemented URDF parsing functionality in RigidBodyLoader, including link and joint parsing --- .../src/Systems/RigidBodyLoaderURDF.cpp | 187 +++++++++++++++++- 1 file changed, 185 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp index 1934bb14..d84c6268 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -8,7 +8,190 @@ #include #include "EngineLib/LogMacros.h" +#include + +#include +#include +#include + +using namespace tinyxml2; +using namespace constants; namespace systems { - // --- URDF Loader --- -} \ No newline at end of file + // rpy(radians) -> quaternion, q = qz*qy*qx, where qx = roll, qy = pitch, qz = yaw + static Quat urdf_rpyToQuat(const mathlib::Vec3& rpy) { + const Quat qx(Eigen::AngleAxisd(rpy.x(), Vec3(1.0, 0.0, 0.0))); + const Quat qy(Eigen::AngleAxisd(rpy.y(), Vec3(0.0, 1.0, 0.0))); + const Quat qz(Eigen::AngleAxisd(rpy.z(), Vec3(0.0, 0.0, 1.0))); + return (qz * qy * qx).normalized(); + } + // Parse a space-separated triple of doubles from a string, e.g. "1.0 2.0 3.0" + static Vec3 parseTriple(const char* s, mathlib::Vec3 fallback = Vec3::Zero()) { + if (!s) { return fallback; } + std::istringstream iss(s); + double x = fallback.x(), y = fallback.y(), z = fallback.z(); + iss >> x >> y >> z; + return mathlib::Vec3(x, y, z); + } + // Translate a URDF mesh path to a platform-specific path, e.g. "package://my_robot/meshes/part.stl" -> "rigidbody_models/airbus_vispa/my_robot/meshes/part.stl" + static std::string translateMeshPath(const std::string& raw) { + std::string path = raw; + // Remove "package://" prefix if present + const std::string pkg = "package://"; + if (path.rfind(pkg, 0) == 0) { path = path.substr(pkg.size()); } + // Replace forward slashes with platform-specific separators + std::replace(path.begin(), path.end(), '\\', '/'); + return "rigidbody_models/airbus_vispa/" + path; + } + // Link Parsing + static void urdf_parseLink(XMLElement* lEl, RigidBodyLink& link) { + link.name = lEl->Attribute("name") ? lEl->Attribute("name") : ""; + // Visual + if (XMLElement* v = lEl->FirstChildElement("visual")) { + // Origin + if (XMLElement* o = v->FirstChildElement("origin")) { + link.visual.origin_xyz = parseTriple(o->Attribute("xyz"), link.visual.origin_xyz); + link.visual.origin_rpy = parseTriple(o->Attribute("rpy"), link.visual.origin_rpy); + } + // Geometry & Mesh + if (XMLElement* g = v->FirstChildElement("geometry")) { + if (XMLElement* m = g->FirstChildElement("mesh")) { + if (const char* fn = m->Attribute("filename")) { + VisualMeshEntry entry; + entry.meshFile = translateMeshPath(fn); + link.visual.meshEntries.push_back(entry); + } + } + } + } + // Inertial + if (XMLElement* i = lEl->FirstChildElement("inertial")) { + // Mass + if (XMLElement* m = i->FirstChildElement("mass")) { + if (m->Attribute("value")) { link.inertial.mass = std::stod(m->Attribute("value")); } + } + // Origin + if (XMLElement* o = i->FirstChildElement("origin")) { + link.inertial.com_xyz = parseTriple(o->Attribute("xyz"), link.inertial.com_xyz); + } + // Inertia + if (XMLElement* I = i->FirstChildElement("inertia")) { + I->QueryDoubleAttribute("ixx", &link.inertial.inertia.ixx); + I->QueryDoubleAttribute("ixy", &link.inertial.inertia.ixy); + I->QueryDoubleAttribute("ixz", &link.inertial.inertia.ixz); + I->QueryDoubleAttribute("iyy", &link.inertial.inertia.iyy); + I->QueryDoubleAttribute("iyz", &link.inertial.inertia.iyz); + I->QueryDoubleAttribute("izz", &link.inertial.inertia.izz); + } + } + } + // Joint Parsing + static eJointType urdf_jointType(const std::string& t) { + if (t == "revolute") { return eJointType::REVOLUTE; } + if (t == "continuous") { return eJointType::REVOLUTE; } + if (t == "prismatic") { return eJointType::PRISMATIC; } + if (t == "fixed") { return eJointType::FIXED; } + if (t == "floating") { return eJointType::FREE; } + return eJointType::REVOLUTE; // default to revolute if unknown + } + // Parse a URDF joint element into a RigidBodyJoint structure + static void urdf_parseJoint(XMLElement* jEl, RigidBodyJoint& joint) { + joint.name = jEl->Attribute("name") ? jEl->Attribute("name") : ""; + const std::string typeStr = urdf_jointType(jEl->Attribute("type") ? jEl->Attribute("type") : ""); + const bool continuous = (typeStr == "continuous"); + joint.type = urdf_jointType(typeStr); + // Parse parent and child links + if (XMLElement* p = jEl->FirstChildElement("parent")) { + joint.parent = p->Attribute("link") ? p->Attribute("link") : ""; + } + if (XMLElement* c = jEl->FirstChildElement("child")) { + joint.child = c->Attribute("link") ? c->Attribute("link") : ""; + } + // Origin + if (XMLElement* o = jEl->FirstChildElement("origin")) { + joint.origin_xyz = parseTriple(o->Attribute("xyz"), joint.origin_xyz); + joint.origin_rpy = parseTriple(o->Attribute("rpy"), joint.origin_rpy); + } + joint.origin_q = urdf_rpyToQuat(joint.origin_rpy); + // Fixed joint special case: set axis to zero and limits to zero + if (joint.type == eJointType::FIXED) { + joint.axis = Vec3::Zero(); + joint.limits.continuous = false; + joint.limits.minAngle = 0.0; + joint.limits.maxAngle = 0.0; + return; + } + // Axis + if (XMLElement* a = jEl->FirstChildElement("axis")) { + joint.axis = parseTriple(a->Attribute("xyz"), Vec3(0, 0, 1)); + if (joint.axis.norm() < 1e-6) { joint.axis = Vec3(0, 0, 1); } // default axis if zero + else { joint.axis.normalize(); } + } else { + joint.axis = Vec3(0, 0, 1); // default axis if not specified + } + // Limits + if (XMLElement* l = jEl->FirstChildElement("limit")) { + l->QueryDoubleAttribute("lower", &joint.limits.minAngle); + l->QueryDoubleAttribute("upper", &joint.limits.maxAngle); + if (!continuous) { + L->QueryDoubleAttribute("lower", &joint.limits.minAngle); + L->QueryDoubleAttribute("upper", &joint.limits.maxAngle); + if (joint.limits.minAngle > joint.limits.maxAngle) { + std::swap(joint.limits.minAngle, joint.limits.maxAngle); + } + } + } + if (continuous) { joint.limits.minAngle = -PI_d; joint.limits.maxAngle = PI_d; } + + // Unlike my URDF-style JSON, URDF does not specify any control gains + // CHANGE AS NEEDED BUT DO NOT REMOVE (I will likely update my model to have a more robust way of dealing with these soon) + joint.dynamics.damping = 0.2; + joint.dynamics.friction = 0.05; + joint.wn_target = 2.5; + joint.zeta_target = 1.0; + } + + // Public API for loading a URDF file into a RigidBodyModel + RigidBodyModel RigidBodyLoader::loadFromURDF(const std::string& fp) { + RigidBodyModel rb; + LOG_INFO("Loading rigidbody model from URDF file: %s", fp.c_str()); + XMLDocument doc; + if (doc.LoadFile(fp.c_str()) != XML_SUCCESS) { + LOG_ERROR("Failed to load URDF file: %s (%s)", fp.c_str(), doc.ErrorStr()); + return rb; + } + + // Robot Element (may update for more generalised applications, though I know URDF's are usually robot-based configurations) + XMLElement* robot = doc.FirstChildElement("robot"); + if (!robot) { LOG_ERROR("No element found in URDF file: %s", fp.c_str()); return rb; } + rb.Name = robot->Attribute("name") ? robot->Attribute("name") : "unnamed_body"; + rb.scale = 1.0f; // URDF does not specify a scale, so we default to 1.0 + rb.kinematicsModel = eKinematicsModel::URDF; + + // URDF has NO baseframe, most of the models I use need a Z-up -> engine -90deg X rotation. + // We will default to this idea, BUT I will NEED to make this more configurable in the future for actual usability. + rb.baseFrameIsEngineAligned = false; + { + mathlib::Quat q = urdf_rpyToQuat(mathlib::Vec3(-PI_d / 2.0, 0.0, 0.0)); // Default base frame rotation to align URDF Z-up to engine Y-up + rb.baseFrame = mathlib::Mat4::Identity(); // Initialise base frame to identity + rb.baseFrame.block<3, 3>(0, 0) = q.toRotationMatrix(); // Set the rotation part of the base frame to the quaternion's rotation matrix + } + + // Links + for (XMLElement* lEl = robot->FirstChildElement("link"); lEl; lEl = lEl->NextSiblingElement("link")) { + RigidBodyLink link; + urdf_parseLink(lEl, link); + rb.links.push_back(link); + LOG_INFO("Link: %s | Mass: %.3f", link.name.c_str(), link.inertial.mass); + } + // Joints + for (XMLElement* jEl = robot->FirstChildElement("joint"); jEl; jEl = jEl->NextSiblingElement("joint")) { + RigidBodyJoint joint; + urdf_parseJoint(jEl, joint); + rb.joints.push_back(joint); + LOG_INFO("Joint: %s | %s -> %s | type=%d", joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), static_cast(joint.type)); + } + LOG_INFO("RigidBody (URDF) loaded: %d links, %d joints", static_cast(rb.links.size()), static_cast(rb.joints.size())); + return rb; + } +} // namespace systems \ No newline at end of file From 322594d485add6536001c214a0c85a30375306fe Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 15:36:53 +0100 Subject: [PATCH 047/114] feat: Added RigidBodyLoader implementation for loading URDF and JSON files --- .../DSFE_Core/src/Systems/RigidBodyLoader.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp new file mode 100644 index 00000000..0a3c2458 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp @@ -0,0 +1,26 @@ +/* + * File: Systems/RigidBodyLoaderURDF.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "Systems/RigidBodyLoader.h" +#include + +#include "EngineLib/LogMacros.h" + +namespace systems { + // Load dispatcher for all loading methods (JSON, URDF, etc.) + RigidBodyModel RigidBodyLoader::load(const std::string& fp) { + std::string ext; + const auto dot = fp.find_last_of('.'); + if (dot != std::string::npos) { + ext = fp.substr(dot + 1); + for (char& c : ext) { c = static_cast(std::tolower((unsigned char)c)); } + } + if (ext == "urdf" || ext == "xml") { return loadFromURDF(fp); } + if (ext == "json") { return loadFromJSON(fp); } + LOG_ERROR("Unsupported file extension '%s' for rigidBody model: %s", ext.c_str(), fp.c_str()); + return RigidBodyModel(); + } +} // namespace systems \ No newline at end of file From b0bbfea007f7e592ec1739553b3fd4cc421fb44c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 15:59:46 +0100 Subject: [PATCH 048/114] refactor: Enhanced URDF mesh path translation and link parsing with mesh directory support --- .../src/Systems/RigidBodyLoaderURDF.cpp | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp index d84c6268..10b31243 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -7,12 +7,14 @@ #include "Systems/RigidBodyLoader.h" #include +#include "Platform/Paths.h" #include "EngineLib/LogMacros.h" #include #include #include #include +#include using namespace tinyxml2; using namespace constants; @@ -34,17 +36,19 @@ namespace systems { return mathlib::Vec3(x, y, z); } // Translate a URDF mesh path to a platform-specific path, e.g. "package://my_robot/meshes/part.stl" -> "rigidbody_models/airbus_vispa/my_robot/meshes/part.stl" - static std::string translateMeshPath(const std::string& raw) { - std::string path = raw; + static std::string translateMeshPath(const std::string& raw, const std::string& meshdir) { + std::string p = raw; // Remove "package://" prefix if present const std::string pkg = "package://"; - if (path.rfind(pkg, 0) == 0) { path = path.substr(pkg.size()); } + if (p.rfind(pkg, 0) == 0) { p = p.substr(pkg.size()); } // Replace forward slashes with platform-specific separators - std::replace(path.begin(), path.end(), '\\', '/'); - return "rigidbody_models/airbus_vispa/" + path; + std::replace(p.begin(), p.end(), '\\', '/'); + const auto slash = p.find_last_of('/'); + const std::string fn = (slash == std::string::npos) ? p : p.substr(slash + 1); + return meshdir + "/" + fn; } // Link Parsing - static void urdf_parseLink(XMLElement* lEl, RigidBodyLink& link) { + static void urdf_parseLink(XMLElement* lEl, RigidBodyLink& link, const std::string& meshdir) { link.name = lEl->Attribute("name") ? lEl->Attribute("name") : ""; // Visual if (XMLElement* v = lEl->FirstChildElement("visual")) { @@ -58,7 +62,7 @@ namespace systems { if (XMLElement* m = g->FirstChildElement("mesh")) { if (const char* fn = m->Attribute("filename")) { VisualMeshEntry entry; - entry.meshFile = translateMeshPath(fn); + entry.meshFile = translateMeshPath(fn, meshdir); link.visual.meshEntries.push_back(entry); } } @@ -178,6 +182,18 @@ namespace systems { } // Links + const std::filesystem::path urdfDir = std::filesystem::path(filepath).parent_path(); + const std::filesystem::path assetRoot = paths::assets(); + // Attempts to compute the relative path from the URDF directory to the assets root, and appends "meshes" to it for mesh file resolution + std::string meshDirRel; + { + std::error_code ec; + auto rel = std::filesystem::relative(urdfDir, assetsRoot, ec); + meshDirRel = ec ? urdfDir.string() : rel.string(); + std::replace(meshDirRel.begin(), meshDirRel.end(), '\\', '/'); // Ensures forward slashes for consistency across platforms + meshDirRel += "/meshes"; // Append "meshes" to the relative path for mesh files + } + // Parse links and joints from the URDF for (XMLElement* lEl = robot->FirstChildElement("link"); lEl; lEl = lEl->NextSiblingElement("link")) { RigidBodyLink link; urdf_parseLink(lEl, link); From 4fc603184e7c101fc5d15d4cc0cf3a1ce4a116be Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 15:59:56 +0100 Subject: [PATCH 049/114] refactor: Updated RigidBody file loading to use a unified path and loader method --- DSFE_App/DSFE_Core/CMakeLists.txt | 1 + DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 00c3bd2e..c9d2cc48 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -49,6 +49,7 @@ set(DEP_CORE_SRC ) set(LOADING_SRC + src/Systems/RigidBodyLoader.cpp src/Systems/RigidBodyLoaderJSON.cpp src/Systems/RigidBodyLoaderURDF.cpp ) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index ac40260a..eac43876 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -440,15 +440,15 @@ namespace systems { resetDampingRatioToTarget(); // Construct path to rigidBody JSON file - const std::filesystem::path jsonPath = paths::assets() / "objects" / "Robotic_Arm_Models" / name / (name + ".json"); - if (!std::filesystem::exists(jsonPath)) { - LOG_ERROR("RigidBody JSON file not found -> %s", jsonPath.string().c_str()); - D_ERROR("RigidBody JSON file not found -> %s", jsonPath.string().c_str()); + const std::filesystem::path bodyPath = paths::assets() / name; + if (!std::filesystem::exists(bodyPath)) { + LOG_ERROR("RigidBody file not found -> %s", bodyPath.string().c_str()); + D_ERROR("RigidBody file not found -> %s", bodyPath.string().c_str()); return; } // Load rigidBody model from JSON - _body = systems::RigidBodyLoader::loadFromJSON(jsonPath.string()); + _body = systems::RigidBodyLoader::load(bodyPath.string()); const size_t n = _body.joints.size(); const size_t m = _body.links.size(); From 070920ae29531bafdd956a88d929d14393030057 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 16:01:04 +0100 Subject: [PATCH 050/114] refactor: Moved all existing robot model assets including license, JSON configuration, and mesh files to new path (more exact) --- .../H1/H1.json | 0 .../H1/LICENSE | 0 .../H1/meshes/left_ankle_link.stl | Bin .../H1/meshes/left_elbow_link.stl | Bin .../H1/meshes/left_hip_pitch_link.stl | Bin .../H1/meshes/left_hip_roll_link.stl | Bin .../H1/meshes/left_hip_yaw_link.stl | Bin .../H1/meshes/left_knee_link.stl | Bin .../H1/meshes/left_shoulder_pitch_link.stl | Bin .../H1/meshes/left_shoulder_roll_link.stl | Bin .../H1/meshes/left_shoulder_yaw_link.stl | Bin .../H1/meshes/logo_link.stl | Bin .../H1/meshes/pelvis.stl | Bin .../H1/meshes/right_ankle_link.stl | Bin .../H1/meshes/right_elbow_link.stl | Bin .../H1/meshes/right_hip_pitch_link.stl | Bin .../H1/meshes/right_hip_roll_link.stl | Bin .../H1/meshes/right_hip_yaw_link.stl | Bin .../H1/meshes/right_knee_link.stl | Bin .../H1/meshes/right_shoulder_pitch_link.stl | Bin .../H1/meshes/right_shoulder_roll_link.stl | Bin .../H1/meshes/right_shoulder_yaw_link.stl | Bin .../H1/meshes/torso_link.stl | Bin .../Panda/LICENSE | 0 .../Panda/Panda.json | 0 .../Panda/meshes/hand.stl | Bin .../Panda/meshes/link0.stl | Bin .../Panda/meshes/link1.stl | Bin .../Panda/meshes/link2.stl | Bin .../Panda/meshes/link3.stl | Bin .../Panda/meshes/link4.stl | Bin .../Panda/meshes/link6.stl | Bin .../Panda/meshes/link7.stl | Bin .../UR5e/LICENSE | 0 .../UR5e/UR5e.json | 0 .../Z1/LICENSE | 0 .../Z1/Z1.json | 0 .../Z1/meshes/z1_GripperMover.dae | 0 .../Z1/meshes/z1_GripperStator.dae | 0 .../Z1/meshes/z1_Link00.dae | 0 .../Z1/meshes/z1_Link01.dae | 0 .../Z1/meshes/z1_Link02.dae | 0 .../Z1/meshes/z1_Link03.dae | 0 .../Z1/meshes/z1_Link04.dae | 0 .../Z1/meshes/z1_Link05.dae | 0 .../Z1/meshes/z1_Link06.dae | 0 .../airbus_vispa/CITATION.cff | 26 +++ .../airbus_vispa}/LICENSE.txt | 0 .../airbus_vispa}/VISPA.json | 0 .../Link0-DHReference-PublicRelease.stl | Bin .../Link1-DHReference-PublicRelease.stl | Bin .../Link2-DHReference-PublicRelease.stl | Bin .../Link3-DHReference-PublicRelease.stl | Bin .../Link4-DHReference-PublicRelease.stl | Bin .../Link5-DHReference-PublicRelease.stl | Bin .../Link6-DHReference-PublicRelease.stl | Bin .../airbus_vispa/meshes/package.xml | 59 +++++++ .../airbus_vispa/urdf/VISPA_modifiedDH.urdf | 142 +++++++++++++++ .../urdf/VISPA_modifiedDH_CoppeliaSim.urdf | 142 +++++++++++++++ .../urdf/VISPA_modifiedDH_Ros2.urdf | 165 ++++++++++++++++++ .../iiwa14/LICENSE | 0 .../iiwa14/iiwa14.json | 0 .../iiwa14/meshes/collision/link_0.stl | Bin .../iiwa14/meshes/collision/link_0_s.stl | Bin .../iiwa14/meshes/collision/link_1.stl | Bin .../iiwa14/meshes/collision/link_1_s.stl | Bin .../iiwa14/meshes/collision/link_2.stl | Bin .../iiwa14/meshes/collision/link_2_s.stl | Bin .../iiwa14/meshes/collision/link_3.stl | Bin .../iiwa14/meshes/collision/link_3_s.stl | Bin .../iiwa14/meshes/collision/link_4.stl | Bin .../iiwa14/meshes/collision/link_4_s.stl | Bin .../iiwa14/meshes/collision/link_5.stl | Bin .../iiwa14/meshes/collision/link_5_s.stl | Bin .../iiwa14/meshes/collision/link_6.stl | Bin .../iiwa14/meshes/collision/link_6_s.stl | Bin .../iiwa14/meshes/collision/link_7.stl | Bin .../iiwa14/meshes/collision/link_7_s.stl | 0 .../iiwa14/meshes/visual/link_0.stl | Bin .../iiwa14/meshes/visual/link_1.stl | Bin .../iiwa14/meshes/visual/link_2.stl | Bin .../iiwa14/meshes/visual/link_3.stl | Bin .../iiwa14/meshes/visual/link_4.stl | Bin .../iiwa14/meshes/visual/link_5.stl | Bin .../iiwa14/meshes/visual/link_6.stl | Bin .../iiwa14/meshes/visual/link_7.stl | Bin 86 files changed, 534 insertions(+) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/H1.json (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/LICENSE (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_ankle_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_elbow_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_hip_pitch_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_hip_roll_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_hip_yaw_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_knee_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_shoulder_pitch_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_shoulder_roll_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/left_shoulder_yaw_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/logo_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/pelvis.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_ankle_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_elbow_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_hip_pitch_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_hip_roll_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_hip_yaw_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_knee_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_shoulder_pitch_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_shoulder_roll_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/right_shoulder_yaw_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/H1/meshes/torso_link.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/LICENSE (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/Panda.json (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/hand.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/link0.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/link1.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/link2.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/link3.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/link4.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/link6.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Panda/meshes/link7.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/UR5e/LICENSE (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/UR5e/UR5e.json (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/LICENSE (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/Z1.json (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_GripperMover.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_GripperStator.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_Link00.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_Link01.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_Link02.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_Link03.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_Link04.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_Link05.dae (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/Z1/meshes/z1_Link06.dae (100%) create mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/CITATION.cff rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/LICENSE.txt (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/VISPA.json (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/meshes/Link0-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/meshes/Link1-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/meshes/Link2-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/meshes/Link3-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/meshes/Link4-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/meshes/Link5-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models/VISPA => rigidbody_models/airbus_vispa}/meshes/Link6-DHReference-PublicRelease.stl (100%) create mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/package.xml create mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH.urdf create mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_CoppeliaSim.urdf create mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/LICENSE (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/iiwa14.json (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_0.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_0_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_1.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_1_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_2.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_2_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_3.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_3_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_4.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_4_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_5.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_5_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_6.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_6_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_7.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/collision/link_7_s.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_0.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_1.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_2.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_3.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_4.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_5.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_6.stl (100%) rename DSFE_App/DSFE_Engine/assets/{objects/Robotic_Arm_Models => rigidbody_models}/iiwa14/meshes/visual/link_7.stl (100%) diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/H1.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/H1.json similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/H1.json rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/H1.json diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/LICENSE b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/LICENSE similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/LICENSE rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/LICENSE diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_ankle_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_ankle_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_ankle_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_ankle_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_elbow_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_elbow_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_elbow_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_elbow_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_hip_pitch_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_hip_pitch_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_hip_pitch_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_hip_pitch_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_hip_roll_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_hip_roll_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_hip_roll_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_hip_roll_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_hip_yaw_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_hip_yaw_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_hip_yaw_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_hip_yaw_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_knee_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_knee_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_knee_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_knee_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_shoulder_pitch_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_shoulder_pitch_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_shoulder_pitch_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_shoulder_pitch_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_shoulder_roll_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_shoulder_roll_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_shoulder_roll_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_shoulder_roll_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_shoulder_yaw_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_shoulder_yaw_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/left_shoulder_yaw_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/left_shoulder_yaw_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/logo_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/logo_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/logo_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/logo_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/pelvis.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/pelvis.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/pelvis.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/pelvis.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_ankle_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_ankle_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_ankle_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_ankle_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_elbow_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_elbow_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_elbow_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_elbow_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_hip_pitch_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_hip_pitch_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_hip_pitch_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_hip_pitch_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_hip_roll_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_hip_roll_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_hip_roll_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_hip_roll_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_hip_yaw_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_hip_yaw_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_hip_yaw_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_hip_yaw_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_knee_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_knee_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_knee_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_knee_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_shoulder_pitch_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_shoulder_pitch_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_shoulder_pitch_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_shoulder_pitch_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_shoulder_roll_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_shoulder_roll_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_shoulder_roll_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_shoulder_roll_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_shoulder_yaw_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_shoulder_yaw_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/right_shoulder_yaw_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/right_shoulder_yaw_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/torso_link.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/torso_link.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/H1/meshes/torso_link.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/H1/meshes/torso_link.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/LICENSE b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/LICENSE similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/LICENSE rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/LICENSE diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/Panda.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/Panda.json similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/Panda.json rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/Panda.json diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/hand.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/hand.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/hand.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/hand.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link0.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link0.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link0.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link0.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link1.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link1.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link1.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link1.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link2.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link2.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link2.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link2.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link3.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link3.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link3.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link3.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link4.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link4.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link4.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link4.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link6.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link6.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link6.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link6.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link7.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link7.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Panda/meshes/link7.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Panda/meshes/link7.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/UR5e/LICENSE b/DSFE_App/DSFE_Engine/assets/rigidbody_models/UR5e/LICENSE similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/UR5e/LICENSE rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/UR5e/LICENSE diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/UR5e/UR5e.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/UR5e/UR5e.json similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/UR5e/UR5e.json rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/UR5e/UR5e.json diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/LICENSE b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/LICENSE similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/LICENSE rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/LICENSE diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/Z1.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/Z1.json similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/Z1.json rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/Z1.json diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_GripperMover.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_GripperMover.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_GripperMover.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_GripperMover.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_GripperStator.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_GripperStator.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_GripperStator.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_GripperStator.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link00.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link00.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link00.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link00.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link01.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link01.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link01.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link01.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link02.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link02.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link02.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link02.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link03.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link03.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link03.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link03.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link04.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link04.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link04.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link04.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link05.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link05.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link05.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link05.dae diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link06.dae b/DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link06.dae similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/Z1/meshes/z1_Link06.dae rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/Z1/meshes/z1_Link06.dae diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/CITATION.cff b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/CITATION.cff new file mode 100644 index 00000000..c15ff00c --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/CITATION.cff @@ -0,0 +1,26 @@ +cff-version: 1.2.0 +message: "If you use this software, please cite it using these metadata." +authors: +- family-names: "Shilton" + given-names: "Mark" + orcid: "https://orcid.org/0000-0002-6410-8139" +- family-names: "Garland" + given-names: "Martin" +- family-names: "Hackett" + given-names: "Chris" +- family-names: "Allouis" + given-names: "Elie" +- family-names: "Lisle" + given-names: "Matt" +- family-names: "Meringolo" + given-names: "Connor" +- family-names: "Paganini" + given-names: "Davide" +- family-names: "Hall" + given-names: "Alexander" + orcid: "https://orcid.org/0000-0002-5866-2221" +title: "VISPA URDF" +version: 0.2 +url: "https://github.com/AirbusDefenceAndSpace/vispa" + + diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/LICENSE.txt b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/LICENSE.txt similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/LICENSE.txt rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/LICENSE.txt diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/VISPA.json similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/VISPA.json diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link0-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link0-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link0-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link0-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link1-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link1-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link1-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link1-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link2-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link2-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link2-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link2-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link3-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link3-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link3-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link3-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link4-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link4-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link4-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link4-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link5-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link5-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link5-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link5-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link6-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link6-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/meshes/Link6-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link6-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/package.xml b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/package.xml new file mode 100644 index 00000000..f274c0c1 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/package.xml @@ -0,0 +1,59 @@ + + + CAD_STL + 0.0.0 + The CAD_STL package + + + + + mark + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH.urdf new file mode 100644 index 00000000..19e3efcf --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH.urdf @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_CoppeliaSim.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_CoppeliaSim.urdf new file mode 100644 index 00000000..45acd39a --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_CoppeliaSim.urdf @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf new file mode 100644 index 00000000..f36e7f92 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/LICENSE b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/LICENSE similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/LICENSE rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/LICENSE diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/iiwa14.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/iiwa14.json similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/iiwa14.json rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/iiwa14.json diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_0.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_0.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_0.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_0.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_0_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_0_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_0_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_0_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_1.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_1.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_1.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_1.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_1_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_1_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_1_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_1_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_2.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_2.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_2.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_2.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_2_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_2_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_2_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_2_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_3.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_3.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_3.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_3.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_3_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_3_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_3_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_3_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_4.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_4.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_4.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_4.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_4_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_4_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_4_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_4_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_5.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_5.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_5.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_5.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_5_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_5_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_5_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_5_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_6.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_6.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_6.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_6.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_6_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_6_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_6_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_6_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_7.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_7.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_7.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_7.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_7_s.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_7_s.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/collision/link_7_s.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/collision/link_7_s.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_0.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_0.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_0.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_0.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_1.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_1.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_1.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_1.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_2.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_2.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_2.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_2.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_3.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_3.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_3.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_3.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_4.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_4.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_4.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_4.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_5.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_5.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_5.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_5.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_6.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_6.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_6.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_6.stl diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_7.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_7.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/iiwa14/meshes/visual/link_7.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/iiwa14/meshes/visual/link_7.stl From 0a0069e2704df67248ddc469b02014c5085def35 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:40:46 +0100 Subject: [PATCH 051/114] refactor: Updated existing models --- .../urdf/VISPA_modifiedDH_Ros2.urdf | 165 ------------------ .../{airbus_vispa => vispa}/CITATION.cff | 0 .../{airbus_vispa => vispa}/LICENSE.txt | 0 .../Link0-DHReference-PublicRelease.stl | Bin .../Link1-DHReference-PublicRelease.stl | Bin .../Link2-DHReference-PublicRelease.stl | Bin .../Link3-DHReference-PublicRelease.stl | Bin .../Link4-DHReference-PublicRelease.stl | Bin .../Link5-DHReference-PublicRelease.stl | Bin .../Link6-DHReference-PublicRelease.stl | Bin .../meshes/package.xml | 0 .../urdf/VISPA_modifiedDH.urdf | 0 .../VISPA.json => vispa/vispa.json} | 0 .../vispa.urdf} | 0 14 files changed, 165 deletions(-) delete mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/CITATION.cff (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/LICENSE.txt (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/Link0-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/Link1-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/Link2-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/Link3-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/Link4-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/Link5-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/Link6-DHReference-PublicRelease.stl (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/meshes/package.xml (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa => vispa}/urdf/VISPA_modifiedDH.urdf (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa/VISPA.json => vispa/vispa.json} (100%) rename DSFE_App/DSFE_Engine/assets/rigidbody_models/{airbus_vispa/urdf/VISPA_modifiedDH_CoppeliaSim.urdf => vispa/vispa.urdf} (100%) diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf deleted file mode 100644 index f36e7f92..00000000 --- a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_Ros2.urdf +++ /dev/null @@ -1,165 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/CITATION.cff b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/CITATION.cff similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/CITATION.cff rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/CITATION.cff diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/LICENSE.txt b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/LICENSE.txt similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/LICENSE.txt rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/LICENSE.txt diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link0-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link0-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link0-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link0-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link1-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link1-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link1-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link1-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link2-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link2-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link2-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link2-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link3-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link3-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link3-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link3-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link4-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link4-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link4-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link4-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link5-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link5-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link5-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link5-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link6-DHReference-PublicRelease.stl b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link6-DHReference-PublicRelease.stl similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/Link6-DHReference-PublicRelease.stl rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/Link6-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/package.xml b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/package.xml similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/meshes/package.xml rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/package.xml diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/urdf/VISPA_modifiedDH.urdf similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH.urdf rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/urdf/VISPA_modifiedDH.urdf diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/VISPA.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.json similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/VISPA.json rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.json diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_CoppeliaSim.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf similarity index 100% rename from DSFE_App/DSFE_Engine/assets/rigidbody_models/airbus_vispa/urdf/VISPA_modifiedDH_CoppeliaSim.urdf rename to DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf From ca76e61a3f4d189ba1ce4e76f1a79c120c4e5931 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:40:57 +0100 Subject: [PATCH 052/114] refactor: Improved control torque saturation logic for better numerical stability --- DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index 07dca9a8..80d6a292 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -459,7 +459,7 @@ namespace physics { const Scalar Q_max = static_cast(snap.model->joints[i].limits.maxEffort); LOG_INFO_ONCE("Max effort for joint %zu: %g Nm", i, mathlib::real(Q_max)); - tau_i = Q_max * mathlib::tanh(tau_i / Q_max); // saturate control torque to max effort using smooth tanh saturation + if (Q_max > Scalar(1e-9)) { tau_i = Q_max * mathlib::tanh(tau_i / Q_max); } // saturate control torque to max effort using smooth tanh saturation scratch.dense.tau[i] = tau_i; From 57628254e180aa4f624e25df67a657ac9586daa5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:41:12 +0100 Subject: [PATCH 053/114] refactor: Enhanced URDF joint parsing and logging for improved clarity and functionality --- .../DSFE_Core/src/Systems/RigidBodyLoader.cpp | 1 + .../src/Systems/RigidBodyLoaderURDF.cpp | 24 +++++++++++-------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp index 0a3c2458..91474df1 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp @@ -18,6 +18,7 @@ namespace systems { ext = fp.substr(dot + 1); for (char& c : ext) { c = static_cast(std::tolower((unsigned char)c)); } } + LOG_INFO("Loading rigidBody model from file: %s (ext: %s)", fp.c_str(), ext.c_str()); if (ext == "urdf" || ext == "xml") { return loadFromURDF(fp); } if (ext == "json") { return loadFromJSON(fp); } LOG_ERROR("Unsupported file extension '%s' for rigidBody model: %s", ext.c_str(), fp.c_str()); diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp index 10b31243..ba29fa10 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -101,7 +101,7 @@ namespace systems { // Parse a URDF joint element into a RigidBodyJoint structure static void urdf_parseJoint(XMLElement* jEl, RigidBodyJoint& joint) { joint.name = jEl->Attribute("name") ? jEl->Attribute("name") : ""; - const std::string typeStr = urdf_jointType(jEl->Attribute("type") ? jEl->Attribute("type") : ""); + const std::string typeStr = jEl->Attribute("type") ? jEl->Attribute("type") : "revolute"; const bool continuous = (typeStr == "continuous"); joint.type = urdf_jointType(typeStr); // Parse parent and child links @@ -135,11 +135,11 @@ namespace systems { } // Limits if (XMLElement* l = jEl->FirstChildElement("limit")) { - l->QueryDoubleAttribute("lower", &joint.limits.minAngle); - l->QueryDoubleAttribute("upper", &joint.limits.maxAngle); + l->QueryDoubleAttribute("effort", &joint.limits.maxEffort); + l->QueryDoubleAttribute("lower", &joint.limits.minAngle); + l->QueryDoubleAttribute("upper", &joint.limits.maxAngle); + l->QueryDoubleAttribute("velocity", &joint.limits.maxqd); if (!continuous) { - L->QueryDoubleAttribute("lower", &joint.limits.minAngle); - L->QueryDoubleAttribute("upper", &joint.limits.maxAngle); if (joint.limits.minAngle > joint.limits.maxAngle) { std::swap(joint.limits.minAngle, joint.limits.maxAngle); } @@ -168,7 +168,7 @@ namespace systems { // Robot Element (may update for more generalised applications, though I know URDF's are usually robot-based configurations) XMLElement* robot = doc.FirstChildElement("robot"); if (!robot) { LOG_ERROR("No element found in URDF file: %s", fp.c_str()); return rb; } - rb.Name = robot->Attribute("name") ? robot->Attribute("name") : "unnamed_body"; + rb.name = robot->Attribute("name") ? robot->Attribute("name") : "unnamed_body"; rb.scale = 1.0f; // URDF does not specify a scale, so we default to 1.0 rb.kinematicsModel = eKinematicsModel::URDF; @@ -182,13 +182,13 @@ namespace systems { } // Links - const std::filesystem::path urdfDir = std::filesystem::path(filepath).parent_path(); + const std::filesystem::path urdfDir = std::filesystem::path(fp).parent_path(); const std::filesystem::path assetRoot = paths::assets(); // Attempts to compute the relative path from the URDF directory to the assets root, and appends "meshes" to it for mesh file resolution std::string meshDirRel; { std::error_code ec; - auto rel = std::filesystem::relative(urdfDir, assetsRoot, ec); + auto rel = std::filesystem::relative(urdfDir, paths::assets(), ec); meshDirRel = ec ? urdfDir.string() : rel.string(); std::replace(meshDirRel.begin(), meshDirRel.end(), '\\', '/'); // Ensures forward slashes for consistency across platforms meshDirRel += "/meshes"; // Append "meshes" to the relative path for mesh files @@ -196,7 +196,7 @@ namespace systems { // Parse links and joints from the URDF for (XMLElement* lEl = robot->FirstChildElement("link"); lEl; lEl = lEl->NextSiblingElement("link")) { RigidBodyLink link; - urdf_parseLink(lEl, link); + urdf_parseLink(lEl, link, meshDirRel); rb.links.push_back(link); LOG_INFO("Link: %s | Mass: %.3f", link.name.c_str(), link.inertial.mass); } @@ -205,7 +205,11 @@ namespace systems { RigidBodyJoint joint; urdf_parseJoint(jEl, joint); rb.joints.push_back(joint); - LOG_INFO("Joint: %s | %s -> %s | type=%d", joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), static_cast(joint.type)); + LOG_INFO("Joint: %s | %s -> %s | type=%d | axis=(%.3f, %.3f, %.3f) | maxEffort=%.3f | limits=(%.3f, %.3f) | maxVelocity=%.3f", + joint.name.c_str(), joint.parent.c_str(), joint.child.c_str(), static_cast(joint.type), + joint.axis.x(), joint.axis.y(), joint.axis.z(), + joint.limits.maxEffort, joint.limits.minAngle, joint.limits.maxAngle, joint.limits.maxqd + ); } LOG_INFO("RigidBody (URDF) loaded: %d links, %d joints", static_cast(rb.links.size()), static_cast(rb.joints.size())); return rb; From c18261d16f4573382610cd4fb29227e64bdd0236 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:41:32 +0100 Subject: [PATCH 054/114] refactor: Updated LoadCmd constructor to support robot type and improve path handling --- DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp index 9f997964..cf40d26c 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp @@ -19,14 +19,25 @@ namespace commands { // constructor LoadCmd::LoadCmd(const std::string& id, const std::vector& tokens) { - if (id == "rigidbody") { _target.type = LoadTargetType::RigidBody ; } + if (id == "rigidbody" || id == "robot") { + _target.type = LoadTargetType::RigidBody; + std::string name = toLower(tokens[0]); + if (tokens.size() == 1) { + LOG_WARN("load() command called with single token, assuming .urdf in rigidbody_models/%s", name.c_str()); + _path = "rigidbody_models/" + name + "/" + name + ".urdf"; + _target.path = _path; + return; + } + _path = "rigidbody_models/" + name + "/" + toLower(tokens[1]); + _target.path = _path; + return; + } else { std::string errMsg = "Invalid load(,...) identifier -> " + id; markFailed(errMsg); D_FAIL(errMsg.c_str()); return; } - _path = tokens[0]; _target.path = tokens[0]; } // Execute the command From 59e53a570c3b6fb712969d045dffa64498f69616 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:41:47 +0100 Subject: [PATCH 055/114] refactor: Updated `WorkspaceData` structure with rigid body path and lowercased name for JSON serialisation --- DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h | 3 ++- DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h index 990118ff..27feb228 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h @@ -12,7 +12,8 @@ namespace gui { struct WorkspaceData { int version = 1; QString name; - QString rigidBodyName; // empty = no rigid body loaded + QString rigidBodyName; // empty = no rigid body loaded (informational only, not a file path) + QString rigidBodyPath; // original rigid body file path if one was opened (necessary for re-load) QString scriptText; // DSL script embedded — file is self-contained QString scriptPath; // original script file if one was opened (informational) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp index 773230b3..874283ee 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp @@ -12,7 +12,8 @@ namespace gui { o["name"] = name; QJsonObject content; - content["rigid_body"] = rigidBodyName; + content["rigid_body"] = rigidBodyName.toLower(); + content["rigid_body_path"] = rigidBodyPath; content["script_text"] = scriptText; content["script_path"] = scriptPath; o["content"] = content; @@ -41,7 +42,8 @@ namespace gui { w.name = o["name"].toString(); const QJsonObject content = o["content"].toObject(); - w.rigidBodyName = content["rigid_body"].toString(); + w.rigidBodyName = content["rigid_body"].toString().toLower(); + w.rigidBodyPath = content["rigid_body_path"].toString(); w.scriptText = content["script_text"].toString(); w.scriptPath = content["script_path"].toString(); From 2315e731bcd22ff0c654ff8bf481a319f1a1c28f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:42:13 +0100 Subject: [PATCH 056/114] refactor: Normalised string handling in script file operations and improve logging --- DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp | 8 +++++--- .../src/MainWindow/Widgets/DSLEditorWidget.cpp | 10 +++++----- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 4b2e81bb..0fdcea6a 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -206,9 +206,11 @@ namespace window { QString robotName = QString::fromStdString(platform::RoboticSystems().toString(sys)); QAction* robotAction = familyMenus[family]->addAction(robotName); connect(robotAction, &QAction::triggered, this, [this, robotName]() { - LOG_INFO("Menu clicked: Project -> Load Robot -> %s", robotName.toStdString().c_str()); - showProjectPage(); // renderer init if we're still on the home page - _sim->load_rigidBody(robotName.toStdString()); + std::string n = robotName.toStdString(); + std::transform(n.begin(), n.end(), n.begin(), [](unsigned char c){ return std::tolower(c); }); + const std::string path = "rigidbody_models/" + n + "/" + n + ".urdf"; + LOG_INFO("Menu clicked: Project -> Load Robot -> %s", path.c_str()); + showProjectPage(); _sim->load_rigidBody(path); }); } } diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index 91022caf..15d38bbb 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp @@ -75,7 +75,7 @@ namespace widgets { QFile file(fileName); std::string nameStr = filenameFromPath(fileName.toStdString()).c_str(); if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - LOG_ERROR("Failed to open script file: %s", nameStr); + LOG_ERROR("Failed to open script file: %s", nameStr.c_str()); return false; } _scriptEditor->setPlainText(file.readAll()); @@ -85,8 +85,8 @@ namespace widgets { _scriptLinesLabel->setText(QString("Lines: %1").arg(_scriptEditor->toPlainText().split('\n').size())); _scriptCharsLabel->setText(QString("Chars: %1").arg(_scriptEditor->toPlainText().size())); - LOG_INFO("DSL script loaded from file: %s", nameStr); - D_INFO("DSL script loaded from file: %s", nameStr); + LOG_INFO("DSL script loaded from file: %s", nameStr.c_str()); + D_INFO("DSL script loaded from file: %s", nameStr.c_str()); return true; } @@ -102,8 +102,8 @@ namespace widgets { file.close(); _currentScriptPath = fileName; - LOG_INFO("DSL script saved to file: %s", nameStr); - D_INFO("DSL script saved to file: %s", nameStr); + LOG_INFO("DSL script saved to file: %s", nameStr.c_str()); + D_INFO("DSL script saved to file: %s", nameStr.c_str()); return true; } From 1d013a05dcac89c1da432997c944381f120d593a Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:42:34 +0100 Subject: [PATCH 057/114] refactor: Added `currentRigidBodyPath` to SimulationManager for correct workspace data handling --- DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h | 2 ++ DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp | 7 +++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h index 1bc213e2..e11cf18d 100644 --- a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h +++ b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h @@ -271,6 +271,7 @@ namespace gui { void gatherWorkspace(gui::WorkspaceData& w) const; // Fill the manager-owned parts of a workspace (rigidBody, camera). const std::string& currentRigidBodyName() const { return _currentRigidBodyName; } + const std::string& currentRigidBodyPath() const { return _currentRigidBodyPath; } void setManipulating(bool on); bool isManipulating() const; @@ -359,6 +360,7 @@ namespace gui { double _fixedDt = 1.0 / 180.0; double _telemetryHz = 100.0; std::string _currentRigidBodyName; + std::string _currentRigidBodyPath; scene::Object* _selectedObject = nullptr; std::vector> _objects; diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index e67cd6aa..a6ca71ae 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -156,6 +156,7 @@ namespace gui { _systems.add(std::make_unique(model, world_src, _mesh_store, *_sim_renderer), _scene); _core->clearRigidBodyPresentationDirty(); _currentRigidBodyName = model.name; + _currentRigidBodyPath = name; LOG_INFO("RigidBody loaded: %s", model.name.c_str()); } @@ -358,6 +359,7 @@ namespace gui { _renderer.destroy_all_meshes(); _mesh_store.clear(); _currentRigidBodyName.clear(); + _currentRigidBodyPath.clear(); _core->setScriptRunning(false); _core->stopSimulation(); @@ -378,14 +380,15 @@ namespace gui { _camera.setPosition(w.cameraPos); _camera.setYaw(w.cameraYaw); _camera.setPitch(w.cameraPitch); - if (!w.rigidBodyName.isEmpty()) { - load_rigidBody(w.rigidBodyName.toStdString()); // Core re-load or skip; GUI visuals rebuilt fresh + if (!w.rigidBodyPath.isEmpty()) { + load_rigidBody(w.rigidBodyPath.toStdString()); // Core re-load or skip; GUI visuals rebuilt fresh } LOG_INFO("Workspace applied: '%s'", w.name.toUtf8().constData()); } // Gather the current workspace state, filling the provided WorkspaceData structure with the current camera position, orientation, and rigidBody name void SimulationManager::gatherWorkspace(gui::WorkspaceData& w) const { w.rigidBodyName = QString::fromStdString(_currentRigidBodyName); + w.rigidBodyPath = QString::fromStdString(_currentRigidBodyPath); w.integrationMethod = static_cast(integrationMethod()); w.adIntegrationMethod = static_cast(autoDiffIntegrationMethod()); w.autoDiff = autoDiffEnabled(); From 445e3e181c7a33953a513d73c17b6205393fb296 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:42:40 +0100 Subject: [PATCH 058/114] refactor: Updated robot button click handling to use lowercase paths for URDF loading --- .../src/MainWindow/Widgets/RobotSelectorWidget.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp index c0693a0d..adb1aa75 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp @@ -35,8 +35,11 @@ namespace widgets { auto* button = new QPushButton(robotName + "\n" + company); layout()->addWidget(button); connect(button, &QPushButton::clicked, this, [this, robotName]() { - LOG_INFO("Selected robot: %s", robotName.toStdString().c_str()); - if (_sim) { _sim->load_rigidBody(robotName.toStdString()); } + std::string n = robotName.toStdString(); + std::transform(n.begin(), n.end(), n.begin(), [](unsigned char c){ return std::tolower(c); }); + const std::string path = "rigidbody_models/" + n + "/" + n + ".urdf"; + LOG_INFO("Selected robot: %s -> %s", n.c_str(), path.c_str()); + if (_sim) { _sim->load_rigidBody(path); } }); } } // namespace widgets \ No newline at end of file From b88299860bd283a224a0bb33dbbc5db4a813fee9 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 17:42:46 +0100 Subject: [PATCH 059/114] refactor: Updated file headers and simplified mesh path handling in MultiBodySystem and RigidBodyPresentationBuilder --- DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp | 7 +++++-- .../DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp | 7 +++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp index 7ec73830..26346af5 100644 --- a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp @@ -1,4 +1,7 @@ -// DSFE_GUI Systems/MultiBodySystem.cpp +/* + * File: Systems/MultiBodySystem.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "Systems/MultiBodySystem.h" #include "Simulation/SimulationScene.h" #include "Simulation/MeshStore.h" @@ -34,7 +37,7 @@ namespace gui { glm::vec3 lo(1e30f), hi(-1e30f); bool anyVerts = false; for (const auto& entry : link.visual.meshEntries) { - fs::path full = paths::assets() / "objects" / "Robotic_Arm_Models" / entry.meshFile; + fs::path full = paths::assets() / entry.meshFile; auto meshes = loader.load(full.string()); if (meshes.empty()) { LOG_ERROR("No meshes in %s", full.string().c_str()); continue; } for (auto& mptr : meshes) { diff --git a/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp index e31406c4..d1a972c4 100644 --- a/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp @@ -1,4 +1,7 @@ -// DSFE_GUI RobotPresentationBuilder.cpp +/* + * File: Systems/RigidBodyPresentationBuilder.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "Systems/RigidBodyPresentationBuilder.h" #include "Systems/RigidBodyModel.h" @@ -20,7 +23,7 @@ RigidBodyRenderBinding RigidBodyPresentationBuilder::build(const systems::RigidB for (const auto& link : model.links) { auto& visuals = binding.linkVisuals[link.name]; for (const auto& mesh : link.visual.meshEntries) { - fs::path fullPath = paths::assets() / "objects" / "Robotic_Arm_Models" / mesh.meshFile; + fs::path fullPath = paths::assets() / mesh.meshFile; auto meshes = loader.load(fullPath.string()); for (auto& m : meshes) { auto obj = std::make_unique(m); From b95f6e62fe41e98a50beac5dc41a10e241db1928 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 18:24:33 +0100 Subject: [PATCH 060/114] refactor: Updated link/joint naming styles --- .../assets/rigidbody_models/vispa/vispa.urdf | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf index 45acd39a..5286db3c 100644 --- a/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf @@ -1,6 +1,6 @@ - + @@ -13,7 +13,7 @@ - + @@ -26,7 +26,7 @@ - + @@ -39,7 +39,7 @@ - + @@ -52,7 +52,7 @@ - + @@ -65,7 +65,7 @@ - + @@ -78,7 +78,7 @@ - + @@ -91,49 +91,49 @@ - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + From e04c7b3af77fff7497c1a7ed0a2d8d788bea7e8f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 19:27:21 +0100 Subject: [PATCH 061/114] refactor: Added material properties to visual elements in vispa.urdf --- .../DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf index 5286db3c..1bd16932 100644 --- a/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf @@ -5,6 +5,7 @@ + @@ -18,6 +19,7 @@ + @@ -31,6 +33,7 @@ + @@ -44,6 +47,7 @@ + @@ -57,6 +61,7 @@ + @@ -70,6 +75,7 @@ + @@ -83,6 +89,7 @@ + From f51817cc13dc8ed58332008538627459c0fe28ca Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 19:27:26 +0100 Subject: [PATCH 062/114] refactor: Added material properties handling for links in URDF loader --- .../src/Systems/RigidBodyLoaderURDF.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp index ba29fa10..7ca57dd2 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -67,6 +67,24 @@ namespace systems { } } } + if (XMLElement* m = v->FirstChildElement("dsfe_material")) { + double r=0.7,g=0,b=0.2,a=1; + if (const char* rgba = m->Attribute("rgba")) { + std::istringstream iss(rgba); iss >> r >> g >> b >> a; + } + float metallic = 0.1f, roughness = 0.65f; + m->QueryFloatAttribute("metallic", &metallic); + m->QueryFloatAttribute("roughness", &roughness); + for (auto& entry : link.visual.meshEntries) { + LOG_INFO("Link %s has with rgba: %f %f %f %f, metallic: %f, roughness: %f", link.name.c_str(), r, g, b, a, metallic, roughness); + entry.material = Vec4(r, g, b, a); + entry.metallic = metallic; + entry.roughness = roughness; + entry.hasMaterial = true; + } + } else { + LOG_WARN("Link %s has NO ", link.name.c_str()); + } } // Inertial if (XMLElement* i = lEl->FirstChildElement("inertial")) { From efaa1e15107a451f4235a82672f0566bec9ef72b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Mon, 27 Jul 2026 19:28:11 +0100 Subject: [PATCH 063/114] refactor: Removed trailing newline in Workspace.h and added logging for material properties in MultiBodySystem.cpp --- DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h | 1 - 1 file changed, 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h index 27feb228..88cec419 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h @@ -36,5 +36,4 @@ namespace gui { bool saveToFile(const QString& path) const; static bool loadFromFile(const QString& path, WorkspaceData& out); }; - } // namespace gui \ No newline at end of file From 65c203120b965e05200980059b986b33c7ecb276 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 28 Jul 2026 06:48:11 +0100 Subject: [PATCH 064/114] refactor: Updated physical device selection in VulkanContext to prioritize graphics and present capabilities --- .../DSFE_GUI/src/Renderer/VulkanContext.cpp | 44 ++++++++++++++++++- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/Renderer/VulkanContext.cpp b/DSFE_App/DSFE_GUI/src/Renderer/VulkanContext.cpp index e944d485..85f6e9f2 100644 --- a/DSFE_App/DSFE_GUI/src/Renderer/VulkanContext.cpp +++ b/DSFE_App/DSFE_GUI/src/Renderer/VulkanContext.cpp @@ -130,6 +130,7 @@ namespace renderer { return true; } + // AFTER VkPhysicalDevice VulkanContext::find_physical_device() { uint32_t device_count = 0; vkEnumeratePhysicalDevices(_instance, &device_count, nullptr); @@ -141,13 +142,52 @@ namespace renderer { std::vector devices(device_count); vkEnumeratePhysicalDevices(_instance, &device_count, devices.data()); + // Returns true if this device has a queue family supporting BOTH graphics and present to our surface. + auto has_graphics_present = [this](VkPhysicalDevice dev) -> bool { + uint32_t count = 0; + vkGetPhysicalDeviceQueueFamilyProperties(dev, &count, nullptr); + std::vector families(count); + vkGetPhysicalDeviceQueueFamilyProperties(dev, &count, families.data()); + for (uint32_t i = 0; i < count; ++i) { + if (!(families[i].queueFlags & VK_QUEUE_GRAPHICS_BIT)) { continue; } + VkBool32 present = VK_FALSE; + vkGetPhysicalDeviceSurfaceSupportKHR(dev, i, _surface, &present); + if (present) { return true; } + } + return false; + }; + + VkPhysicalDevice best = VK_NULL_HANDLE; + int bestScore = -1; + for (VkPhysicalDevice device : devices) { VkPhysicalDeviceProperties props{}; vkGetPhysicalDeviceProperties(device, &props); - LOG_INFO("Found Vulkan device: %s", props.deviceName); + + // Ineligible if it can't present to our surface — skip regardless of type. + if (!has_graphics_present(device)) { + LOG_INFO("Found Vulkan device: %s (no graphics+present queue — skipping)", props.deviceName); + continue; + } + + int score = 0; + if (props.deviceType == VK_PHYSICAL_DEVICE_TYPE_DISCRETE_GPU) { score += 1000; } + else if (props.deviceType == VK_PHYSICAL_DEVICE_TYPE_INTEGRATED_GPU) { score += 100; } + score += static_cast(props.limits.maxImageDimension2D / 1000); // tiebreak on capability + + LOG_INFO("Found Vulkan device: %s (type %d, score %d)", props.deviceName, props.deviceType, score); + if (score > bestScore) { bestScore = score; best = device; } + } + + if (best == VK_NULL_HANDLE) { + LOG_ERROR("No Vulkan device with a graphics+present queue for this surface."); + return VK_NULL_HANDLE; } - return devices.front(); + VkPhysicalDeviceProperties p{}; + vkGetPhysicalDeviceProperties(best, &p); + LOG_INFO("Selected Vulkan device: %s", p.deviceName); + return best; } // Find a graphics queue family that supports both graphics and present operations for the given physical device and surface. From 0009ee075c0d98eb9feb4a5f3247e4abad11e374 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 28 Jul 2026 06:49:21 +0100 Subject: [PATCH 065/114] fixes: Bad include header capitalisation --- DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp index 7ca57dd2..65846a5d 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -11,7 +11,7 @@ #include "EngineLib/LogMacros.h" #include -#include +#include #include #include #include From 1664ce02c9d772793c1d51444de225d11c145cb5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 28 Jul 2026 08:08:25 +0100 Subject: [PATCH 066/114] refactor: Updated joint information display in ControlPanelWidget and update styling in dsfe_dark.qss --- DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe | 20 +- .../MainWindow/Widgets/ControlPanelWidget.h | 4 + .../DSFE_GUI/resources/style/dsfe_dark.qss | 54 ++- .../MainWindow/Widgets/ControlPanelWidget.cpp | 311 ++++++++++-------- .../src/MainWindow/Workspace/HomePage.cpp | 2 +- 5 files changed, 244 insertions(+), 147 deletions(-) diff --git a/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe b/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe index f036e5bb..1b687151 100644 --- a/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe +++ b/DSFE_App/DSFE_GUI/assets/templates/vispa.dsfe @@ -1,22 +1,28 @@ { "camera": { - "pitch": -0.06199825182557106, + "pitch": -0.4689960479736328, "pos": [ - 3.1640207767486572, - 1.1873780488967896, - 2.983812093734741 + 1.2224150896072388, + 0.6096836924552917, + -0.11915026605129242 ], - "yaw": -8.675688743591309 + "yaw": -9.512792587280273 }, "content": { - "rigid_body": "VISPA", + "rigid_body": "vispa", + "rigid_body_path": "rigidbody_models/vispa/vispa.urdf", "script_path": "", - "script_text": "# --------------------------------------------\n# TEMPLATE PROJECT: Airbus VISPA Ready-State Observer Positoion\n# --------------------------------------------\n#\n# MISSION PROFILE\n# ---------------\n# 1. System Initialization & Calibration Check (0.0s - 5.0s) - Before start\n# 2. Nominal Deployment to Ready-State Observer Position (5.0s - 35.0s)\n#\n# JOINT LIMITS\n# ------------\n# j1-j6: +/- ~180 deg (+/-3.14149 rad)\n# v_max: 5.38 deg/s (0.0940 rad/s) hardware limit\n# v_operating: < 1.5 deg/s\n# Q_max: 50 Nm per joint\n# Damping / Friction: 0.2 / 0.05 (all joints)\n#\n# LINK MASSES (kg)\n# ----------------\n# link00 0.627 base adapter (gold coloured)\n# link01 2.328 shoulder yaw\n# link02 3.995 upper arm (0.8m, heaviest link)\n# link03 2.328 elbow\n# link04 3.157 forearm (0.65m)\n# link05 2.695 wrist roll\n# link06 0.924 end-effector flange\n#\n# Create By: Joss Salton\n# GitHub: SaltyJoss\n#\n# --------------------------------------------\n\nload(robot, VISPA)\n\nwait(4.0)\ntrajClear()\nwait(1.0)\n\n# Begins sim run and logging.\nstart()\n\n# Nominal Deployment\nparallel(30.0) {\n trajSet(link01, TRAP, -30.0, 1.5, 5.0)\n trajSet(link02, TRAP, 45.0, 2.0, 5.0)\n trajSet(link03, TRAP, -90.0, 2.0, 5.0)\n trajSet(link04, TRAP, 0.0, 1.0, 3.0)\n trajSet(link05, TRAP, 45.0, 1.5, 4.0)\n trajSet(link06, TRAP, 0.0, 1.0, 3.0)\n}\n\nwait(30.0)\n\n# --------------------------------------------\n# END: ~30 seconds\n# --------------------------------------------" + "script_text": "# --------------------------------------------\n# TEMPLATE PROJECT: Airbus VISPA Ready-State Observer Positoion\n# --------------------------------------------\n#\n# MISSION PROFILE\n# ---------------\n# 1. System Initialization & Calibration Check (0.0s - 5.0s) - Before start\n# 2. Nominal Deployment to Ready-State Observer Position (5.0s - 35.0s)\n#\n# JOINT LIMITS\n# ------------\n# j1-j6: +/- ~180 deg (+/-3.14149 rad)\n# v_max: 5.38 deg/s (0.0940 rad/s) hardware limit\n# v_operating: < 1.5 deg/s\n# Q_max: 50 Nm per joint\n# Damping / Friction: 0.2 / 0.05 (all joints)\n#\n# LINK MASSES (kg)\n# ----------------\n# link00 0.627 base adapter (gold coloured)\n# link01 2.328 shoulder yaw\n# link02 3.995 upper arm (0.8m, heaviest link)\n# link03 2.328 elbow\n# link04 3.157 forearm (0.65m)\n# link05 2.695 wrist roll\n# link06 0.924 end-effector flange\n#\n# Create By: Joss Salton\n# GitHub: SaltyJoss\n#\n# --------------------------------------------\n\nload(rigidbody, vispa, vispa.urdf)\n\nwait(4.0)\ntrajClear()\nwait(1.0)\n\n# Begins sim run and logging.\nstart()\n\n# Nominal Deployment\nparallel(30.0) {\n trajSet(link01, TRAP, -30.0, 1.5, 5.0)\n trajSet(link02, TRAP, 45.0, 2.0, 5.0)\n trajSet(link03, TRAP, -90.0, 2.0, 5.0)\n trajSet(link04, TRAP, 0.0, 1.0, 3.0)\n trajSet(link05, TRAP, 45.0, 1.5, 4.0)\n trajSet(link06, TRAP, 0.0, 1.0, 3.0)\n}\n\nwait(30.0)\n\n# --------------------------------------------\n# END: ~30 seconds\n# --------------------------------------------" }, "name": "vispa", "simulation": { "ad_integration_method": 0, "auto_diff": false, + "gravity": [ + 0, + 0, + 0 + ], "integration_method": 4, "sim_dt": 0.005555555555555556, "telemetry_dt": 0.008333333333333333 diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 7d2b6b5d..cd23a34d 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h @@ -103,6 +103,10 @@ namespace widgets { void updateSimClock(); + QLabel* _jointChainLabel = nullptr; + QLabel* _jointIndexLabel = nullptr; + void refreshJointChainLabel(int idx); + gui::SimulationManager* _sim = nullptr; QVBoxLayout* _contentLayout = nullptr; diff --git a/DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss b/DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss index e66fb07a..6e118994 100644 --- a/DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss +++ b/DSFE_App/DSFE_GUI/resources/style/dsfe_dark.qss @@ -95,6 +95,7 @@ QPushButton { border: 1px solid rgb(70,70,70); border-radius: 3px; padding: 4px 10px; + font-family: Consolas, monospace; } QPushButton:hover { @@ -145,10 +146,12 @@ QListWidget { background-color: rgb(26,26,26); border: 1px solid rgb(70,70,70); border-radius: 4px; + outline: none; } QListWidget::item { padding: 8px; + outline: none; border-radius: 3px; } @@ -158,6 +161,7 @@ QListWidget::item:hover { QListWidget::item:selected { background-color: rgb(86,156,214); + outline: none; } /* ---- Home page ---- */ @@ -178,15 +182,19 @@ QListWidget::item:selected { } #brand_title { - font-size: 34px; + font-family: "Cascadia Mono", Consolas, monospace; + font-size: 44px; font-weight: 700; - color: rgb(235,235,240); + color: rgb(240,240,250); + letter-spacing: 4px; } #brand_subtitle { - font-size: 16px; + font-family: Consolas, monospace; + font-size: 14px; font-weight: 500; color: rgb(130,130,138); + letter-spacing: 1px; } #section_header { @@ -201,6 +209,7 @@ QListWidget::item:selected { border-radius: 5px; color: rgb(255,255,255); font-weight: 600; + font-family: Consolas, monospace; } #prim_btn:hover { background-color: rgb(104,170,224); } #prim_btn:pressed { background-color: rgb(70,140,200); } @@ -211,6 +220,7 @@ QListWidget::item:selected { border-radius: 5px; color: rgb(220,220,225); font-weight: 500; + font-family: Consolas, monospace; } #second_btn:hover { background-color: rgb(68,68,74); } @@ -261,4 +271,42 @@ QListWidget::item:selected { background: transparent; font-family: Consolas, monospace; font-size: 12px; +} + +/* ---- Telemetry / Joint info panels ---- */ + +/* Section headers: STATE, REFERENCE, TRAJECTORY, LIMITS, PHYSICAL */ +#telem_header { + color: rgb(200,205,215); + font-weight: bold; + font-size: 12px; + letter-spacing: 1px; +} + +/* Math-symbol row labels: θ, ω, τ, θ_ref ... (rich text sets italics inline) */ +#telem_symbol { + color: rgb(170,175,185); + background: transparent; + font-size: 12px; +} + +/* Numeric value readouts: monospace, right-aligned digits */ +#telem_value { + color: rgb(225,228,235); + background: transparent; + font-family: Consolas, monospace; + font-size: 12px; +} + +/* Joint chain context line: parent -> [joint] -> child */ +#joint_chain { + background: transparent; + font-size: 12px; +} + +/* "joint n / total" index readout */ +#joint_index { + background: transparent; + font-family: Consolas, monospace; + font-size: 11px; } \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 2e6c61c9..69d5cef8 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -202,60 +202,114 @@ namespace widgets { _jointInfoGroup = new QGroupBox("RigidBody Joint Information"); auto* layout = new QVBoxLayout(_jointInfoGroup); _jointInfoGroup->setLayout(layout); - layout->addWidget(new QLabel("Joint")); + + // --- Joint chain context: parent -> [joint] -> child --- + _jointChainLabel = new QLabel(); + _jointChainLabel->setTextFormat(Qt::RichText); + _jointChainLabel->setAlignment(Qt::AlignCenter); + _jointChainLabel->setWordWrap(true); + _jointChainLabel->setStyleSheet("color: rgb(200,205,215);"); + layout->addWidget(_jointChainLabel); + + // --- Index readout: "joint 3 / 6" --- + _jointIndexLabel = new QLabel(); + _jointIndexLabel->setTextFormat(Qt::RichText); + _jointIndexLabel->setAlignment(Qt::AlignCenter); + layout->addWidget(_jointIndexLabel); + + layout->addSpacing(4); _jointIdxSlider = new QSlider(Qt::Horizontal, _jointInfoGroup); _jointIdxSlider->setMinimum(1); _jointIdxSlider->setValue(1); - - connect(_jointIdxSlider, &QSlider::valueChanged, this, [this](int value) { selectJointAndFollow(value - 1); }); - + _jointIdxSlider->setTickPosition(QSlider::TicksBelow); + _jointIdxSlider->setTickInterval(1); + _jointIdxSlider->setSingleStep(1); + _jointIdxSlider->setPageStep(1); + + connect(_jointIdxSlider, &QSlider::valueChanged, this, [this](int value) { + selectJointAndFollow(value - 1); refreshJointChainLabel(value - 1); + }); layout->addWidget(_jointIdxSlider); - + layout->addSpacing(6); buildTelemetryWidgets(layout); - _contentLayout->addWidget(_jointInfoGroup); } - - if (!_sim || !_sim->hasRigidBody()) { - _jointInfoGroup->setVisible(false); - return; - } + if (!_sim || !_sim->hasRigidBody()) { _jointInfoGroup->setVisible(false); return; } auto& body = _sim->rigidBodySystem(); auto& joints = body.joints(); auto& links = body.links(); - - _jointInfoGroup->setVisible(!joints.empty() && !links.empty()); if (joints.empty() || links.empty()) { _jointInfoGroup->setVisible(false); return; } + _jointInfoGroup->setVisible(true); _jointIdxSlider->setMaximum(static_cast(joints.size())); static int currentJointIndex = 0; currentJointIndex = std::clamp(currentJointIndex, 0, (int)joints.size() - 1); + refreshJointChainLabel(_jointIdxSlider->value() - 1); + } + + // Renders "parent -> [ Joint_n ] -> child" and the "n / total" index readout for the given joint. + void ControlPanelWidget::refreshJointChainLabel(int idx) { + if (!_sim || !_sim->hasRigidBody()) { return; } + auto& body = _sim->rigidBodySystem(); + auto& joints = body.joints(); + if (joints.empty()) { return; } + idx = std::clamp(idx, 0, (int)joints.size() - 1); + const auto& j = joints[idx]; + const QString parent = QString::fromStdString(j.parent); + const QString child = QString::fromStdString(j.child); + const QString name = QString::fromStdString(j.name); + // parent link (dim) -> joint (bright, italic-ish) -> child link (dim) + _jointChainLabel->setText(QString( + "%1" + " \u27F6 " + "[ %2 ]" + " \u27F6 " + "%3") + .arg(parent, name, child) + ); + // "joint n / total" with the count in a dim weight + _jointIndexLabel->setText(QString( + "joint " + "%1" + " / " + "%2") + .arg(idx + 1).arg(joints.size()) + ); } void ControlPanelWidget::updateTelemetryInfo(const diagnostics::JointTelemetry& j) { auto& t = _telemetryLabels; - const float e = static_cast(j.q_ref - j.q); - - t.q->setText(QString("%1 rad").arg(j.q)); - t.qd->setText(QString("%1 rad/s").arg(j.qd)); - t.tau->setText(QString("%1 Nm").arg(j.torqueNm)); - - t.qRef->setText(QString("%1 rad").arg(j.q_ref)); - t.qdRef->setText(QString("%1 rad/s").arg(j.qd_ref)); - t.qddRef->setText(QString("%1 rad/s²").arg(j.qdd_ref)); - t.err->setText(QString("%1 rad").arg(e)); + const double e = j.q_ref - j.q; - t.qTraj->setText(QString("%1 rad").arg(j.traj_q)); - t.qdTraj->setText(QString("%1 rad/s").arg(j.traj_qd)); - t.qddTraj->setText(QString("%1 rad/s²").arg(j.traj_qdd)); - - t.qClamped->setText(j.clampTheta ? "On" : "Off"); - t.qdClamped->setText(j.clampOmega ? "On" : "Off"); - - t.damping->setText(QString("%1 kg·m²/s").arg(j.damping)); - t.friction->setText(QString("%1 N·m").arg(j.friction)); + // fixed-width numeric formatting so columns don't jitter as values change + auto num = [](double v, const char* unit) { + return QString("%1 %2").arg(v, 0, 'f', 4).arg(unit); + }; + // State Telemetry + t.q->setText(num(j.q, "rad")); + t.qd->setText(num(j.qd, "rad/s")); + t.tau->setText(num(j.torqueNm, "N\u00B7m")); + // Reference Telemetry + t.qRef->setText(num(j.q_ref, "rad")); + t.qdRef->setText(num(j.qd_ref, "rad/s")); + t.qddRef->setText(num(j.qdd_ref, "rad/s\u00B2")); + t.err->setText(num(e, "rad")); + // Trajectory Telemetry + t.qTraj->setText(num(j.traj_q, "rad")); + t.qdTraj->setText(num(j.traj_qd, "rad/s")); + t.qddTraj->setText(num(j.traj_qdd, "rad/s\u00B2")); + // Clamping Telemetry + t.qClamped->setText(j.clampTheta + ? "active" + : "\u2014"); + t.qdClamped->setText(j.clampOmega + ? "active" + : "\u2014"); + // Constants Telemetry + t.damping->setText(num(j.damping, "kg\u00B7m\u00B2/s")); + t.friction->setText(num(j.friction, "N\u00B7m")); } void ControlPanelWidget::buildTelemetryWidgets(QVBoxLayout* layout) { @@ -263,120 +317,105 @@ namespace widgets { QFont font = label->font(); font.setBold(true); font.setPointSize(font.pointSize() + 1); + font.setLetterSpacing(QFont::PercentageSpacing, 115); // tracked-out caps read as section headers label->setFont(font); - label->setStyleSheet("color: rgb(220,220,220);"); + label->setStyleSheet("color: rgb(200,205,215);"); + }; + + // A math-symbol row label: rich-text italic variable, e.g. "θ" or "θ_ref". + auto symLabel = [](const QString& html) { + auto* l = new QLabel(html); + l->setTextFormat(Qt::RichText); + l->setObjectName("telem_symbol"); + return l; }; + // A value label: monospace, right-aligned so digits column up. + auto valueLabel = [](QLabel* l) { + l->setTextFormat(Qt::RichText); + l->setAlignment(Qt::AlignRight | Qt::AlignVCenter); + QFont f("Consolas"); // or "JetBrains Mono"/"Cascadia Mono" if bundled + f.setStyleHint(QFont::Monospace); + f.setPointSize(l->font().pointSize()); + l->setFont(f); + l->setStyleSheet("color: rgb(225,228,235);"); + return l; + }; auto& t = _telemetryLabels; - t.stateHeader = new QLabel("STATE"); - t.referenceHeader = new QLabel("REFERENCE"); + t.stateHeader = new QLabel("STATE"); + t.referenceHeader = new QLabel("REFERENCE"); t.trajectoryHeader = new QLabel("TRAJECTORY"); - t.clampedHeader = new QLabel("LIMITS"); - t.constantsHeader = new QLabel("PHYSICAL"); - - headerFont(t.stateHeader); - headerFont(t.referenceHeader); - headerFont(t.trajectoryHeader); - headerFont(t.clampedHeader); - headerFont(t.constantsHeader); - - t.q = new QLabel(); - t.qd = new QLabel(); - t.tau = new QLabel(); - - t.qRef = new QLabel(); - t.qdRef = new QLabel(); - t.qddRef = new QLabel(); - t.err = new QLabel(); - - t.qTraj = new QLabel(); - t.qdTraj = new QLabel(); - t.qddTraj = new QLabel(); - - t.qClamped = new QLabel(); - t.qdClamped = new QLabel(); - - t.damping = new QLabel(); - t.friction = new QLabel(); - - auto* stateGrid = new QGridLayout(); - stateGrid->addWidget(new QLabel("Position"), 0, 0); - stateGrid->addWidget(t.q, 0, 1); - stateGrid->addWidget(new QLabel("Velocity"), 1, 0); - stateGrid->addWidget(t.qd, 1, 1); - stateGrid->addWidget(new QLabel("Torque"), 2, 0); - stateGrid->addWidget(t.tau, 2, 1); - - stateGrid->setHorizontalSpacing(12); - stateGrid->setColumnStretch(0, 0); - stateGrid->setColumnStretch(1, 1); - - auto* refGrid = new QGridLayout(); - refGrid->addWidget(new QLabel("Target Pos"), 0, 0); - refGrid->addWidget(t.qRef, 0, 1); - refGrid->addWidget(new QLabel("Target Vel"), 1, 0); - refGrid->addWidget(t.qdRef, 1, 1); - refGrid->addWidget(new QLabel("Target Acc"), 2, 0); - refGrid->addWidget(t.qddRef, 2, 1); - refGrid->addWidget(new QLabel("Error"), 3, 0); - refGrid->addWidget(t.err, 3, 1); - - refGrid->setHorizontalSpacing(12); - refGrid->setColumnStretch(0, 0); - refGrid->setColumnStretch(1, 1); - - auto* trajGrid = new QGridLayout(); - trajGrid->addWidget(new QLabel("Position"), 0, 0); - trajGrid->addWidget(t.qTraj, 0, 1); - trajGrid->addWidget(new QLabel("Velocity"), 1, 0); - trajGrid->addWidget(t.qdTraj, 1, 1); - trajGrid->addWidget(new QLabel("Acceleration"), 2, 0); - trajGrid->addWidget(t.qddTraj, 2, 1); - - trajGrid->setHorizontalSpacing(12); - trajGrid->setColumnStretch(0, 0); - trajGrid->setColumnStretch(1, 1); - - auto* limitGrid = new QGridLayout(); - limitGrid->addWidget(new QLabel("Position Clamp"), 0, 0); - limitGrid->addWidget(t.qClamped, 0, 1); - limitGrid->addWidget(new QLabel("Velocity Clamp"), 1, 0); - limitGrid->addWidget(t.qdClamped, 1, 1); - - limitGrid->setHorizontalSpacing(12); - limitGrid->setColumnStretch(0, 0); - limitGrid->setColumnStretch(1, 1); - - auto* physicalGrid = new QGridLayout(); - physicalGrid->addWidget(new QLabel("Damping"), 0, 0); - physicalGrid->addWidget(t.damping, 0, 1); - physicalGrid->addWidget(new QLabel("Friction"), 1, 0); - physicalGrid->addWidget(t.friction, 1, 1); - - physicalGrid->setHorizontalSpacing(12); - physicalGrid->setColumnStretch(0, 0); - physicalGrid->setColumnStretch(1, 1); - - layout->addSpacing(5); - layout->addWidget(t.stateHeader); - layout->addLayout(stateGrid); + t.clampedHeader = new QLabel("LIMITS"); + t.constantsHeader = new QLabel("PHYSICAL"); + for (QLabel* h : { t.stateHeader, t.referenceHeader, t.trajectoryHeader, t.clampedHeader, t.constantsHeader }) + headerFont(h); + + t.q = new QLabel(); t.qd = new QLabel(); t.tau = new QLabel(); + t.qRef = new QLabel(); t.qdRef = new QLabel(); t.qddRef = new QLabel(); t.err = new QLabel(); + t.qTraj = new QLabel(); t.qdTraj = new QLabel(); t.qddTraj = new QLabel(); + t.qClamped = new QLabel(); t.qdClamped = new QLabel(); + t.damping = new QLabel(); t.friction = new QLabel(); + for (QLabel* v : { t.q, t.qd, t.tau, t.qRef, t.qdRef, t.qddRef, t.err, + t.qTraj, t.qdTraj, t.qddTraj, t.qClamped, t.qdClamped, t.damping, t.friction }) + valueLabel(v); + + auto makeGrid = [](std::initializer_list> rows) { + auto* g = new QGridLayout(); + int r = 0; + for (auto& [sym, val] : rows) { + g->addWidget(sym, r, 0, Qt::AlignLeft | Qt::AlignVCenter); + g->addWidget(val, r, 1); + ++r; + } + g->setHorizontalSpacing(14); + g->setVerticalSpacing(3); + g->setColumnStretch(0, 0); + g->setColumnStretch(1, 1); + return g; + }; - layout->addSpacing(8); - layout->addWidget(t.referenceHeader); - layout->addLayout(refGrid); + // θ (theta), ω (omega), τ (tau); subscripts for ref/traj; Δ for error. + auto* stateGrid = makeGrid({ + { symLabel("\u03B8"), t.q }, // θ position + { symLabel("\u03C9"), t.qd }, // ω velocity + { symLabel("\u03C4"), t.tau }, // τ torque + }); - layout->addSpacing(8); - layout->addWidget(t.trajectoryHeader); - layout->addLayout(trajGrid); + auto* refGrid = makeGrid({ + { symLabel("\u03B8ref"), t.qRef }, + { symLabel("\u03C9ref"), t.qdRef }, + { symLabel("\u03B1ref"), t.qddRef }, // α target accel + { symLabel("\u0394\u03B8"), t.err }, // Δθ error + }); - layout->addSpacing(8); - layout->addWidget(t.clampedHeader); - layout->addLayout(limitGrid); + auto* trajGrid = makeGrid({ + { symLabel("\u03B8traj"), t.qTraj }, + { symLabel("\u03C9traj"), t.qdTraj }, + { symLabel("\u03B1traj"), t.qddTraj }, + }); - layout->addSpacing(8); - layout->addWidget(t.constantsHeader); - layout->addLayout(physicalGrid); + auto* limitGrid = makeGrid({ + { symLabel("\u03B8 clamp"), t.qClamped }, + { symLabel("\u03C9 clamp"), t.qdClamped }, + }); + + auto* physicalGrid = makeGrid({ + { symLabel("b damping"), t.damping }, + { symLabel("c friction"), t.friction }, + }); + + auto addSection = [layout](QLabel* header, QGridLayout* grid, int gap) { + layout->addSpacing(gap); + layout->addWidget(header); + layout->addLayout(grid); + }; + + addSection(t.stateHeader, stateGrid, 5); + addSection(t.referenceHeader, refGrid, 10); + addSection(t.trajectoryHeader, trajGrid, 10); + addSection(t.clampedHeader, limitGrid, 10); + addSection(t.constantsHeader, physicalGrid, 10); } void ControlPanelWidget::updateTelemetryDisplay() { diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp index 350fc164..4b85421c 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp @@ -303,8 +303,8 @@ void HomePage::buildDiagnosticsPanel(QHBoxLayout* into) { for (const QString& path : recents) { auto* item = new QListWidgetItem(); item->setData(Qt::UserRole, path); - item->setText(QFileInfo(path).baseName() + "\n" + path); item->setData(Qt::ToolTipRole, path); + item->setText(QFileInfo(path).baseName() + "\n" + path); _recents_list->addItem(item); } if (recents.empty()) { From 5fcb9cfdb062eb6470722e8303aacdb548f03966 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 28 Jul 2026 08:15:45 +0100 Subject: [PATCH 067/114] refactor: Added `RecentItemDelegate` for custom item rendering in MainWindow --- DSFE_App/DSFE_GUI/CMakeLists.txt | 2 + .../MainWindow/style/RecentItemDelegate.h | 19 +++++++++ .../MainWindow/style/RecentItemDelegate.cpp | 39 +++++++++++++++++++ 3 files changed, 60 insertions(+) create mode 100644 DSFE_App/DSFE_GUI/include/MainWindow/style/RecentItemDelegate.h create mode 100644 DSFE_App/DSFE_GUI/src/MainWindow/style/RecentItemDelegate.cpp diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index d23528b8..4f357157 100644 --- a/DSFE_App/DSFE_GUI/CMakeLists.txt +++ b/DSFE_App/DSFE_GUI/CMakeLists.txt @@ -142,6 +142,8 @@ set(WIDGETS_SRC include/MainWindow/DSL/DSLSyntaxHighlighter.h src/MainWindow/Widgets/GravityVectorWidget.cpp include/MainWindow/Widgets/GravityVectorWidget.h + src/MainWindow/style/RecentItemDelegate.cpp + include/MainWindow/style/RecentItemDelegate.h ) set(OBJECT_SRC diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/style/RecentItemDelegate.h b/DSFE_App/DSFE_GUI/include/MainWindow/style/RecentItemDelegate.h new file mode 100644 index 00000000..b8d55ab9 --- /dev/null +++ b/DSFE_App/DSFE_GUI/include/MainWindow/style/RecentItemDelegate.h @@ -0,0 +1,19 @@ + +#pragma once + +#include + +class QPainter; +class QStyleOptionViewItem; +class QModelIndex; +class QSize; + +namespace style { + // A delegate that paints line 1 bold (title) and line 2 dim (path), Consolas. + class RecentItemDelegate : public QStyledItemDelegate { + public: + using QStyledItemDelegate::QStyledItemDelegate; + void paint(QPainter* p, const QStyleOptionViewItem& opt, const QModelIndex& idx) const override; + QSize sizeHint(const QStyleOptionViewItem&, const QModelIndex&) const override; + }; +} // namespace style \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/style/RecentItemDelegate.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/style/RecentItemDelegate.cpp new file mode 100644 index 00000000..7d4a2dea --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/style/RecentItemDelegate.cpp @@ -0,0 +1,39 @@ + +#include "style/RecentItemDelegate.h" + +#include +#include + +namespace style { + // A delegate that paints line 1 bold (title) and line 2 dim (path), Consolas. + void RecentItemDelegate::paint(QPainter* p, const QStyleOptionViewItem& opt, const QModelIndex& idx) const { + // selection/hover background — let the base draw it so your sheet's blue applies + QStyleOptionViewItem o(opt); + initStyleOption(&o, idx); + o.text.clear(); + o.widget->style()->drawControl(QStyle::CE_ItemViewItem, &o, p, o.widget); + + const QString full = idx.data(Qt::DisplayRole).toString(); + const int nl = full.indexOf('\n'); + const QString title = nl >= 0 ? full.left(nl) : full; + const QString sub = nl >= 0 ? full.mid(nl + 1) : QString(); + + QRect r = opt.rect.adjusted(8, 4, -8, -4); + const bool sel = opt.state & QStyle::State_Selected; + + QFont tf("Consolas"); tf.setBold(true); tf.setPointSize(10); + p->setFont(tf); + p->setPen(sel ? QColor(255,255,255) : QColor(230,230,235)); + p->drawText(QRect(r.x(), r.y(), r.width(), r.height()/2), + Qt::AlignLeft | Qt::AlignVCenter, title); + + QFont sf("Consolas"); sf.setPointSize(8); + p->setFont(sf); + p->setPen(sel ? QColor(220,225,235) : QColor(140,140,148)); + p->drawText(QRect(r.x(), r.y() + r.height()/2, r.width(), r.height()/2), + Qt::AlignLeft | Qt::AlignVCenter, + p->fontMetrics().elidedText(sub, Qt::ElideMiddle, r.width())); + } + // sizeHint is the height of two lines of Consolas, plus 8px vertical padding. + QSize RecentItemDelegate::sizeHint(const QStyleOptionViewItem&, const QModelIndex&) const { return QSize(0, 46); } +} // namespace style \ No newline at end of file From daa5ce4c5c02384735754707b41c613b88126962 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Tue, 28 Jul 2026 08:15:52 +0100 Subject: [PATCH 068/114] refactor: Integrated `RecentItemDelegate` for improved item rendering in recents list --- DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp index 4b85421c..22937ef9 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp @@ -15,6 +15,8 @@ #include #include +#include "style/RecentItemDelegate.h" + #include "Platform/SystemInfo.h" #include "Platform/Paths.h" @@ -81,6 +83,7 @@ namespace Workspace { _recents_list = new QListWidget(rail); _recents_list->setFrameShape(QFrame::NoFrame); + _recents_list->setItemDelegate(new style::RecentItemDelegate(_recents_list)); rail_col->addWidget(_recents_list, 1); // Right Column: Branding and primary actions From d04b6f4badec848251e3c9ae75a21760dde51e1b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Wed, 29 Jul 2026 07:36:21 +0100 Subject: [PATCH 069/114] feat: Added cube robot model with visual and inertial properties * Obviously its not a robot, but its for testing single body objects --- .../assets/rigidbody_models/cube/cube.urdf | 17 +++++++++++++++++ .../rigidbody_models/cube/meshes/cube.fbx | Bin 0 -> 26764 bytes 2 files changed, 17 insertions(+) create mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf create mode 100644 DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/meshes/cube.fbx diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf new file mode 100644 index 00000000..9f8f7d4b --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/meshes/cube.fbx b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/meshes/cube.fbx new file mode 100644 index 0000000000000000000000000000000000000000..0d4af35d07bce3314ace3e6008f0d961897f9ead GIT binary patch literal 26764 zcmc&-dz2hinXkNOl8_fc9tIgOf$&I}08tWwuFiBOnPet2rY8fzS2{B_GYvi6O+Ug6 zBJvOcmm_-AJqU}7z#dV2;3A?6gg|f;TohLD#fk?7mWQZ8fsIIDf4_Ubs=n3TRn?jN zVI3+}UH5*!`@QaWzkBP}us$_5kSe5F+E<;^vMQZT6~MMZW+O^H_B$M&h{6^@+Hvq zj^&*W6j_%MT8_DG1t+CS(<4r@lp4uDh7z~!X&x+^eBLOSyxB<=I$nYlZy8Ugbmg+8 z;f@)2G#3yq%A`bUtm9pHc%1QY^4eUv&@mfNW5&~|UFmGOrmi3{!?l$!_#R>&|D#T5`L`8I9cesWpS~T+M02cg*v*AZ&}iE z+S27qPiyJyP2zs}(q*SEZT%92oCuoZa5g9VovhQ59n2*U!Ou0B+}2cvKdlLsk|vaF zDmsNks+2k!l;r9loRevQq*=OBN@-?K)dTo39qM+ErY@(HBGzzF?N1s7Q+m^-jFTks z^~U3A$@13ooc@w3xYH<@8ZVcIp%oJR*m&Nw-Wj_vR~RU&lIP5ljn1|-{ZN(cGfJkl z%LV5_jH=|4k z5%alnc0jX%Qp-kliqMG@Z6AS)OL1{LpUoMz~1uCLTBeH{d?81MY zUPPFWt?BAjdUPLD^q3^Gt;Wk1bwwe z$H)-GxEfa)vGK|9GicD-pdlI5E4pxPI!+^hT(Hm5*zdu`^B;rRv-EhAbV?-z#Nw#n(;nXF@gm~a-3E;>+U^DgYafpj z&JXvnHs#}^>7oj*X+&4`sti&lFM)y3*a+ z3rG^e8^xGneOxG8H6;ho3T0%wch|Ptmcwus-Y(j({#x5^g)*|;Q=lBGiZ+M59Ee;W zj?(lyILbo4T$6KHs3MQ#;9h|^T|+Rz+LTR~lKrWS)1K-tVGN?(2XbX3KI(Qud1R}Y zn}Y$4J+WqlP0C^8gdFs8kCFXH`xwyZS!*>K-7`M2HH|^OEtdg9a7#Q`5<&ny0rIOrZAPLz$Rg;hdk|gt~EK=!0Ny)NaJtBg-!BwRSyjoiY z;1t|L1S+Nikjn?s-}%cIcd#4YF7$@6+segKZlt{c6>P+y8*S{{z7sW9;ZDlA|^5LaWU39ncuYI2cvp%CPZldIaX^KR;v?MP>i-6woLmqJh=4F_$ZC zfFE@F>(u*o7`wb2(>FSUPQl6cJ4N@Ay3Srx`8fkFn?{d9K3(hpt*cirpbwz#7NbBE zPWSw_Jf^7AwPw^5gDm)mim%oJup!@Z>+!mE1|1WPHlpzH67uk^n4JBo7K{P=pM{>S z&ygH|tk)UIXXu91WBb-o-7~L2Rrm1O{W63r28uXN*q!E#ju1C|na4}9M9s}l1Wqyv z>Z~VMRLWNUq7OY^<~~X4Bc8xN>Zrcy*l{@W0G z9CQm#u{2KpEbFU4p}wTcOvdfTTqdJJm%70Nt4hk3gk~j*zy~eYjimw%6>W&kPT`z- zkLutoLqg8BgK#$2;%ruZwu+xCfI}TqQQ(JT*DCbep#rBw+7T))fkIcF<3D`eL$5D> zE|#dd*)E(d(TMcI-f|(^%UOT*;8(@WJ|SJENjYpJ(nWaln1|4TH3_BQt+|{`Oi|0~ z;6KaJkI3u^I3Wvr5f%lya_s4=GUY-LU0LX4NML3Px{XdM!;vAJo<4k^(94XP^qQ!J zt+vq5u*6gv3zdFP>q=$w}(GY3-A%PGru!r@eN1cozLp02u*gFDqazz%*5=OcQ2a8VW zoKO{7)){aaDxxm&<_O}8g;?A`g-^5`sl%6!UDou?1uw_$7y45*=S&vJd9(&ketO~5 z%f9q%Y!W<&+#9I{0UA``CUv|XTPG-n9f*>=pzJsoF+oY^8nMHt1jg`2!nEfKBdJmc zmhwjHrD)*Z+U}=RLqNe%*>I=@_Qc*6O0fWgBirmPq_Ra0X;J1s3zK6iFo-_N11_mg z&sg4*-j<7WzwHt9QSI*&dWnAwV3eQj_oXuBsPMTQa|ViU#6Oq3yMVRVAcJLujwu2H zkLvtfE-V)UQ*mY=UlFo}nvA2!8W^b^Gan9xRmnS^SkDNB5vSVIPG+EcurKXgs8$%w z-z2Gw5I5(KTwyQ7iX26JA5LM_pg2eI?t9?Jz^{Zo9E!2I8l%pznXvy&Fw8)c!yw-2 zWQQc;8go1m;@Hnmex|Pww|^tVhQ4)@raM=V_U2=)s>-|=rqr}U57g9pondhpbTp2n zJ}NG8t`eQ*ovMx}Vu_lY8-!^b?E3qD`dK>N`8~KZ~e#M|V zGd?w;;;oS;CrHjiZj1!^B7joI#^|>JFPwTZscZu?+J*KHgUOzd8 zQpi+-NF#a7f7&wXd=D5Y2B~B@mI=wd^v8L)Wk}K8syEk}%bgz&G~Idf_@?~&B$hZohhNfLLCxFNMIJ;c;0aPsl3+BPu~c8sKk^jcP(Hh&`|M{73e zTWxqz7v$py9i`RsUR`}EtE)LV;+mTg{7wFmaq8&nfBJSZuDv{?q2!sy=^AI2ZyAT>l zLZ#JkZ(nhY)>W%+6Vpg9YLuKvzrS3lKcJ~q+{zUky$;gSolo_<6ZnW_{Cfq+5S=cs zqzLu-w}sH~H&^tm)0or85gO<&ArRpXV_8=Ugp2|H_}!q}?-C57_f{ln$nTpMYC(b# z!sXZ#YXd@+<0HFv?HT6ypOChViLWd|B~yb=9D8@_MJ;IEdce;$V`mj*cXp%N1rvnu zwjf9nH6I9Wqb^cu49jSgtZIJ6ZnqW#9_@byPINnV0hfF~7ZI)$B%}9Zu5n7DD>al& z52l^Mxw+g(kQBRx*l@#B5-lESi=qvSoSW??j9HZA6fFq@hz}w>aCX>hBk053r^CKV z=nX^YToSA7+|+|aK&|`NF$7q(cKMrUR^MH~rXNr8h#)crQxA%7#4=+=GSS^0&kn@n zLHdsgwc){_PB{6}aQC2^9S2dCg>o_4h68aSu^oGZiuz4ZJ-0&*>-$1wE_{(Ho*j}U z-%xR9+HVSg1Rb^RK>%OA7>bd$PXo_XnP2F5wv6cf$^oeI*jNLC-M!zw!u&-=5r{}Cy-It8)6Jl|CG6K8B`>{EzjDeG@*yTaI z-U_mwjkzaPApK6qZthgV7Q+k_pxzq>ZyuNi(u)Pcm^P~g1{Wg8=^1?*JYK@c$m+DR z`O>Qizt=dq5vNob`z_dD9ZF-o?);6A&cx%|_g&;4@4LE?o<7FWR}SCXt{gH^){EnW zt%+ZmHLJTi@55_ea(S=49u&BQh9mN-Rk=~@s8mZLjZVY8`M6~r49*EtjhGr8FJcc6 zDW~C*J%#iLx5oBiQ{mQhhM5fmS=AJU2Dn;?jRd|8d$)=$$>G#M?!vnBand~`bVhI7 z==3{y=O0B0;zo{ZW7Oy??T0lkj@VU^f0A?#j zFa2N{X>B2k4yxvh`J6Ymsng5f0zECf9w%IoLz|md=`3%&hVDhuOyG4InElHIR;$HP`y+#Mv=^}>>28-w-$ zy7jB!z~k@TC!$+ZS~wNGX*Mo?j?W2_)rq3q97OrAQIzKfQT{rdGBp~=?ZHTeLgY-1 z26EdKMH$HLl|pF*Ket1LzE62X=!|tVmk#@s`W;L}I@2TRQg=2}H}JxS);wgMl-HP# zdKjhj;2_pQi#iDo$Vdpn<3kMq>1;Rx=(Yxc&W&CavaH_=5VS;>WeDINLA*MNre0{D zj<`#bwVHYcE3EODkeDtd8~Jf#^NRrK_ZLc{zY6 zR0=^Ie+aq*s1y=88--*XH+T)~(Z3HR3+&E^V@)8qwZOjI6G9decCHE~3uJP8C|Mwr zuL&9E0n{XYyCXa76w;-jyc;#FlaTLIBSx5#aWve=E(0BXYzcD6N?Uzs|A}6HJn!54B zG$d3Z5~d-c?hzRDe_Co5F388Y341`T|21T%w<16q5-5&kZ|Wt8lZM2(5FlI&k)ej- z=*5|(A~!D~66&*4*ufHj@-{3wS2MLy#2_M7YdeOxa|*ZV?=-_6`8D0tUH zUZ&#GUf>SHdd;HHdbF0Hvpa#+vLJ%RSdit2B|{3Qww?W%7IUzw_l#{-c8&3uyW7RM zxkRwnq7>XSXC<~bQq?>lOeE^h1hrkGGJhkgJ4RLiuEDG~Le0ZE6unxOo0z2;%XU>i z>mWKpzZzwb^j$uB>`hkbrP^E)QlxPCkNWC_Hw_#Ukyz4eIjEXTbWQl(#RW60%B|2c^0xT`b~4Udtw8iCqKBjI`Dl9R+k;U=>3TMiHC7>tFR*~rFa6@5I29RRe;H-kY>oYa&`5Cit3P5(ugg)g(Jx+ z=b>5!vVW`rwqT|ET@l4L^;PqNztQ~Za+ z%Bu3nxZ$1`*)ad7JF-s^&d^I}!o4SEgEPw5r2#nC&~L~49sU)A=odO5P=_a>cXJKy zXl~>qOzTY?jYpdTlP@zoyJJyo(1M0%RNMo6&|K~lPh zN4k16pUW~&nu@2v5MKrawL!lC1P2^^SeVf-{zLSEdg}1}$-DUEcHvUzb$zZ7J_25K2nuH~2@@hm! zMWerNo{EyN=y>(5_6h-5jH-~ENhfs`@xt~hP*GcUg-TOI>a5;~@u2WP9|I3fxUlCH z-OYvpDnqq5g;EAS#tm+{(w0_}n>ueYB}h*sDFB z8rLOCbNfon< z&HTo6r|#W3QXH<-1N#%L=80Ir~+Cka4mZ&-u+(Bnx-^{Jgu*{ni_?Q;-Bz zg$vS0GjQFmXI~vX<4>_s0gyEXHMatQp+n_&VL`{*8@pFfL=)5{*)X@Q)*{e+mG8gb z4uc3<0P;gD-=LLoJb>Yx${rCPvQ0wOAt76+$=H=cK4fE%{<=^Yu48<47WN_AujVa_ zUGZp}{YOENNdj3l5W+uXb1{~6YjNvign!6(+Y`m@kMCP)mjuH!T-Aqc;fCw5w+jT; zvZ(cgaoNvZrwevnfdAf#P3-XrbP$!YU;6;<>+Ds6VRWcQw_dh989&m#yq3Pv@1W2R z7upXBy@WFk2Hw|0k)!ucdpZUg_Gg)Z%*Czu$pFG)A)QC-v2C+dVAFNexoc2?eT8d|P44K%LBOg%j@@O{pSUTnYStg6erMF2s@Gao z?KyvaUXN)PcjdS9$&RBsUWiS?U`|QM=p3$-#JTdT_RCa*S0z2oW4x{XD6(&Q$_ogeLclHP2o_3^gyR>zv) zmZ?OLn-+hMw%o&)U$_s|brNQjt^(>>YJ*f@RA@p*jz2Ep6bCpzBpE~F=!E_))@8Hb z*z%dy`;Y26a^+EjvpN^t^yx=-4qt!9%A<~*b?XH`o@D*`TgRlI5~vXn&Fjf!#)fiP zB08gLfjCW29$VDEdf}Wtz7Ole)H6dRfA7U_MpfQV#`z|G(|}}%^;ph(9;A6o%2}7k z+xUHauda_(WNXUmfgz`uX3*r(sxJzT`C z)dRg+qTFRrnk1{3rP33(GBeEHctN`N6!qU!241sxhA` zUly`?C_2ph^EK6kmAflNKM8oc-+1CwK16J^tj**B-E+z^~VK?rq)I zde)=cXZP%Def_9cCS37_BWHX_G(@9KgI4$#7%tXwZV|ZD?P{UDP)K?q#bA* z>WcL^O7Wf;E-mY@E}yZ?cbt7r#a{DGd$|ky%4%OTtm?Dc{YF8h1HB?o$!$;vVtM_Fmy(KMl1`$mRm6*=4twrfPUlEj7pE2e($6hoV&faawY3*3kX&IRkk*2kZf5aMh{{tgdH+GxzYdv{YDTI;LBig*&sQO?~SO zu|iwf8Pv)Rf^fLk3VWN`imT!8H`*5H1CDz>qCN6hluuEwZ#tXr>^Ysa)y@rf0!a>w!Nv3_RsH^E-3vFYp$zj{osc>N=f zKm0yBaru%pBNy&W*@>qva6WnCNpZXDLz?4ujmjXfU#}iFt0jmQ%hET`yz|aE_7{Lj zkEe!_*dKz+e!eIMh4qM_EY>LGEMij|d)mYg>$XqSRP^1APBE7$b0`g()uWPS3n7yN zFsllkunwIM413W59~fuM!TKh}y`SyBlTbqU;;_Bln1 zO8Zp4ajd*@3fu^Q`jw*(U~Y(;UU|oU_B48y635zgCN3z_q;YiG2F3% zp63dvt{FfCdBs&mS(R7ZBt?F&*db3F^@{jb?G+2big?AnH($Yg#?m`B^cxE87F3s7 zC0?+l!m`gV9!`-`A3neM{1)vO2NdmF-X0rMG1NM=c)7aS@xn2icf26jCB^ldaj*V@b7p3b;rev><>T`dh)n@uEpnS;>Vlq%Yb>{K6j6x9DvU~ z1BeL_j?7d?qQ4tuRX#Tt-a<0J&z&Yu8}+$91SV#^i*Po%xl8Bmx9sFVWT_Xz?o(@m zdxvSVWqnavBHNzdG0fc~1&n_)eVb2MRc$kQ!l11dxq}#T_}umb{n`oQD3sOC=#3Pp zd$an>oi#WAByg)V9J`sj%OrAL-&-*;ZvG2+6i`Mdj^Hj{1MfRAhaIR9FM8Js8nLr8 zzoFiN_-{4?j_fSM32jll#&@k))(t{_swSr`_h-|BWxXuo80vLjh=;2;z+5LdHR^To z8c^@!F-Pc^Xn~q>@!pcFTqo!q-3t45AziLXIg#>68+yUM!hRDF3}6Wj!QNSU9X>97 zoPlmK1u&;+M6U5v5F5}4>Nj?Ld8krvMq4hM)w=?Uw@BpRRA@q5aw0C>-QEPH(ul2L)R&y66JKffuy4BoW0iAB^*Ve7(#z+s% zl~`T>8#L}Qh1p~AVLWleJaWVW_gkqz>Q<}0zIopr&5!SXa`3y`{(i-R|2y`Q+8X?S c@%*EXz2{zg-fv(3^(C*oH1pZ=b;r;6f6z+f-~a#s literal 0 HcmV?d00001 From b252e9f2717ca2626bfd90f17d226ea0c3fad4c6 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Wed, 29 Jul 2026 07:36:26 +0100 Subject: [PATCH 070/114] refactor: Corrected enum values and added missing mappings for robotic systems --- DSFE_App/DSFE_GUI/include/Platform/SystemMap.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h b/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h index b21bd609..1eef4a3f 100644 --- a/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h +++ b/DSFE_App/DSFE_GUI/include/Platform/SystemMap.h @@ -11,7 +11,8 @@ namespace platform { Panda = 2, iiwa14 = 3, VISPA = 4, - H1 = 5 + H1 = 5, + Cube = 6 }; enum class eRoboticSystemFamilies { @@ -20,7 +21,7 @@ namespace platform { Franka = 2, KUKA = 3, Airbus = 4, - Othjer = 5 + Other = 5 }; struct RoboticSystems { @@ -30,7 +31,8 @@ namespace platform { { eRoboticSystems::Panda, eRoboticSystemFamilies::Franka }, { eRoboticSystems::iiwa14, eRoboticSystemFamilies::KUKA }, { eRoboticSystems::VISPA, eRoboticSystemFamilies::Airbus }, - { eRoboticSystems::H1, eRoboticSystemFamilies::Unitree } + { eRoboticSystems::H1, eRoboticSystemFamilies::Unitree }, + { eRoboticSystems::Cube, eRoboticSystemFamilies::Other } }; inline std::string toString(eRoboticSystems sys) { @@ -41,6 +43,7 @@ namespace platform { case eRoboticSystems::iiwa14: return "iiwa14"; case eRoboticSystems::VISPA: return "VISPA"; case eRoboticSystems::H1: return "H1"; + case eRoboticSystems::Cube: return "Cube"; default: return "Unknown"; } } @@ -52,6 +55,7 @@ namespace platform { case eRoboticSystemFamilies::Franka: return "Franka Robotics"; case eRoboticSystemFamilies::KUKA: return "KUKA"; case eRoboticSystemFamilies::Airbus: return "Airbus"; + case eRoboticSystemFamilies::Other: return "Other"; default: return "Unknown"; } } From 0be6bc4c6acf49daa9565f8d770aba3f4ab14141 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 07:44:41 +0100 Subject: [PATCH 071/114] refactor: Added free-joint states to `RigidBodyJoint` struct for enhanced kinematic modeling --- DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h index be4b6bda..7165875b 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h @@ -141,6 +141,13 @@ namespace systems { mathlib::Vec3 origin_rpy{ 0.0, 0.0, 0.0 }; // roll, pitch, yaw in radians mathlib::Quat origin_q{ 1,0,0,0 }; // Rotation matrix from link frame to base frame, derived from rpy_deg in JSON + // Free-Joint States + mathlib::Vec3 free_pos{ 0.0, 0.0, 0.0 }; // Position of the free joint in world frame + mathlib::Vec3 free_rot_v{ 0.0, 0.0, 0.0 }; // Rotation of the free joint in world frame (Euler angles) + mathlib::Quat free_qref{ 1,0,0,0 }; // Rotation of the free joint in world frame (Quaternion) + mathlib::VecX free_vel = mathlib::VecX::Zero(6); // Velocity of the free joint in world frame (linear + angular) + + // Axis expressed IN JOINT FRAME mathlib::Vec3 axis{ 0.0, 0.0, 1.0 }; From bb1dcf7bc15f49a9a8ee0abfe715a8e05e400776 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 07:44:50 +0100 Subject: [PATCH 072/114] refactor: Added joint degree of freedom calculations and quaternion conversion for enhanced rigid body dynamics --- .../include/Systems/RigidBodySystem.h | 16 +++ .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 112 +++++++++++------- 2 files changed, 86 insertions(+), 42 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 4e4a44fb..b73cc395 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -49,6 +49,22 @@ namespace systems { inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) + inline int jointDOF(eJointType t) { + switch(t) { + case eJointType::FREE: return 6; + case eJointType::REVOLUTE: return 1; + case eJointType::PRISMATIC: return 1; + case eJointType::FIXED: return 0; + default: return 1; + } + } + + inline mathlib::Quat expToQuat(const mathlib::Vec3& rv) { + const double theta = rv.norm(); + if (theta < 1e-9) { return mathlib::Quat(1, 0, 0, 0); } + return mathlib::Quat(Eigen::AngleAxisd(theta, rv / theta)); + } + class DSFE_API RigidBodySystem { public: RigidBodySystem(); diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index eac43876..58759efc 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -157,65 +157,86 @@ namespace systems { // Method to pack rigidBody joint states into a state vector mathlib::VecX RigidBodySystem::packState() const { - const size_t n = static_cast(_body.joints.size()); - mathlib::VecX x(2 * n); - + int nv = 0; + // Count total DOF + for (const auto& j : _body.joints) { nv += jointDOF(j.type); } + mathlib::VecX x(2 * nv); + int off = 0; // Pack angles and velocities - for (size_t i = 0; i < n; ++i) { - auto& j = _body.joints[i]; - - // Current states - x[i] = j.q; - x[i + n] = j.qd; + for (const auto& j : _body.joints) { + const int dof = jointDOF(j.type); + if (dof == 0) { continue; } // Skip fixed joints + if (dof == 1) { // Revolute or Prismatic joint + x[off] = j.q; + x[off + nv] = j.qd; + } + else { + x.segment(off, 3) = j.free_pos; // Position + x.segment(off + 3, 3) = j.free_rot_v; // Euler angles + x.segment(nv + off, 6) = j.free_vel; // linear + angular velocity + } + off += dof; // increment offset by the DOF of the joint } - return x; // state vector + return x; } // Method to unpack state vector into rigidBody joints void RigidBodySystem::unpackState(const mathlib::VecX& x) { const size_t n = static_cast(_body.joints.size()); + int nv = 0; + for (const auto& j : _body.joints) { nv += jointDOF(j.type); } // Resize clamping vectors if necessary if (_clampTheta.size() != n) { _clampTheta.assign(n, 0); } if (_clampOmega.size() != n) { _clampOmega.assign(n, 0); } - // For each joint + int off = 0; for (size_t i = 0; i < n; ++i) { auto& j = _body.joints[i]; + const int dof = jointDOF(j.type); + if (dof == 0) { _clampTheta[i] = 0; _clampOmega[i] = 0; continue; } // Skip fixed joints + + if (dof == 1) { // Revolute or Prismatic joint + // Current states + double theta_in = x[off]; // [rad] + double omega_in = x[off + nv]; // [rad/s] + double theta_out = clampJointAngle(j, theta_in); // [rad], clamped to joint limits + double wMax_hw = std::abs(j.limits.maxqd); // [rad/s], max |omega| for this joint + double omega_out = omega_in; // [rad/s], will be clamped if necessary + + // Velocity limit clamping + if (wMax_hw > 0.0f) { + const double eps = 0.05f; + if (std::abs(omega_in) > (1.0f + eps) * wMax_hw) { + omega_out = std::clamp(omega_in, -wMax_hw, wMax_hw); + } + } - // Current states - double theta_in = x[i]; // [rad] - double omega_in = x[i + n]; // [rad/s] - - // Clamp joint angle - double theta_out = clampJointAngle(j, theta_in); - - // max |omega| - double wMax_hw = std::abs(j.limits.maxqd); - double omega_out = omega_in; - - // Velocity limit clamping - if (wMax_hw > 0.0f) { - const double eps = 0.05f; - if (std::abs(omega_in) > (1.0f + eps) * wMax_hw) { - omega_out = std::clamp(omega_in, -wMax_hw, wMax_hw); + // Velocity limit enforcement + if (theta_out != theta_in) { + const double upperLimit = j.limits.maxAngle; + const double lowerLimit = j.limits.minAngle; + if (theta_out >= upperLimit && omega_in > 0.0f) { omega_out = 0.0f; } + if (theta_out <= lowerLimit && omega_in < 0.0f) { omega_out = 0.0f; } } - } - // Velocity limit enforcement - if (theta_out != theta_in) { - const double upperLimit = j.limits.maxAngle; - const double lowerLimit = j.limits.minAngle; - if (theta_out >= upperLimit && omega_in > 0.0f) { omega_out = 0.0f; } - if (theta_out <= lowerLimit && omega_in < 0.0f) { omega_out = 0.0f; } + // Record clamping + _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; + _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; + // Update joint states + j.q = theta_out; + j.qd = omega_out; } - - // Record clamping - _clampTheta[i] = (theta_in != theta_out) ? 1 : 0; - _clampOmega[i] = (omega_in != omega_out) ? 1 : 0; - // Update joint states - j.q = theta_out; - j.qd = omega_out; + else { + j.free_pos = x.segment(off, 3); // Position + mathlib::Vec3 rot_v = x.segment(off + 3, 3); // Euler angles + j.free_vel = x.segment(nv + off, 6); // linear + angular velocity + j.free_qref = (j.free_qref * expToQuat(rot_v)).normalized(); // Update quaternion based on Euler angles + j.free_rot_v = mathlib::Vec3::Zero(); // Reset Euler angles to zero after conversion + _clampTheta[i] = 0; + _clampOmega[i] = 0; // No clamping for free joints + } + off += dof; // increment offset by the DOF of the joint } } @@ -336,7 +357,14 @@ namespace systems { void RigidBodySystem::step(double dt, double simTime) { if (!_hasBody) { return; } - if (_useAutoDiff) { + bool hasFree = false; + for (const auto& j : _body.joints) { + if (j.type == eJointType::FREE) { + hasFree = true; break; + } + } + + if (_useAutoDiff && !hasFree) { step_AD(dt, simTime); return; } From 569b634b8ada30084e926ca606218eee106ab377 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 08:19:00 +0100 Subject: [PATCH 073/114] refactor: Added joint state offset and total degrees of freedom methods for improved rigid body system functionality --- .../include/Systems/RigidBodySystem.h | 6 ++-- .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 29 +++++++++++++++---- 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index b73cc395..8c105aab 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -90,10 +90,12 @@ namespace systems { std::size_t linkCount() const { return _body.links.size(); } std::size_t jointCount() const { return _body.joints.size(); } - std::string findRootLink() const; - bool hasLinkName(const std::string& linkName) const { return _link_idx.find(linkName) != _link_idx.end(); } + + int jointStateOffset(size_t joint_idx) const; + int totalDOF() const; + const std::string& rigidBodyName() const { return _body.name; } bool hasRigidBody() const { return _hasBody; } diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 58759efc..bba55ef6 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -23,6 +23,13 @@ using namespace constants; using namespace physics; namespace systems { + // Helper function to convert std::vector to Eigen::VectorXd + static VecX toVecX(const std::vector& a) { + VecX v(a.size()); + for (size_t i = 0; i < a.size(); ++i) { v(i) = a[i]; } + return v; + } + // Constructor RigidBodySystem::RigidBodySystem() : _integrator(std::make_unique()), _curIntMethod(integration::eIntegrationMethod::RK4), @@ -41,13 +48,23 @@ namespace systems { return names; } - // Helper function to convert std::vector to Eigen::VectorXd - static VecX toVecX(const std::vector& a) { - VecX v(a.size()); - for (size_t i = 0; i < a.size(); ++i) { v(i) = a[i]; } - return v; + /* + * Method to compute the offset of a joint's state in the packed state vector based on its index + */ + int RigidBodySystem::jointStateOffset(size_t joint_idx) const { + int off = 0; + for (size_t i = 0; i < joint_idx; ++i) { off += jointDOF(_body.joints[i].type); } + return off; } - + /* + * Method to compute the total degrees of freedom (DOF) of the rigidBody system based on its joints + */ + int RigidBodySystem::totalDOF() const { + int nv = 0; + for (const auto& j : _body.joints) { nv += jointDOF(j.type); } + return nv; + } + // --- HELPER METHODS --- // Method to clamp a joint angle to its limits From 594385c5dfe0ac100e67cebf978efad9e9a9602d Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 08:42:15 +0100 Subject: [PATCH 074/114] refactor: Moved joint degrees of freedom function to `RigidBodyModel.h` for better organisation --- .../DSFE_Core/include/Systems/RigidBodyModel.h | 14 ++++++++++++++ .../DSFE_Core/include/Systems/RigidBodySystem.h | 10 ---------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h index 7165875b..8543ba67 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h @@ -39,6 +39,20 @@ namespace systems { CONTROLLED // Full physics simulation with active control (e.g., for testing control algorithms, trajectory tracking, or simulating real-world behavior) }; + /* + * Helper function to get the degrees of freedom (DOF) for a given joint type. + * @param t The joint type (eJointType). + */ + inline int jointDOF(eJointType t) { + switch(t) { + case eJointType::FREE: return 6; + case eJointType::REVOLUTE: return 1; + case eJointType::PRISMATIC: return 1; + case eJointType::FIXED: return 0; + default: return 1; + } + } + // --- RigidBody Model Links --- // Inertia tensor struct, representing the inertia of a link about its center of mass, expressed in the link's local frame diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 8c105aab..bbe0484a 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -49,16 +49,6 @@ namespace systems { inline constexpr size_t AD_VARS = 14; // number of independent variables for autodiff (used for pre-allocating AD integrator buffers) - inline int jointDOF(eJointType t) { - switch(t) { - case eJointType::FREE: return 6; - case eJointType::REVOLUTE: return 1; - case eJointType::PRISMATIC: return 1; - case eJointType::FIXED: return 0; - default: return 1; - } - } - inline mathlib::Quat expToQuat(const mathlib::Vec3& rv) { const double theta = rv.norm(); if (theta < 1e-9) { return mathlib::Quat(1, 0, 0, 0); } From 5749070686c1b0e9ccb0a96918cbebb8bf2f26b4 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 08:42:25 +0100 Subject: [PATCH 075/114] refactor: Updated state vector handling for joint dynamics to accommodate variable degrees of freedom --- .../include/Physics/RigidBodyDynamics.inl | 57 ++++++++----------- 1 file changed, 24 insertions(+), 33 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index 80d6a292..d13dfa79 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -397,22 +397,21 @@ namespace physics { DynamicsScratch& scratch, DynamicsResult& out ) { - const size_t n = model.joints.size(); - mathlib::VecX_T dx(2 * n); + const size_t n = model.joints.size(); + int nv = 0; // number of degrees of freedom (DOF) in the model + for (const auto& j : model.joints) { nv += robots::jointDOF(j.type); } + mathlib::VecX_T dx(2 * nv); - Eigen::Map> q(x.data(), n); - Eigen::Map> qd(x.data() + n, n); + Eigen::Map> q(x.data(), nv); + Eigen::Map> qd(x.data() + nv, nv); // Quick guard: check for non-finite states and bail with zero derivative - for (size_t i = 0; i < n; ++i) { + for (size_t i = 0; i < nv; ++i) { double q_r = mathlib::real(q[i]); double qd_r = mathlib::real(qd[i]); if (!std::isfinite(q_r) || !std::isfinite(qd_r)) { LOG_ERROR("Non-finite state detected in derivative_spatial: q[%zu]=%g qd[%zu]=%g", i, q_r, i, qd_r); - // Return zero derivative to avoid propagating NaNs - dx.setZero(); - out.qdd.setZero(); - return dx; + dx.setZero(); out.qdd.setZero(); return dx; } } @@ -424,59 +423,51 @@ namespace physics { ); mathlib::MatX_T M = SpatialDynamics::CRBA(model, scratch.spatial.Xup, scratch); - mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(n); - mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(n); + mathlib::VecX_T qd_zero = mathlib::VecX_T::Zero(nv); + mathlib::VecX_T qdd_zero = mathlib::VecX_T::Zero(nv); mathlib::VecX_T tau_g = SpatialDynamics::RNEA(model, q, qd_zero, qdd_zero, scratch); - scratch.dense.tau.setZero(); + scratch.dense.tau.setZero(nv); + int off = 0; // offset for indexing into the state vector for joints with multiple DOF for (size_t i = 0; i < n; ++i) { const systems::SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { - scratch.dense.tau[i] = Scalar(0); - continue; + const int dof = robots::jointDOF(joint.type); // number of degrees of freedom for this joint + if (dof == 0) { continue; } // skip fixed joints + if (dof !- 1 || !isControlledJoint(joint.type)) { + off += dof; continue; // free (6-DOF) or uncontrolled means no control torque is applied, so skip to next joint } const Scalar wn = static_cast(snap.model->joints[i].wn_target); const Scalar z = static_cast(snap.model->joints[i].zeta_target); - - const Scalar err = snap.q_ref[i] - q[i]; - const Scalar err_d = snap.qd_ref[i] - qd[i]; - + const Scalar err = snap.q_ref[i] - q[off]; + const Scalar err_d = snap.qd_ref[i] - qd[off]; const Scalar eps = static_cast(1e-6); - - const Scalar I_eff = mathlib::LSE_smoothMax(M(i, i), eps); + const Scalar I_eff = mathlib::LSE_smoothMax(M(off, off), eps); const Scalar k_p = I_eff * wn * wn; const Scalar k_d = Scalar(2) * z * I_eff * wn; - const Scalar b = static_cast(snap.model->joints[i].dynamics.damping); // viscous damping coefficient const Scalar c = static_cast(snap.model->joints[i].dynamics.friction); // Coulomb friction coefficient const Scalar eps_f = static_cast(1e-3); Scalar tau_i = k_p * err + k_d * err_d + I_eff * snap.qdd_ref[i]; - Scalar tau_f = c * mathlib::tanh(qd[i] / Scalar(0.1)) + b * qd[i]; // simple friction model with viscous and Coulomb friction - tau_i += tau_f; - + tau_i += c * mathlib::tanh(qd[off] / Scalar(0.1)) + b * qd[off]; // add friction compensation (smooth tanh for Coulomb friction) const Scalar Q_max = static_cast(snap.model->joints[i].limits.maxEffort); - LOG_INFO_ONCE("Max effort for joint %zu: %g Nm", i, mathlib::real(Q_max)); - if (Q_max > Scalar(1e-9)) { tau_i = Q_max * mathlib::tanh(tau_i / Q_max); } // saturate control torque to max effort using smooth tanh saturation scratch.dense.tau[i] = tau_i; - out.metrics.q[i] = mathlib::real(q[i]); - out.metrics.qd[i] = mathlib::real(qd[i]); - + out.metrics.q[i] = mathlib::real(q[off]); + out.metrics.qd[i] = mathlib::real(qd[off]); out.metrics.err[i] = mathlib::real(err); out.metrics.errd[i] = mathlib::real(err_d); - out.metrics.I_eff[i] = mathlib::real(I_eff); out.metrics.tau[i] = mathlib::real(tau_i); } out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); out.metrics.qdd = out.qdd; - dx.head(n) = qd; - dx.tail(n) = out.qdd; + dx.head(nv) = qd; + dx.tail(nv) = out.qdd; return dx; } From acefc77da2ecbed0c67a2218edb5602c204a539e Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 08:42:35 +0100 Subject: [PATCH 076/114] refactor: Updated snapshot handling and joint state updates for variable degrees of freedom --- .../include/Systems/RigidBodySystemStep.inl | 40 ++++++++++++------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index 73a2d8e8..5d880e71 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -27,13 +27,20 @@ namespace systems { for (size_t i = 0; i < n; ++i) { const auto& j = _body.joints[i]; - - snap.q[i] = j.q; - snap.qd[i] = j.qd; - - snap.q_ref[i] = j.q_ref; - snap.qd_ref[i] = j.qd_ref; - snap.qdd_ref[i] = j.qdd_ref; + if (jointDOF(j.type) == 1) { + snap.q[i] = j.q; + snap.qd[i] = j.qd; + snap.q_ref[i] = j.q_ref; + snap.qd_ref[i] = j.qd_ref; + snap.qdd_ref[i] = j.qdd_ref; + } else { + snap.q[i] = T(0); + snap.qd[i] = T(0); + snap.q_ref[i] = T(0); + snap.qd_ref[i] = T(0); + snap.qdd_ref[i] = T(0); + } + } snap.root_pose = _root_pose.template cast(); @@ -155,9 +162,10 @@ namespace systems { template void RigidBodySystem::postStepUpdate(const mathlib::VecX& x, const physics::DynamicsScratch& dynScratch, const RigidBodyStepResult_T& result) { const size_t n = result.snap.model->joints.size(); - - Eigen::Map q_next(x.data(), n); - Eigen::Map qd_next(x.data() + n, n); + int nv = 0; + for (const auto& j : _body.joints) { nv += jointDOF(j.type); } + Eigen::Map q_next(x.data(), nv); + Eigen::Map qd_next(x.data() + nv, nv); // Enforce joint limits /*for (auto& j : _body.joints) { enforceJointLimits(j); }*/ @@ -202,18 +210,20 @@ namespace systems { if (buf) { auto dynResult = result.dynamics; + int off = 0; for (size_t i = 0; i < n; ++i) { const RigidBodyJoint& j = _body.joints[i]; - + const int dof = jointDOF(j.type); + if (dof == 0) { continue; } // skip fixed joints const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); - const double err = q_ref_real[i] - q_real[i]; - const double err_d = qd_ref_real[i] - qd_real[i]; - JointLogBuffer::JointLogEntry e{}; + const double err = q_ref_real[i] - q_real[off]; + const double err_d = qd_ref_real[i] - qd_real[off];# + JointLogBuffer::JointLogEntry e{}; e.sim_time = _simTime; e.dt_taken = mathlib::real(result.stepOut.dt_taken); e.dt_sug = mathlib::real(result.stepOut.dt_sug); - e.theta = q_real[i]; e.omega = qd_real[i]; e.alpha = mathlib::real(dynResult.metrics.qdd[i]); + e.theta = q_real[off]; e.omega = qd_real[off]; e.alpha = mathlib::real(dynResult.metrics.qdd[off]); e.err = err; e.err_d = err_d; e.I_eff = I_eff; e.tau = mathlib::real(dynResult.metrics.tau[i]); e.tau_ff = tau_rnea_real[i]; e.tau_gravity = mathlib::real(dynResult.metrics.tau_g[i]); From 5e78796e28dd5217d57f7f504a8522d2ba8357e5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 08:42:54 +0100 Subject: [PATCH 077/114] refactor: Added comment to clarify the purpose of `nfDOF` in `SpatialJoint` --- DSFE_App/DSFE_Core/include/Systems/SpatialModel.h | 1 + 1 file changed, 1 insertion(+) diff --git a/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h index f03ca3a2..c1a15309 100644 --- a/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h @@ -19,6 +19,7 @@ namespace systems { mathlib::SpatialMat_T Xtree; mathlib::SpatialMat_T inertia; mathlib::SpatialVec_T S; + int nfDOF = 1; // number of degrees of freedom for this joint (1 for revolute/prismatic, 0 for fixed, 6 for free) std::string name; }; From ba40d6310db91820ca96b607a99cf88de45756d5 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Thu, 30 Jul 2026 08:43:03 +0100 Subject: [PATCH 078/114] refactor: Set degrees of freedom for joint types in `RigidBodySystem` --- DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index bba55ef6..0b253cf5 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -160,12 +160,19 @@ namespace systems { switch (j.type) { case eJointType::REVOLUTE: sj.S = mathlib::SpatialVec(j.axis.normalized(), mathlib::Vec3::Zero()); + sj.nfDOF = 1; break; case eJointType::PRISMATIC: sj.S = mathlib::SpatialVec(mathlib::Vec3::Zero(), j.axis.normalized()); + sj.nfDOF = 1; + break; + case eJointType::FREE: + sj.S = mathlib::SpatialVec(); + sj.nfDOF = 6; break; default: sj.S = mathlib::SpatialVec(); + sj.nfDOF = 0; break; } } From efc1434998cde5a8c9f09b232af732568d391f1a Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 12:35:43 +0100 Subject: [PATCH 079/114] refactor: Update pxmlib version to v1.0.2 in CMakeLists.txt --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5449e043..54821592 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,7 +24,7 @@ FetchContent_MakeAvailable(eigen) FetchContent_Declare( pxmlib GIT_REPOSITORY https://github.com/SaltyJoss/PxM-Lib.git - GIT_TAG v1.0.1 + GIT_TAG v1.0.2 ) FetchContent_MakeAvailable(pxmlib) From e6674c26c5f3a257a5d49c15519387631a6c50fb Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 12:35:50 +0100 Subject: [PATCH 080/114] refactor: Add additional buffers for joint dynamics in `DenseDynamicsScratch` --- DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h index f2d75ba8..28cedd2c 100644 --- a/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h @@ -84,7 +84,7 @@ namespace physics { std::vector> Xup; // spatial transformation from parent to current link std::vector> IA; // articulated body inertia std::vector> Ia; // articulated body inertia in the link frame - + std::vector> v; // spatial velocity std::vector> c; // spatial bias acceleration std::vector> a; // spatial acceleration @@ -92,6 +92,9 @@ namespace physics { std::vector> U; // articulated body force std::vector> f_ext; // spatial force + std::vector> dblk; // 6x6 per joint (free joints use it) + std::vector> ublk; // 6 per joint + mathlib::VecX_T g; // gravity vector in spatial coordinates (6D) mathlib::VecX_T u; // joint force contribution mathlib::VecX_T d; // joint inertia contribution @@ -120,7 +123,9 @@ namespace physics { pA.resize(nJoints); U.resize(nJoints); f_ext.resize(nJoints); - + dblk.resize(nJoints); + ublk.resize(nJoints); + g.resize(6); // gravity vector is always 6D u.resize(nJoints); d.resize(nJoints); @@ -144,11 +149,13 @@ namespace physics { pA.clear(); U.clear(); f_ext.clear(); + dblk.clear(); + ublk.clear(); g.resize(0); u.resize(0); d.resize(0); - + dXup_dq.clear(); dv_dq.clear(); dv_dqd.clear(); From faaa9eaf9eb397af3367b5626aa04001bcaee2a3 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 12:36:06 +0100 Subject: [PATCH 081/114] refactor: Updated spatial dynamics computations with additional output buffers and joint handling --- .../include/Physics/SpatialDynamics.h | 6 +- .../include/Physics/SpatialDynamics.inl | 91 +++++++++++++++---- 2 files changed, 76 insertions(+), 21 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h index 910849bb..825c804f 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h @@ -69,7 +69,9 @@ namespace physics { std::vector>& Ia_out, mathlib::VecX_T& u_out, mathlib::VecX_T& d_out, - std::vector>& U_out + std::vector>& U_out, + std::vector>& dblk_out, + std::vector>& ublk_out ); template @@ -81,6 +83,8 @@ namespace physics { const mathlib::VecX_T& d_out, const std::vector>& U, const SpatialVec_T& a0, + const std::vector>& dblk, + const std::vector>& ublk, std::vector>& a_out, mathlib::VecX_T& qdd_out ); diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 14679270..a43ac9a3 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -5,6 +5,12 @@ #pragma once namespace physics { + static mathlib::Quat expToQuat(const mathlib::Vec3& rv) { + const double theta = rv.norm(); + if (theta < 1e-9) { return mathlib::Quat(1, 0, 0, 0); } + return mathlib::Quat(Eigen::AngleAxisd(theta, rv / theta)); + } + template void SpatialDynamics::computeSpatialKinematicsAndBias( const systems::SpatialModel& model, @@ -15,9 +21,16 @@ namespace physics { std::vector>& c_out ) { const size_t n = model.joints.size(); + // Resize output vectors v_out.resize(n); Xup_out.resize(n); c_out.resize(n); + // Compute offsets for joint degrees of freedom + std::vector off(n); + { + int acc = 0; + for (size_t i = 0; i < n; ++i) { off[i] = acc; acc += model.joints[i].nfDOF; } + } for (size_t i = 0; i < n; ++i) { const systems::SpatialJoint& j = model.joints[i]; @@ -25,6 +38,21 @@ namespace physics { // Joint Transform XJ mathlib::SpatialMat_T XJ = mathlib::SpatialMat_T::Identity(); + if (j.type == systems::eJointType::FREE) { + if constexpr (std::is_same_v) { + mathlib::Vec3_T pos = q.template segment<3>(off[i]); // Free Joint Position + mathlib::Vec3_T rv = q.template segment<3>(off[i] + 3); // Free Joint Rotation Vector + mathlib::Quat_T q_full = (j.free_qref * expToQuat(rv)).normalized(); // Free Joint Orientation with Reference + mathlib::Mat3_T R = q_full.toRotationMatrix(); + XJ = mathlib::spatialTransform(R, pos); + Xup_out[i] = XJ * j.Xtree; // Combined Transform + mathlib::SpatialVec_T vJ; vJ.v = qd.template segment<6>(off[i]); + v_out[i] = (j.parent < 0) ? vJ : (Xup_out[i] * v_out[j.parent] + vJ); + c_out[i] = crossMotion(v_out[i], vJ); // Coriolis Term + } + continue; + } + if (j.type == systems::eJointType::REVOLUTE) { mathlib::Vec3_T axis = mathlib::safeNormalised(j.S.angular()); mathlib::Mat3_T R = mathlib::AngleAxis(q[i], axis); @@ -196,10 +224,11 @@ namespace physics { std::vector>& Ia_out, mathlib::VecX_T& u_out, mathlib::VecX_T& d_out, - std::vector>& U_out + std::vector>& U_out, + std::vector>& dblk_out, + std::vector>& ublk_out ) { const size_t n = model.joints.size(); - // Resize scratch buffers IA_out.resize(n); pA_out.resize(n); @@ -207,11 +236,17 @@ namespace physics { U_out.resize(n); u_out.resize(n); d_out.resize(n); + // Compute offsets for joint degrees of freedom + std::vector off(n); + { + int acc = 0; + for (size_t i = 0; i < n; ++i) { off[i] = acc; acc += model.joints[i].nfDOF; } + } // Upward pass: compute articulated body inertias and bias forces for (int i = (int)n - 1; i >= 0; --i) { const systems::SpatialJoint& j = model.joints[i]; - + // For fixed joints, propagate the articulated body inertia and bias force to the parent joint if (j.type == systems::eJointType::FIXED) { Ia_out[i] = IA_out[i]; if (j.parent >= 0) { @@ -221,19 +256,18 @@ namespace physics { } continue; } - + // For free joints, store the articulated body inertia and bias force in the dblk and ublk scratch buffers + if (j.nfDOF == 6) { + dblk_out[i] = IA_out[i]; ublk_out[i] = tau.segment(off[i], 6) - pA_out[i].v; + } + // Compute articulated body inertia and bias force for the current joint U_out[i] = IA_out[i] * j.S; d_out[i] = dot(j.S, U_out[i]); - if (d_out[i] < Scalar(1e-12)) { - d_out[i] = Scalar(1e-12); - } - + if (d_out[i] < Scalar(1e-12)) { d_out[i] = Scalar(1e-12); } u_out[i] = tau[i] - dot(j.S, pA_out[i]); Ia_out[i] = IA_out[i] - outer(U_out[i]) / d_out[i]; - // pA = pA + Ia * c + U * (u/d) pA_out[i] += Ia_out[i] * c[i] + U_out[i] * (u_out[i] / d_out[i]); - if (j.parent >= 0) { mathlib::SpatialMat_T XupT = Xup[i].transpose(); IA_out[j.parent] += XupT * Ia_out[i] * Xup[i]; @@ -251,26 +285,39 @@ namespace physics { const mathlib::VecX_T& d_out, const std::vector>& U, const SpatialVec_T& a0, + const std::vector>& dblk, + const std::vector>& ublk, std::vector>& a_out, mathlib::VecX_T& qdd_out ) { const size_t n = model.joints.size(); + // Resize output buffers a_out.resize(n); qdd_out.resize(n); - + // Compute offsets for joint degrees of freedom + std::vector off(n); + { + int acc = 0; + for (size_t i = 0; i < n; ++i) { off[i] = acc; acc += model.joints[i].nfDOF; } + } + // Downward pass: compute joint accelerations and spatial accelerations for each link for (size_t i = 0; i < n; ++i) { const systems::SpatialJoint& j = model.joints[i]; - + if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } - - if (j.type == systems::eJointType::FIXED) { - qdd_out[i] = Scalar(0); + if (j.nfDOF == 0) { continue; } // Skip fixed joints + if (j.nfDOF == 6) { + const mathlib::MatX_T& IAmat = dblk[i]; + mathlib::VecX_T rhs = ublk[i] - IAmat * a_out[i].v; + mathlib::VecX_T qdd_blk = IAmat.ldlt().solve(rhs); // Solve for joint accelerations using the articulated body inertia matrix + qdd_out.segment(off[i], 6) = qdd_blk; + a_out[i].v += qdd_blk; continue; } - qdd_out[i] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; - a_out[i] += j.S * qdd_out[i]; + qdd_out[off[i]] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; + a_out[i] += j.S * qdd_out[off[i]]; } } @@ -283,7 +330,9 @@ namespace physics { DynamicsScratch& scratch ) { const size_t n = model.joints.size(); - mathlib::VecX_T qdd = mathlib::VecX_T::Zero(n); + int nv = 0; + for (const auto& j : model.joints) { nv += j.nfDOF; } + mathlib::VecX_T qdd = mathlib::VecX_T::Zero(nv); mathlib::SpatialVec_T a0; // base acceleration (gravity) a0.v << @@ -309,14 +358,16 @@ namespace physics { model, scratch.spatial.Xup, scratch.spatial.v, scratch.spatial.c, tau, scratch.spatial.IA, scratch.spatial.pA, scratch.spatial.Ia, - scratch.spatial.u, scratch.spatial.d, scratch.spatial.U + scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, + scratch.spatial.dblk, scratch.spatial.ublk ); // Compute joint accelerations using the articulated body algorithm computeAccelerations_ABA( model, scratch.spatial.Xup, scratch.spatial.c, scratch.spatial.u, scratch.spatial.d, scratch.spatial.U, - a0, scratch.spatial.a, qdd + a0, scratch.spatial.dblk, scratch.spatial.ublk, + scratch.spatial.a, qdd ); return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) From 6c9b5aeac08e2f30cbdccd4eaea631648cdaf78f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 12:36:15 +0100 Subject: [PATCH 082/114] refactor: Updated joint DOF computation to use systems namespace --- DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index d13dfa79..4742021f 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -399,7 +399,7 @@ namespace physics { ) { const size_t n = model.joints.size(); int nv = 0; // number of degrees of freedom (DOF) in the model - for (const auto& j : model.joints) { nv += robots::jointDOF(j.type); } + for (const auto& j : model.joints) { nv += systems::jointDOF(j.type); } mathlib::VecX_T dx(2 * nv); Eigen::Map> q(x.data(), nv); @@ -431,9 +431,9 @@ namespace physics { int off = 0; // offset for indexing into the state vector for joints with multiple DOF for (size_t i = 0; i < n; ++i) { const systems::SpatialJoint& joint = model.joints[i]; - const int dof = robots::jointDOF(joint.type); // number of degrees of freedom for this joint + const int dof = systems::jointDOF(joint.type); // number of degrees of freedom for this joint if (dof == 0) { continue; } // skip fixed joints - if (dof !- 1 || !isControlledJoint(joint.type)) { + if (dof != 1 || !isControlledJoint(joint.type)) { off += dof; continue; // free (6-DOF) or uncontrolled means no control torque is applied, so skip to next joint } From 11101a1494835f1124fab5f28bc7c1807aa8647c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 12:36:28 +0100 Subject: [PATCH 083/114] feat: Added joint reference orientation to SpatialJoint struct and log joint metrics in postStepUpdate --- DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl | 4 ++-- DSFE_App/DSFE_Core/include/Systems/SpatialModel.h | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index 5d880e71..99af018b 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -217,8 +217,8 @@ namespace systems { if (dof == 0) { continue; } // skip fixed joints const double I_eff = (j.type == eJointType::FIXED) ? 1.0 : mathlib::real(dynResult.metrics.I_eff[i]); const double err = q_ref_real[i] - q_real[off]; - const double err_d = qd_ref_real[i] - qd_real[off];# - + const double err_d = qd_ref_real[i] - qd_real[off]; + // Log the joint metrics to the buffer JointLogBuffer::JointLogEntry e{}; e.sim_time = _simTime; e.dt_taken = mathlib::real(result.stepOut.dt_taken); diff --git a/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h index c1a15309..2b28aec2 100644 --- a/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h @@ -19,6 +19,7 @@ namespace systems { mathlib::SpatialMat_T Xtree; mathlib::SpatialMat_T inertia; mathlib::SpatialVec_T S; + mathlib::Quat_T free_qref{Scalar(1),Scalar(0),Scalar(0),Scalar(0)}; // free joint reference orientation int nfDOF = 1; // number of degrees of freedom for this joint (1 for revolute/prismatic, 0 for fixed, 6 for free) std::string name; From 4a242fc55a74fdbed69372019ef43da77d744c7a Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 12:36:34 +0100 Subject: [PATCH 084/114] feat: Stored free joint reference orientation in SpatialJoint during model construction --- DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 0b253cf5..72281ac4 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -175,6 +175,8 @@ namespace systems { sj.nfDOF = 0; break; } + + sj.free_qref = j.free_qref; // Store the free joint reference orientation } LOG_INFO("SpatialModel built: joints=%d", (long long)_spatialModel.joints.size()); } From ea252885bdbb1d87f6b443fc33d1dc211775a113 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:18:09 +0100 Subject: [PATCH 085/114] feat: Implemented `resetRigidBody` method to clear rigid body state and update runScriptToCompletion --- DSFE_App/DSFE_Core/include/Scene/SimulationCore.h | 1 + DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp | 13 ++++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 1018f9f2..a8454ae2 100644 --- a/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h @@ -98,6 +98,7 @@ namespace core { bool hasRigidBody() const override; void loadRigidBody(const std::string& name) override; void loadRigidBodyInternal(const std::string& name); // Internal method that assumes ownership + void resetRigidBody() override; // Reset the rigidBody system to its initial state, clearing any loaded rigidBody and resetting the simulation state // Run a script to completion synchronously with a specific integrator bool runScriptToCompletion(dsl::IStoredProgram* program, integration::eIntegrationMethod method) override; diff --git a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 4797e72f..a8f65351 100644 --- a/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp +++ b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp @@ -332,7 +332,7 @@ namespace core { LOG_INFO("SimulationCore::runScriptToCompletion -> START method=%s dt=%.6f hasRigidBody=%d", methodName.c_str(), _dt, (int)hasRigidBody()); // Reset rigidBody state - _rigidBody->resetRigidBody(); + resetRigidBody(); _traj->clearAll(); // Clear reference buffer (external for now) @@ -453,6 +453,12 @@ namespace core { if (!_rigidBody) { LOG_ERROR("Cannot load rigidBody: RigidBodySystem not set"); return; } _rigidBody->loadRigidBody(name); } + // Resets the rigidBody system to its initial state + void SimulationCore::resetRigidBody() { + if (!_rigidBody) { LOG_ERROR("Cannot reset rigidBody: RigidBodySystem not set"); return; } + _rigidBody->resetRigidBody(); + } + // Sets an external force on a specific link of the rigidBody system at a given world point bool SimulationCore::setLinkExternalForce(const std::string& link, const mathlib::Vec3& worldPoint, const mathlib::Vec3& worldForce) { return _rigidBody->setLinkExtForce(link, worldPoint, worldForce); @@ -559,6 +565,11 @@ namespace core { } void SimulationCore::setManipulating(bool on) { + // Check if the type is free-floating (no joints) and if so, ignore manipulation state changes + if (_rigidBody && _rigidBody->jointCount() == 0) { + D_WARN("setManipulating called on free-floating rigidBody; ignoring manipulation state change."); + return; + } if (on == _manipulating.load()) { return; } if (on) { setupSimulationIntegrator(); From 415e72332e230a13f227bbfe1484ff44ae72e21c Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:18:21 +0100 Subject: [PATCH 086/114] feat: Added `resetRigidBody` method to reset the rigid body system to its initial state --- DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h | 1 + 1 file changed, 1 insertion(+) diff --git a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h index 0d3de299..8fb21b37 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -68,6 +68,7 @@ namespace core { virtual void loadRigidBody(const std::string& name) = 0; virtual bool rigidBodyPresentationDirty() const = 0; virtual void clearRigidBodyPresentationDirty() = 0; + virtual void resetRigidBody() = 0; // Reset the rigidBody system to its initial state, clearing any loaded rigidBody and resetting the simulation state // Script execution virtual void setRunTag(const std::string& tag) = 0; virtual void setScriptRunning(bool running) = 0; From d29181c660fcd89d062fcc4eaef30fa1c015a53b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:19:12 +0100 Subject: [PATCH 087/114] feat: Added callback to core's `resetRigidBody` functionality to SimulationManager --- .../DSFE_GUI/src/Simulation/SimulationManager.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index a6ca71ae..c6eaef63 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -159,7 +159,12 @@ namespace gui { _currentRigidBodyPath = name; LOG_INFO("RigidBody loaded: %s", model.name.c_str()); } - + // Resets the rigidBody system to its initial position + void SimulationManager::resetRigidBody() { + if (!_core) { LOG_ERROR("Simulation core not initialised, cannot reset rigidBody"); return; } + _core->resetRigidBody(); + } + // Clears the rigidBody system and removes all associated objects from the scene void SimulationManager::clearRigidBody() { _systems.clear_all(_scene); _scene.clear(); } // -------------------------------------------------- @@ -411,12 +416,11 @@ namespace gui { // Set a highlight color for a specific link in the rigidBody system. This is typically used to visually indicate selection or focus on a particular link in the GUI. void SimulationManager::setLinkHighlight(const std::string& link, bool on) { - const auto names = _core->linkNames(); + const auto names = linkNames(); int idx = -1; for (size_t i = 0; i < names.size(); ++i) { if (names[i] == link) { idx = (int)i; break; } } - if (idx < 0) { return; } + if (idx <= 0) { return; } if (on) { - // Cache original, then tint faint blue. if (const auto* r = _scene.renderable((uint32_t)idx)) { _highlightIdx = idx; _highlightAlbedo0 = glm::vec3(r->albedo); From 4a4cac6f2e9146fd096d7ee565c9c5f3aeeb21bd Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:19:26 +0100 Subject: [PATCH 088/114] feat: Added `resetRigidBody` method and menu action to MainWindow for rigid body reset functionality --- .../include/MainWindow/DSFE_MainWindow.h | 1 + .../src/MainWindow/DSFE_MainWindow.cpp | 12 +++++- .../MainWindow/Widgets/ControlPanelWidget.cpp | 40 +++++++++++++------ 3 files changed, 39 insertions(+), 14 deletions(-) diff --git a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h index 0486ed88..e0b732ee 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/DSFE_MainWindow.h @@ -29,6 +29,7 @@ namespace window { void buildMenuBar(); void buildSceneMenu(QMenu* sceneMenu); void buildRobotMenu(QMenu* projectMenu); + void resetRigidBody(); void onLoadMesh(); bool _dirty = false; diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 0fdcea6a..3f67ccb0 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp @@ -150,9 +150,14 @@ namespace window { } // View menu { - auto* sceneMenu = viewMenu->addMenu("SceneOptions"); + auto* sceneMenu = viewMenu->addMenu("Scene"); buildSceneMenu(sceneMenu); viewMenu->addSeparator(); + auto* resetRigidBody = viewMenu->addAction("Reset RigidBody"); + connect(resetRigidBody, &QAction::triggered, this, []() { + LOG_INFO("Menu clicked: View -> Reset RigidBody"); + + }); auto* resetCameraAction = viewMenu->addAction("Reset Camera"); connect(resetCameraAction, &QAction::triggered, this, []() { LOG_INFO("Menu clicked: View -> Reset Camera"); @@ -194,6 +199,11 @@ namespace window { }); } + void DSFE_MainWindow::resetRigidBody() { + if (!_sim) { LOG_ERROR("Simulation Manager not found!"); return; } + _sim->resetRigidBody(); + } + // Build the robot menu dynamically based on the available robotic systems, using the general RigidBody System interface void DSFE_MainWindow::buildRobotMenu(QMenu* projectMenu) { const auto& robotMap = platform::getRobotSystemMap(); diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 69d5cef8..37d2a6fe 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -348,26 +348,40 @@ namespace widgets { t.trajectoryHeader = new QLabel("TRAJECTORY"); t.clampedHeader = new QLabel("LIMITS"); t.constantsHeader = new QLabel("PHYSICAL"); - for (QLabel* h : { t.stateHeader, t.referenceHeader, t.trajectoryHeader, t.clampedHeader, t.constantsHeader }) + for (QLabel* h : { + t.stateHeader, + t.referenceHeader, + t.trajectoryHeader, + t.clampedHeader, + t.constantsHeader + } + ) { headerFont(h); + } t.q = new QLabel(); t.qd = new QLabel(); t.tau = new QLabel(); t.qRef = new QLabel(); t.qdRef = new QLabel(); t.qddRef = new QLabel(); t.err = new QLabel(); t.qTraj = new QLabel(); t.qdTraj = new QLabel(); t.qddTraj = new QLabel(); t.qClamped = new QLabel(); t.qdClamped = new QLabel(); t.damping = new QLabel(); t.friction = new QLabel(); - for (QLabel* v : { t.q, t.qd, t.tau, t.qRef, t.qdRef, t.qddRef, t.err, - t.qTraj, t.qdTraj, t.qddTraj, t.qClamped, t.qdClamped, t.damping, t.friction }) - valueLabel(v); - - auto makeGrid = [](std::initializer_list> rows) { - auto* g = new QGridLayout(); - int r = 0; - for (auto& [sym, val] : rows) { - g->addWidget(sym, r, 0, Qt::AlignLeft | Qt::AlignVCenter); - g->addWidget(val, r, 1); - ++r; - } + for (QLabel* v : { + t.q, t.qd, t.tau, t.err, + t.qRef, t.qdRef, t.qddRef, + t.qTraj, t.qdTraj, t.qddTraj, + t.qClamped, t.qdClamped, + t.damping, t.friction + } + ) { + valueLabel(v); + } + + auto makeGrid = [](std::initializer_list> rows) { + auto* g = new QGridLayout(); int r = 0; + for (auto& [sym, val] : rows) { + g->addWidget(sym, r, 0, Qt::AlignLeft | Qt::AlignVCenter); + g->addWidget(val, r, 1); + ++r; + } g->setHorizontalSpacing(14); g->setVerticalSpacing(3); g->setColumnStretch(0, 0); From 4e289d8cfdc3b9aaede691bdd55dc1335756fec9 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:19:36 +0100 Subject: [PATCH 089/114] fixes: Corrected material RGBA value in cube.urdf and added scale attribute to robot --- DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf index 9f8f7d4b..eb4c9205 100644 --- a/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf @@ -1,11 +1,10 @@ - - + - + From 524e57be84c656ba3c792f4b264cfe8f45bf82ac Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:19:51 +0100 Subject: [PATCH 090/114] feat: Added missing fields `nfDOF` and `free_qref` to `SpatialModel` cast method --- DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl b/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl index eded7fc9..12afbed5 100644 --- a/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl @@ -19,6 +19,8 @@ namespace systems { out_j.inertia = j.inertia.template cast(); out_j.S = j.S.template cast(); out_j.name = j.name; + out_j.nfDOF = j.nfDOF; + out_j.free_qref = j.free_qref.template cast(); } return out; } From 18f5c38e45a69d04e5e896e7e5cf7b98b4e2af00 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:20:25 +0100 Subject: [PATCH 091/114] fixes: Corrected index usage for torque assignment and added missing offset increment in `derivate_spatial` --- .../include/Physics/RigidBodyDynamics.inl | 3 ++- .../DSFE_Core/include/Physics/SpatialDynamics.inl | 14 ++++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index 4742021f..2aae5209 100644 --- a/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -454,7 +454,7 @@ namespace physics { const Scalar Q_max = static_cast(snap.model->joints[i].limits.maxEffort); if (Q_max > Scalar(1e-9)) { tau_i = Q_max * mathlib::tanh(tau_i / Q_max); } // saturate control torque to max effort using smooth tanh saturation - scratch.dense.tau[i] = tau_i; + scratch.dense.tau[off] = tau_i; out.metrics.q[i] = mathlib::real(q[off]); out.metrics.qd[i] = mathlib::real(qd[off]); @@ -462,6 +462,7 @@ namespace physics { out.metrics.errd[i] = mathlib::real(err_d); out.metrics.I_eff[i] = mathlib::real(I_eff); out.metrics.tau[i] = mathlib::real(tau_i); + off += dof; // I forgot to add this in the previous version } out.qdd = SpatialDynamics::ABA(model, q, qd, scratch.dense.tau, scratch); diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index a43ac9a3..0ff26b72 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -44,7 +44,8 @@ namespace physics { mathlib::Vec3_T rv = q.template segment<3>(off[i] + 3); // Free Joint Rotation Vector mathlib::Quat_T q_full = (j.free_qref * expToQuat(rv)).normalized(); // Free Joint Orientation with Reference mathlib::Mat3_T R = q_full.toRotationMatrix(); - XJ = mathlib::spatialTransform(R, pos); + mathlib::Vec3_T zero_R = mathlib::Vec3_T::Zero(); + XJ = mathlib::spatialTransform(R, zero_R); Xup_out[i] = XJ * j.Xtree; // Combined Transform mathlib::SpatialVec_T vJ; vJ.v = qd.template segment<6>(off[i]); v_out[i] = (j.parent < 0) ? vJ : (Xup_out[i] * v_out[j.parent] + vJ); @@ -308,9 +309,14 @@ namespace physics { else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } if (j.nfDOF == 0) { continue; } // Skip fixed joints if (j.nfDOF == 6) { - const mathlib::MatX_T& IAmat = dblk[i]; - mathlib::VecX_T rhs = ublk[i] - IAmat * a_out[i].v; - mathlib::VecX_T qdd_blk = IAmat.ldlt().solve(rhs); // Solve for joint accelerations using the articulated body inertia matrix + mathlib::VecX_T a_prop = a_out[i].v; + mathlib::VecX_T rhs = ublk[i] - dblk[i] * a_prop; + mathlib::VecX_T qdd_blk = dblk[i].ldlt().solve(rhs); // Solve for joint accelerations using the articulated body inertia matrix + if constexpr (std::is_same_v) { + LOG_INFO_ONCE("free a_prop=[%.3f %.3f %.3f | %.3f %.3f %.3f] qdd=[%.3f %.3f %.3f | %.3f %.3f %.3f]", + a_prop(0),a_prop(1),a_prop(2),a_prop(3),a_prop(4),a_prop(5), + qdd_blk(0),qdd_blk(1),qdd_blk(2),qdd_blk(3),qdd_blk(4),qdd_blk(5)); + } qdd_out.segment(off[i], 6) = qdd_blk; a_out[i].v += qdd_blk; continue; From 3a2fccd11060e4c5a6b96e0105eddb3fd8029a61 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:23:58 +0100 Subject: [PATCH 092/114] refactor(For previous commit): Updated free-joint spatial transform to now use a zero translation vector --- DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl | 1 - 1 file changed, 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 0ff26b72..49967b5c 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -321,7 +321,6 @@ namespace physics { a_out[i].v += qdd_blk; continue; } - qdd_out[off[i]] = (u_out[i] - U[i].dot(a_out[i])) / d_out[i]; a_out[i] += j.S * qdd_out[off[i]]; } From 5a5d8cb5c53db7915b087b13170ae9e249fd7c6a Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:24:08 +0100 Subject: [PATCH 093/114] feat: Updated URDF loading by synthesizing FREE joints for single free bodies and converting link names to lowercase --- .../src/Systems/RigidBodyLoaderURDF.cpp | 40 +++++++++++++++++-- 1 file changed, 36 insertions(+), 4 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp index 65846a5d..e8b98890 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -12,6 +12,7 @@ #include #include +#include #include #include #include @@ -22,9 +23,9 @@ using namespace constants; namespace systems { // rpy(radians) -> quaternion, q = qz*qy*qx, where qx = roll, qy = pitch, qz = yaw static Quat urdf_rpyToQuat(const mathlib::Vec3& rpy) { - const Quat qx(Eigen::AngleAxisd(rpy.x(), Vec3(1.0, 0.0, 0.0))); + const Quat qx(Eigen::AngleAxisd(rpy.x(), Vec3(1.0, 0.0, 0.0))); const Quat qy(Eigen::AngleAxisd(rpy.y(), Vec3(0.0, 1.0, 0.0))); - const Quat qz(Eigen::AngleAxisd(rpy.z(), Vec3(0.0, 0.0, 1.0))); + const Quat qz(Eigen::AngleAxisd(rpy.z(), Vec3(0.0, 0.0, 1.0))); return (qz * qy * qx).normalized(); } // Parse a space-separated triple of doubles from a string, e.g. "1.0 2.0 3.0" @@ -49,7 +50,10 @@ namespace systems { } // Link Parsing static void urdf_parseLink(XMLElement* lEl, RigidBodyLink& link, const std::string& meshdir) { - link.name = lEl->Attribute("name") ? lEl->Attribute("name") : ""; + std::string name = lEl->Attribute("name") ? lEl->Attribute("name") : ""; + // convert to lowercase for consistency + std::transform(name.begin(), name.end(), name.begin(), [](unsigned char c) { return std::tolower(c); }); + link.name = name; // Visual if (XMLElement* v = lEl->FirstChildElement("visual")) { // Origin @@ -187,7 +191,7 @@ namespace systems { XMLElement* robot = doc.FirstChildElement("robot"); if (!robot) { LOG_ERROR("No element found in URDF file: %s", fp.c_str()); return rb; } rb.name = robot->Attribute("name") ? robot->Attribute("name") : "unnamed_body"; - rb.scale = 1.0f; // URDF does not specify a scale, so we default to 1.0 + rb.scale = robot->QueryFloatAttribute("scale", &rb.scale) == XML_SUCCESS ? rb.scale : 1.0; rb.kinematicsModel = eKinematicsModel::URDF; // URDF has NO baseframe, most of the models I use need a Z-up -> engine -90deg X rotation. @@ -229,6 +233,34 @@ namespace systems { joint.limits.maxEffort, joint.limits.minAngle, joint.limits.maxAngle, joint.limits.maxqd ); } + // Find links that are no joint's child (roots) + std::set child_links; + for (const auto& j : rb.joints) { child_links.insert(j.child); } + for (const auto& link : rb.links) { + if (child_links.find(link.name) == child_links.end()) { + // This link has no parent joint. If it's the ONLY link (single body), make it FREE. + if (rb.links.size() == 1) { + RigidBodyJoint freeJoint; + freeJoint.name = "free_" + link.name; + freeJoint.type = eJointType::FREE; + freeJoint.parent = "world"; + freeJoint.child = link.name; + freeJoint.free_pos = Vec3(0.0, 0.5, 0.0); // 50cm up + freeJoint.free_qref = Quat(1,0,0,0); + freeJoint.free_rot_v = Vec3::Zero(); + freeJoint.free_vel = VecX::Zero(6); + rb.joints.push_back(freeJoint); + LOG_INFO("Synthesized FREE joint for single free body '%s'", link.name.c_str()); + LOG_INFO("RigidBody (%s) origin q(joint): (%.3f, %.3f, %.3f, %.3f), origin xyz(joint): (%.3f, %.3f, %.3f)", link.name.c_str(), + freeJoint.origin_q.w(), freeJoint.origin_q.x(), freeJoint.origin_q.y(), freeJoint.origin_q.z(), + freeJoint.origin_xyz.x(), freeJoint.origin_xyz.y(), freeJoint.origin_xyz.z() + ); + } + } + LOG_INFO("RigidBody (%s) origin xyz(link): (%.3f, %.3f, %.3f)", link.name.c_str(), + link.visual.origin_xyz.x(), link.visual.origin_xyz.y(), link.visual.origin_xyz.z() + ); + } LOG_INFO("RigidBody (URDF) loaded: %d links, %d joints", static_cast(rb.links.size()), static_cast(rb.joints.size())); return rb; } From 411ae5cbcd6c94d7139bab00733d1e52ae784554 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 16:25:27 +0100 Subject: [PATCH 094/114] chore(WIP): Comment out base pose update logic for free-floating bodies and clean up code * Commiting due to me taking a break for a few hours, but I wanted to make sure I can access it on my laptop --- .../include/Systems/RigidBodySystemStep.inl | 8 +- .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 96 ++++++++++--------- 2 files changed, 57 insertions(+), 47 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index 99af018b..650c0c2b 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -301,10 +301,10 @@ namespace systems { postStepUpdate(x_real, _dynScratch_AD, result); // Update base pose if free-floating - if (_baseIsFree) { - integrateBaseTranslation(dt); - updateBaseRootPose(); - } + // if (_baseIsFree) { + // integrateBaseTranslation(dt); + // updateBaseRootPose(); + // } // Update kinematics computeRigidBodyKinematics(_worldTransforms); } diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 72281ac4..777facc3 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -108,54 +108,44 @@ namespace systems { _spatialModel.joints.clear(); const size_t n = _body.joints.size(); _spatialModel.joints.resize(n); - + // Build the spatial model from the rigidBody joints for (size_t i = 0; i < n; ++i) { const RigidBodyJoint& j = _body.joints[i]; auto& sj = _spatialModel.joints[i]; - sj.name = j.name; sj.type = j.type; - // Find parent joint sj.parent = -1; for (size_t p = 0; p < n; ++p) { if (_body.joints[p].child == j.parent) { - sj.parent = (int)p; - break; + sj.parent = (int)p; break; } } - // Build XTree mathlib::Mat3 R = j.origin_q.toRotationMatrix(); mathlib::Vec3 r = j.origin_xyz; sj.Xtree = mathlib::spatialTransform(R, r); - // Build Spatial Inertia int childLinkIdx = -1; for (size_t l = 0; l < _body.links.size(); ++l) { if (_body.links[l].name == j.child) { - childLinkIdx = (int)l; - break; + childLinkIdx = (int)l; break; } } - if (childLinkIdx >= 0) { const RigidBodyLink& link = _body.links[childLinkIdx]; mathlib::Mat3 I_com; - const auto& I = link.inertial.inertia; I_com << I.ixx, I.ixy, I.ixz, I.ixy, I.iyy, I.iyz, I.ixz, I.iyz, I.izz; - sj.inertia = mathlib::spatialInertia( link.inertial.mass, link.inertial.com_xyz, I_com ); } - // Build S vector (motion subspace) switch (j.type) { case eJointType::REVOLUTE: @@ -175,7 +165,6 @@ namespace systems { sj.nfDOF = 0; break; } - sj.free_qref = j.free_qref; // Store the free joint reference orientation } LOG_INFO("SpatialModel built: joints=%d", (long long)_spatialModel.joints.size()); @@ -398,33 +387,45 @@ namespace systems { _simTime = simTime; const size_t n = _body.joints.size(); mathlib::VecX x = packState(); - // Apply floor contact forces if enabled (Bit crude but yeah) - { - constexpr double k_floor = 400000.0; - constexpr double c_floor = 2000.0; - const size_t nl = _body.links.size(); - if (_prevLinkY.size() != nl) { _prevLinkY.assign(nl, 0.0); } - for (size_t i = 0; i < nl; ++i) { - const Mat4& T = _worldTransforms[i]; - const double lowY = linkWorldMinY(i, T); - const double vy = (lowY - _prevLinkY[i]) / (_dynamics->dt() > 0 ? _dynamics->dt() : (1.0/180.0)); - _prevLinkY[i] = lowY; - if (lowY < 0.0) { - double Fy = -k_floor * lowY - c_floor * vy; - if (Fy < 0.0) { Fy = 0.0; } // floor only pushes, never pulls - setLinkExtForce( - _body.links[i].name, - Vec3(T(0,3), lowY, T(2,3)), - Vec3(0.0, Fy, 0.0) - ); - } - } - } + // // Apply floor contact forces if enabled (Bit crude but yeah) + // { + // constexpr double k_floor = 50000.0; // [N/m] spring constant for floor contact + // constexpr double c_floor = 2000.0; // [N/(m/s)] damping constant for floor contact + // const size_t nl = _body.links.size(); + // if (_prevLinkY.size() != nl) { _prevLinkY.assign(nl, 0.0); } + // for (size_t i = 0; i < nl; ++i) { + // const Mat4& T = _worldTransforms[i]; + // const double lowY = linkWorldMinY(i, T); + // const double vy = (lowY - _prevLinkY[i]) / (_dynamics->dt() > 0 ? _dynamics->dt() : (1.0/180.0)); + // _prevLinkY[i] = lowY; + // if (lowY < 0.0) { + // double Fy = -k_floor * lowY - c_floor * vy; + // if (Fy < 0.0) { Fy = 0.0; } // floor only pushes, never pulls + // setLinkExtForce( + // _body.links[i].name, + // Vec3(T(0,3), lowY, T(2,3)), + // Vec3(0.0, Fy, 0.0) + // ); + // } + // } + // } assembleExtForces(_dynScratch); auto result = step_impl(x, dt, simTime, *_integrator, _dynScratch, _dynResult); clearExtForces(); unpackState(result.stepOut.x_next); + { + static int s_freeLogCount = 0; + const bool logNow = (++s_freeLogCount % 60 == 0); + for (const auto& j : _body.joints) { + if (j.type == eJointType::FREE && logNow) { + LOG_INFO("free pos=(%.4f %.4f %.4f) w=(%.5f %.5f %.5f) v=(%.5f %.5f %.5f)", + j.free_pos.x(), j.free_pos.y(), j.free_pos.z(), + j.free_vel(0), j.free_vel(1), j.free_vel(2), // angular (the NaN one) + j.free_vel(3), j.free_vel(4), j.free_vel(5)); // linear + } + } + } _dynamics->setDt(result.stepOut.dt_taken); const auto scratchCopy = _dynScratch; @@ -433,10 +434,10 @@ namespace systems { postStepUpdate(resultCopy.stepOut.x_next, scratchCopy, resultCopy); // Update base pose if free-floating - if (_baseIsFree) { - integrateBaseTranslation(dt); - updateBaseRootPose(); - } + // if (_baseIsFree) { + // integrateBaseTranslation(dt); + // updateBaseRootPose(); + // } // Update kinematics computeRigidBodyKinematics(_worldTransforms); @@ -698,6 +699,17 @@ namespace systems { int rootIdx = itRoot->second; world[rootIdx] = _root_pose; + for (const auto& j : _body.joints) { + if (j.type == eJointType::FREE) { + mathlib::Quat q_full = (j.free_qref * expToQuat(j.free_rot_v)).normalized(); + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = q_full.toRotationMatrix(); // set rotation to free_qref * exp(free_rot_v) + T.block<3, 1>(0, 3) = j.free_pos; // set translation to free_pos + auto itC = _link_idx.find(j.child); // find child link index + if (itC != _link_idx.end()) { world[itC->second] = T; } + } + } + // parent -> children joints std::unordered_map> children; children.reserve(_body.joints.size()); @@ -717,11 +729,9 @@ namespace systems { int pIdx = itP->second; const Mat4& T_parent = world[pIdx]; - - // Find children joints auto it = children.find(parentName); - if (it == children.end()) continue; + if (it == children.end()) { continue; } // For each child joint for (const RigidBodyJoint* jp : it->second) { From bc6d5a860fa312a4e2017c4b496b141289c49c62 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 17:59:45 +0100 Subject: [PATCH 095/114] feat: Added `hasFreeJoint` method to check for free-floating joints in RigidBodySystem --- DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h | 1 + DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index bbe0484a..30bca934 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -82,6 +82,7 @@ namespace systems { std::size_t jointCount() const { return _body.joints.size(); } std::string findRootLink() const; bool hasLinkName(const std::string& linkName) const { return _link_idx.find(linkName) != _link_idx.end(); } + bool hasFreeJoint() const; int jointStateOffset(size_t joint_idx) const; int totalDOF() const; diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 777facc3..bbb79fc7 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -64,6 +64,13 @@ namespace systems { for (const auto& j : _body.joints) { nv += jointDOF(j.type); } return nv; } + /* + * Method to check if the rigidBody system has any free-floating joints + */ + bool RigidBodySystem::hasFreeJoint() const { + for (const auto& j : _body.joints) { if (j.type == eJointType::FREE) { return true; } } + return false; + } // --- HELPER METHODS --- @@ -777,6 +784,7 @@ namespace systems { // Method to get the angle of a specific rigidBody joint bool RigidBodySystem::tryGetJointAngleRad(const std::string& childLink, double& outAngle) const { if (!_hasBody) { return false; } + if (hasFreeJoint()) { return false; } // Find joint child matching childLink for (const auto& joint : _body.joints) { if (joint.child == childLink) { @@ -790,6 +798,7 @@ namespace systems { // Method to set the angle of a specific rigidBody joint bool RigidBodySystem::trySetJointAngleRad(const std::string& childLink, double angleRad) { if (!_hasBody) { return false; } + if (hasFreeJoint()) { return false; } // Find joint child matching childLink for (auto& joint : _body.joints) { if (joint.child == childLink) { From 2d7ae6443111e9dd47db06322b032e9108ecad56 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Fri, 31 Jul 2026 18:00:04 +0100 Subject: [PATCH 096/114] feat: Added check for free-floating joints in `trajSet` command execution --- DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp index 511e6246..deaf7edf 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp @@ -65,6 +65,11 @@ namespace commands { } auto& body = cntx.RigidBody(); + if (body.hasFreeJoint()) { + SIM_FAIL("trajSet: '%s' is a free body; trajectories apply only to articulated joints.", _link.c_str()); + markFailed("trajSet: cannot set trajectory for free-floating joint."); + return { CmdState::Failed, {}, "trajSet failed" }; + } // Validate joint exists and get current angle as q0. double q0 = 0.0f; if (!body.tryGetJointAngleRad(_link, q0)) { From 12d170f7b5a0b78ca12e35b0c1e7fb02f1e7a438 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 07:57:04 +0100 Subject: [PATCH 097/114] feat: Added methods to get and set free-floating joint velocities in RigidBodySystem --- .../include/Systems/RigidBodySystem.h | 3 ++ .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 30 +++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 30bca934..467b66f7 100644 --- a/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -122,6 +122,9 @@ namespace systems { bool tryGetJointAngleRad(const std::string& childLink, double& outAngle) const; bool trySetJointAngleRad(const std::string& childLink, double angleRad); + bool tryGetFreeVelocity(const std::string& linkName, mathlib::VecX& outVel) const; + bool trySetFreeVelocity(const std::string& linkName, const mathlib::VecX& vel); + bool tryGetJointOmegaRad(const std::string& childLink, double& outOmega) const; bool trySetJointOmegaRad(const std::string& childLink, double omegaRad); bool injectJointOmegaRad(const std::string& childLink, double omega); diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index bbb79fc7..c0cdd5db 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -809,6 +809,36 @@ namespace systems { return false; } + /* + * + */ + bool RigidBodySystem::tryGetFreeVelocity(const std::string& linkName, mathlib::VecX& outVel) const { + if (!_hasBody) { return false; } + for (const auto& joint : _body.joints) { + if (joint.type == eJointType::FREE && joint.child == linkName) { + outVel = joint.free_vel; + return true; + } + } + return false; + } + + /* + * Method to set the free-floating joint velocity of a specific rigidBody joint + * @param linkName: The name of the link associated with the free joint + */ + bool RigidBodySystem::trySetFreeVelocity(const std::string& linkName, const mathlib::VecX& vel) { + if (!_hasBody) { return false; } + for (auto& joint : _body.joints) { + if (joint.type == eJointType::FREE && joint.child == linkName) { + if (vel.size() != 6) { return false; } + joint.free_vel = vel; + return true; + } + } + return false; + } + // Method to get the angular velocity of a specific rigidBody joint bool RigidBodySystem::tryGetJointOmegaRad(const std::string& childLink, double& outOmega) const { if (!_hasBody) { return false; } From a68d95936427d17bc09211c0d019a16f686a1be2 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 07:57:30 +0100 Subject: [PATCH 098/114] feat: Implemented `SetVelocityCmd` for setting velocities of free bodies --- DSFE_App/DSFE_Core/CMakeLists.txt | 1 + .../include/DSL/Commands/SetVelocityCmd.h | 40 ++++++++++ .../src/DSL/Commands/SetVelocityCmd.cpp | 77 +++++++++++++++++++ 3 files changed, 118 insertions(+) create mode 100644 DSFE_App/DSFE_Core/include/DSL/Commands/SetVelocityCmd.h create mode 100644 DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index c9d2cc48..a43baa12 100644 --- a/DSFE_App/DSFE_Core/CMakeLists.txt +++ b/DSFE_App/DSFE_Core/CMakeLists.txt @@ -93,6 +93,7 @@ set(DSL_SRC src/DSL/Commands/SelectCmd.cpp src/DSL/Commands/SetCmd.cpp src/DSL/Commands/SetOmegaCmd.cpp + src/DSL/Commands/SetVelocityCmd.cpp src/DSL/Commands/SpinCmd.cpp src/DSL/Commands/StartCmd.cpp src/DSL/Commands/StopCmd.cpp diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SetVelocityCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SetVelocityCmd.h new file mode 100644 index 00000000..d43c3f1c --- /dev/null +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SetVelocityCmd.h @@ -0,0 +1,40 @@ +/* + * File: DSL/Commands/SetVelocityCmd.h + * Created by: Joss Salton, 26-07-2026 + */ +#include "EngineCore.h" + +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" + +namespace commands { + class SetVelocityCmd final : public Command { + public: + SetVelocityCmd(std::string link, double wx, double wy, double wz, double vx, double vy, double vz); + ~SetVelocityCmd() override = default; + + std::string_view getName() const { return "setVelocity"; } + void setContext(CommandContext& cntx) override { _cntx = &cntx; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } + + private: + CmdResult update(CommandContext& cntx, double dt) override; + void execute() override; + + std::string _link; + double _wx, _wy, _wz; // angular velocity components + double _vx, _vy, _vz; // linear velocity components + bool _done = false; + bool _started = false; + CmdResult _result{ CmdState::NotStarted, {}, "" }; + + protected: + void markFailed(const std::string& message) override; + void markCompleted() override; + bool hasStarted() const override; + }; + std::unique_ptr CreateSetVelocityCmd(const std::string& id, const std::vector& args); +} \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp new file mode 100644 index 00000000..fd31e9eb --- /dev/null +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp @@ -0,0 +1,77 @@ +/* + * File: DSL/Commands/SetVelocityCmd.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "DSL/Commands/SetVelocityCmd.h" +#include "Scene/SimulationCore.h" +#include "Systems/RigidBodySystem.h" + +#include "DSL/Utils.h" +#include "EngineLib/LogMacros.h" + +namespace commands { + // --- Markers --- + void SetVelocityCmd::markFailed(const std::string& message) { setResult({ CmdState::Failed, {}, message }); } + void SetVelocityCmd::markCompleted() { setResult({ CmdState::Executed, {}, "setVelocity ran successfully" }); } + bool SetVelocityCmd::hasStarted() const { return _started; } + + // Constructor + SetVelocityCmd::SetVelocityCmd(std::string link, double wx, double wy, double wz, double vx, double vy, double vz) + : _link(std::move(link)), _wx(wx), _wy(wy), _wz(wz), _vx(vx), _vy(vy), _vz(vz) { + _result = { CmdState::NotStarted, {}, "" }; + } + + CmdResult SetVelocityCmd::update(CommandContext& cntx, double dt) { + if (_done) { return { CmdState::Executed, {}, "setVelocity completed" }; } + if (!_started) { return { CmdState::NotStarted, {}, "setVelocity not started" }; } + + auto* core = cntx.Core(); + if (!core) { + SIM_FAIL("setVelocity: SimulationCore is null."); + markFailed("setVelocity: SimulationCore is null."); + return { CmdState::Failed, {}, "setVelocity failed" }; + } + + auto& body = cntx.RigidBody(); + mathlib::VecX v = mathlib::VecX::Zero(6); + v << _wx, _wy, _wz, _vx, _vy, _vz; // [Angular Velocity (rad/s), Linear Velocity (m/s)] + + if (!body.trySetFreeVelocity(_link, v)) { + SIM_FAIL("setVelocity: '%s' is not a free body.", _link.c_str()); + markFailed("setVelocity: target is not a free body."); + return { CmdState::Failed, {}, "setVelocity failed" }; + } + + _done = true; + markCompleted(); + return { CmdState::Executed, {}, "setVelocity executed" }; + } + + // Execute command + void SetVelocityCmd::execute() { + setResult({ CmdState::Executing, {}, "setVelocity started" }); + } + + // --- Factory --- + std::unique_ptr CreateSetVelocityCmd(const std::string& id, const std::vector& args) { + if (id.empty()) { + D_FAIL("setVelocity: missing identifier (link)."); + return nullptr; + } + if (args.size() != 6) { + D_FAIL("setVelocity expects 6 args: wx, wy, wz, vx, vy, vz."); + return nullptr; + } + const std::string link = id; + double wx = utils::parseDouble(args[0]); + double wy = utils::parseDouble(args[1]); + double wz = utils::parseDouble(args[2]); + double vx = utils::parseDouble(args[3]); + double vy = utils::parseDouble(args[4]); + double vz = utils::parseDouble(args[5]); + + return std::make_unique(link, wx, wy, wz, vx, vy, vz); + } +} // namespace commands \ No newline at end of file From 3ef79ec4dcd9ab9405d1e21b8afac8654a6c50df Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 07:57:39 +0100 Subject: [PATCH 099/114] feat: Registered `SetVelocityCmd` in command factory --- DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp b/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp index 82679d16..156a206e 100644 --- a/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp @@ -15,6 +15,7 @@ #include "DSL/Commands/TrajSetCmd.h" #include "DSL/Commands/TrajClearCmd.h" #include "DSL/Commands/SetOmegaCmd.h" +#include "DSL/Commands/SetVelocityCmd.h" // Primary function commands #include "DSL/Commands/StartCmd.h" @@ -36,13 +37,14 @@ namespace commands { factory.registerCommand("trajset", &commands::CreateTrajSetCmd); // trajSet command factory.registerCommand("trajclear", &commands::CreateTrajClearCmd); // trajClear command factory.registerCommand("setomega", &commands::CreateSetOmegaCmd); // setOmega command + factory.registerCommand("setvelocity", &commands::CreateSetVelocityCmd); // setVelocity command // Primary Function commands - factory.registerCommand("start", &commands::CreateStartCmd); // start command - factory.registerCommand("stop", &commands::CreateStopCmd); // stop command - factory.registerCommand("wait", &commands::CreateWaitCmd); // pause command - factory.registerCommand("select", &commands::CreateSelectCmd); // select command - factory.registerCommand("load", &commands::CreateLoadCmd); // load command - factory.registerCommand("set", &commands::CreateSetCmd); // set command + factory.registerCommand("start", &commands::CreateStartCmd); // start command + factory.registerCommand("stop", &commands::CreateStopCmd); // stop command + factory.registerCommand("wait", &commands::CreateWaitCmd); // pause command + factory.registerCommand("select", &commands::CreateSelectCmd); // select command + factory.registerCommand("load", &commands::CreateLoadCmd); // load command + factory.registerCommand("set", &commands::CreateSetCmd); // set command // New commands later } } // namespace commands \ No newline at end of file From 21df0a59c8815d2c49d67674849170775e933325 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 07:57:47 +0100 Subject: [PATCH 100/114] feat: Added support for `setvelocity` command in identifier checks --- DSFE_App/DSFE_Core/src/DSL/Parser.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/DSFE_App/DSFE_Core/src/DSL/Parser.cpp b/DSFE_App/DSFE_Core/src/DSL/Parser.cpp index ebae5ab3..03b1aa4a 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Parser.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Parser.cpp @@ -31,6 +31,7 @@ namespace dsl { s == "set" || s == "select" || s == "setomega" || + s == "setvelocity" || s == "load"; } From 279337453cc523ce981b095227e4e282f4e7e9a0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 07:57:57 +0100 Subject: [PATCH 101/114] feat: Added `parseSpatialMask` function to interpret spatial masks for angular and linear axes --- DSFE_App/DSFE_Core/src/DSL/Utils.cpp | 32 ++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/DSFE_App/DSFE_Core/src/DSL/Utils.cpp b/DSFE_App/DSFE_Core/src/DSL/Utils.cpp index b6dfda82..14749432 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Utils.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Utils.cpp @@ -199,6 +199,38 @@ namespace utils { } return mask; } + + std::vector parseSpatialMask(const std::string& args) { + std::string s = stripBraces(args); + + AxisMask angular_mask{}; + AxisMask linear_mask{}; + for (char c : s) { + char c_lower = static_cast(std::tolower(static_cast(c))); + switch (c) { + case 'wx': angular_mask.x = true; break; + case 'wy': angular_mask.y = true; break; + case 'wz': angular_mask.z = true; break; + case 'vx': linear_mask.x = true; break; + case 'vy': linear_mask.y = true; break; + case 'vz': linear_mask.z = true; break; + default: break; + } + } + + if (!angular_mask.any() && !linear_mask.any()) { + D_WARN("No valid axes found in spatial mask: %s. Defaulting to angular Z axis.", s.c_str()); + angular_mask.z = true; + } + std::vector spatial_mask; + spatial_mask.push_back(angular_mask); + spatial_mask.push_back(linear_mask); + SIM_RUNTIME("Parsed spatial mask: {wx: %d, wy: %d, wz: %d, vx: %d, vy: %d, vz: %d}", + angular_mask.x, angular_mask.y, angular_mask.z, + linear_mask.x, linear_mask.y, linear_mask.z + ); + return spatial_mask; + } // Helper functions to convert between degrees and radians double degToRad(double degrees) { return degrees * ( PI_d / 180.0); } From ed8504a492332c3f7f614ec4729eb6e82e541b30 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 09:13:02 +0100 Subject: [PATCH 102/114] refactor: Renamed `omega` to `velocity` and update SetTarget structure for clarity --- .../DSFE_Core/include/DSL/Commands/SetCmd.h | 7 +-- .../DSFE_Core/src/DSL/Commands/SetCmd.cpp | 51 +++++++++++-------- 2 files changed, 35 insertions(+), 23 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h index 76d3fb8f..09ac2b3c 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h @@ -19,7 +19,7 @@ namespace commands { enum class SetTargetType { IntegratorMethod, - Omega, + Velocity, FixedDt, Gravity }; @@ -27,9 +27,10 @@ namespace commands { struct DSFE_API SetTarget { SetTargetType type = SetTargetType::IntegratorMethod; IntegratorMethod method = IntegratorMethod::RK4; // Default method - mathlib::Vec3 omega{ 0.0, 0.0, 0.0 }; + mathlib::Vec3 angular_vel{0.0, 0.0, 0.0}; // For Angular Velocity + mathlib::Vec3 linear_vel{0.0, 0.0, 0.0}; // For Linear Velocity double fixedDt = 0.0; - double gravity = 0.0; + mathlib::Vec3 gravity{0.0, 0.0, 0.0}; // Default gravity vector }; // Class representing the SET command diff --git a/DSFE_App/DSFE_Core/src/DSL/Commands/SetCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetCmd.cpp index 502d6617..8f8c22fa 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Commands/SetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/SetCmd.cpp @@ -45,23 +45,33 @@ namespace commands { // --- SetCmd Method Implementations --- // Helper function to parse LoadTarget from string - // Expected formats: "set(integrator,)", "set(colour,)", "set(colour,)" + // Expected formats: "set(integrator,)", "set(dt,)", "set(gravity,)", "set(vel,,,,,,)" static std::optional parseSetTarget(const std::string& id, const std::string& token) { if (startsWith(toLower(id), "integrator")) { std::string s = toLower(token); return SetTarget{ SetTargetType::IntegratorMethod, parseMethod(s) }; } - if (startsWith(toLower(id), "dt")) { std::string s = token; return SetTarget{ SetTargetType::FixedDt, {}, {}, utils::parseDouble(s) }; } - if (startsWith(toLower(id), "gravity")) { std::string s = token; return SetTarget{ SetTargetType::Gravity, {}, {}, {}, utils::parseDouble(s)}; } - if (startsWith(toLower(id), "velocity") || startsWith(toLower(id), "omega")) { + if (startsWith(toLower(id), "dt") || startsWith(toLower(id), "timestep")) { std::string s = token; return SetTarget{ SetTargetType::FixedDt, {}, {}, {}, utils::parseDouble(s) }; } + if (startsWith(toLower(id), "gravity") || startsWith(toLower(id), "g") || startsWith(toLower(id), "grav")) { + std::string s = toLower(token); + if (s.empty()) { D_WARN("set(gravity, /, , ) expects either 1 or 3 arguments."); return std::nullopt; } + if (s.find(',') != std::string::npos) { + mathlib::Vec3 g = utils::parseVec3(s); + return SetTarget{ SetTargetType::Gravity, {}, {}, {}, {}, g }; + } + mathlib::Vec3 g = mathlib::Vec3(0.0, 0.0, utils::parseDouble(s)); + return SetTarget{ SetTargetType::Gravity, {}, {}, {}, {}, g }; + } + if (startsWith(toLower(id), "velocity") || startsWith(toLower(id), "vel")) { std::string s = toLower(token); - - AxisMask m = utils::parseAxisMask(s); - if (!m.any()) { return std::nullopt; } - - Vec3 w = Vec3{ m.x ? utils::parseFloat(s) : 0.0f, - m.y ? utils::parseFloat(s) : 0.0f, - m.z ? utils::parseFloat(s) : 0.0f - }; - - return SetTarget{ SetTargetType::Omega, {}, w }; + if (s.empty()) { D_WARN("set(velocity/vel, , , , , , ) expects 6 arguments."); return std::nullopt; } + std::vector m_vec = parseSpatialMask(s); + if (m_vec.size() != 2) { D_WARN("set(velocity/vel, , , , , , ) expects 6 arguments."); return std::nullopt; } + mathlib::VecX sv = mathlib::VecX::Zero(6); + sv << (m_vec[0].x ? utils::parseDouble(s) : 0.0), + (m_vec[0].y ? utils::parseDouble(s) : 0.0), + (m_vec[0].z ? utils::parseDouble(s) : 0.0), + (m_vec[1].x ? utils::parseDouble(s) : 0.0), + (m_vec[1].y ? utils::parseDouble(s) : 0.0), + (m_vec[1].z ? utils::parseDouble(s) : 0.0); + return SetTarget{ SetTargetType::Velocity, {}, mathlib::Vec3(sv[0], sv[1], sv[2]), mathlib::Vec3(sv[3], sv[4], sv[5])}; } return std::nullopt; } @@ -90,24 +100,25 @@ namespace commands { D_SUCCESS("set() command executed: Integrator method set."); return; } - if (_id == "omega") { + if (_id == "velocity" || _id == "vel" || _id == "v") { auto t = parseSetTarget(_id, _tokens); - if (!t) { markFailed("Invalid omega"); return; } - getProgram()->setOmega(t->omega, AngularUnits::DegPerSec); + if (!t) { markFailed("Invalid velocity"); return; } + getProgram()->setVelocity(t->angular_vel, t->linear_vel); // Doesnt actually do anything right now markCompleted(); + D_SUCCESS("set() command executed: Velocity set."); return; } - if (_id == "dt") { + if (_id == "dt" || _id == "timestep" || _id == "fixed_dt") { auto t = parseSetTarget(_id, _tokens); if (!t) { markFailed("Invalid fixed_dt"); return; } getProgram()->setFixedDt(t->fixedDt); markCompleted(); return; } - if (_id == "gravity") { + if (_id == "gravity" || _id == "g" || _id == "grav") { auto t = parseSetTarget(_id, _tokens); if (!t) { markFailed("Invalid gravity"); return; } - getProgram()->setGravity(t->gravity); + getProgram()->setGravityVec(t->gravity); markCompleted(); return; } From a73dbb1ea7fd4f34a3a0a4384dc8cfe7c99173ad Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 09:13:18 +0100 Subject: [PATCH 103/114] refactor: Renamed `setOmega` to `setAngularVel` and add `setVelocity` for improved clarity --- .../DSFE_Core/include/DSL/IStoredProgram.h | 7 ++-- .../DSFE_Core/include/DSL/StoredProgram.h | 10 +++--- DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp | 34 +++++++++++++------ 3 files changed, 34 insertions(+), 17 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h index a9b038ab..646b7a7d 100644 --- a/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h +++ b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h @@ -70,8 +70,9 @@ namespace dsl { virtual void setIntegratorMethod(IntegratorMethod method) = 0; virtual IntegratorMethod getIntegratorMethod() const = 0; - // Set & Get Omega - virtual void setOmega(mathlib::Vec3 omega, utils::AngularUnits units) = 0; + // Setting of Velocities + virtual void setAngularVel(mathlib::Vec3 wv, utils::AngularUnits units) = 0; + virtual void setVelocity(mathlib::Vec3 wv, mathlib::Vec3 lv) = 0; // Set & Get Fixed Dt virtual void setFixedDt(double dt) = 0; @@ -80,5 +81,7 @@ namespace dsl { // Set & Get Gravity virtual void setGravity(double g) = 0; virtual double getGravity() const = 0; + virtual void setGravityVec(mathlib::Vec3 g) = 0; + virtual mathlib::Vec3 getGravityVec() const = 0; }; } // namespace interpreter \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h b/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h index cdca22af..367244f0 100644 --- a/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h +++ b/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h @@ -64,8 +64,9 @@ namespace dsl { void setIntegratorMethod(IntegratorMethod method) override; IntegratorMethod getIntegratorMethod() const override; - // Set & Get Omega - void setOmega(mathlib::Vec3 omega, utils::AngularUnits units) override; + // Setting of Velocities + void setAngularVel(mathlib::Vec3 wv, utils::AngularUnits units) override; + void setVelocity(mathlib::Vec3 wv, mathlib::Vec3 lv) override; // Set & Get Fixed Dt void setFixedDt(double dt) override; @@ -74,6 +75,8 @@ namespace dsl { // Set & Get Gravity void setGravity(double gravity) override; double getGravity() const override; + void setGravityVec(mathlib::Vec3 g) override; + mathlib::Vec3 getGravityVec() const override; private: core::ISimulationCore* _core = nullptr; @@ -94,8 +97,7 @@ namespace dsl { std::vector> _commands; IntegratorMethod _integratorMethod = IntegratorMethod::RK4; // Default integrator method - double _gravity = 0.0; + mathlib::Vec3 _gravity = mathlib::Vec3::Zero(); double _dt = 0.0; - mathlib::Vec3 _rgb = mathlib::Vec3{ 1.0f, 0.0f, 0.0f }; }; } // namespace interpreter diff --git a/DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp b/DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp index dd0a5f12..511f8482 100644 --- a/DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp @@ -154,7 +154,8 @@ namespace dsl { if (_core) { if (_core->hasRigidBody()) { auto& rs = _core->rigidBodySystem(); - if (method == IntegratorMethod::AD_ImplicitEuler || method == IntegratorMethod::AD_ImplicitMidpoint || method == IntegratorMethod::AD_GLRK2 || method == IntegratorMethod::AD_GLRK3) { + std::vector adMethods = { IntegratorMethod::AD_ImplicitEuler, IntegratorMethod::AD_ImplicitMidpoint, IntegratorMethod::AD_GLRK2, IntegratorMethod::AD_GLRK3 }; + if (std::find(adMethods.begin(), adMethods.end(), method) != adMethods.end()) { _core->setADIntegrationMethod(static_cast(method)); } else { @@ -168,10 +169,14 @@ namespace dsl { IntegratorMethod StoredProgram::getIntegratorMethod() const { return _integratorMethod; } // Set Omega - void StoredProgram::setOmega(mathlib::Vec3 omega, utils::AngularUnits units) { + void StoredProgram::setAngularVel(mathlib::Vec3 wv, utils::AngularUnits units) { _cntx.motion().setAngularUnits(units); } + void StoredProgram::setVelocity(mathlib::Vec3 wv, mathlib::Vec3 lv) { + /* Not added yet, may remove this function if not needed */ + } + // Set Fixed Dt void StoredProgram::setFixedDt(double dt) { _dt = dt; if (_core) { _core->setFixedDt(dt); } } // Get Fixed Dt @@ -182,21 +187,28 @@ namespace dsl { // Set Gravity void StoredProgram::setGravity(double gravity) { - _gravity = gravity; + _gravity.z() = gravity; if (_core) { - if (_core->hasRigidBody()) { - auto& rs = _core->rigidBodySystem(); - rs.setGravity(gravity); - } + if (_core->hasRigidBody()) { auto& rs = _core->rigidBodySystem(); rs.setGravity(gravity); } } } // Get Gravity double StoredProgram::getGravity() const { if (_core) { - if (_core->hasRigidBody()) { - const auto& rs = _core->rigidBodySystem(); - return rs.getGravity(); - } + if (_core->hasRigidBody()) { const auto& rs = _core->rigidBodySystem(); return rs.getGravity(); } + } + return _gravity.z(); + } + // Set Gravity Vector + void StoredProgram::setGravityVec(mathlib::Vec3 g) { + if (_core) { + if (_core->hasRigidBody()) { auto& rs = _core->rigidBodySystem(); rs.setGravityVec(g); } + } + } + // Get Gravity Vector + mathlib::Vec3 StoredProgram::getGravityVec() const { + if (_core) { + if (_core->hasRigidBody()) { const auto& rs = _core->rigidBodySystem(); return rs.getGravityVec(); } } return _gravity; } From fb35214fce7e9a58ffc780cb87312cbf8c224557 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 09:13:42 +0100 Subject: [PATCH 104/114] feat: Added `parseSpatialMask` function to enhance spatial mask interpretation --- DSFE_App/DSFE_Core/include/DSL/Utils.h | 1 + 1 file changed, 1 insertion(+) diff --git a/DSFE_App/DSFE_Core/include/DSL/Utils.h b/DSFE_App/DSFE_Core/include/DSL/Utils.h index 335a2be9..e7b2517f 100644 --- a/DSFE_App/DSFE_Core/include/DSL/Utils.h +++ b/DSFE_App/DSFE_Core/include/DSL/Utils.h @@ -60,6 +60,7 @@ namespace utils { float parseFloat(const std::string s); mathlib::Vec3 parseVec3(const std::string& str); AxisMask parseAxisMask(const std::string& s); + std::vector parseSpatialMask(const std::string& s); //bool tryParseObjID(const std::string& s, scene::ObjectID& out); // --- Unit Conversion Utilities --- From bf896df410774950631bb010ab3868dc6c2f6a48 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 09:16:43 +0100 Subject: [PATCH 105/114] refactor: Renamed `stopAllOmega` to `stopAllVel` for consistency in terminology --- DSFE_App/DSFE_Core/include/DSL/CommandContext.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/DSL/CommandContext.h b/DSFE_App/DSFE_Core/include/DSL/CommandContext.h index 8cd4ed0b..ccd4bee7 100644 --- a/DSFE_App/DSFE_Core/include/DSL/CommandContext.h +++ b/DSFE_App/DSFE_Core/include/DSL/CommandContext.h @@ -53,7 +53,7 @@ namespace commands { double getOmegaClamp() const; // Stops all angular velocity for the body - utils::OpResult stopAllOmega(); // stops all angular velocity + utils::OpResult stopAllVel(); // stops all angular velocity utils::OpResult setJointOmega(const std::string& childLink, double omegaDegPerSec); // deg/s utils::OpResult stopJointOmega(const std::string& childLink); From 9b0724ab77ef3a6c5d3cb98a88a8516331228669 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 09:58:34 +0100 Subject: [PATCH 106/114] fixex: Updated `computeArticulatedBodies_ABA` to add missing continue, and `computerAccelerations_ABA` to resize qdd using a DOF-Index rather than a joint index --- .../DSFE_Core/include/Physics/SpatialDynamics.inl | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 49967b5c..6c7af317 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -259,7 +259,9 @@ namespace physics { } // For free joints, store the articulated body inertia and bias force in the dblk and ublk scratch buffers if (j.nfDOF == 6) { - dblk_out[i] = IA_out[i]; ublk_out[i] = tau.segment(off[i], 6) - pA_out[i].v; + dblk_out[i] = IA_out[i]; + ublk_out[i] = tau.segment(off[i], 6) - pA_out[i].v; + continue; } // Compute articulated body inertia and bias force for the current joint U_out[i] = IA_out[i] * j.S; @@ -292,9 +294,11 @@ namespace physics { mathlib::VecX_T& qdd_out ) { const size_t n = model.joints.size(); + int nv = 0; + for (const auto& jj : model.joints) { nv += jj.nfDOF; } // Resize output buffers a_out.resize(n); - qdd_out.resize(n); + qdd_out.resize(nv); // Compute offsets for joint degrees of freedom std::vector off(n); { @@ -312,11 +316,6 @@ namespace physics { mathlib::VecX_T a_prop = a_out[i].v; mathlib::VecX_T rhs = ublk[i] - dblk[i] * a_prop; mathlib::VecX_T qdd_blk = dblk[i].ldlt().solve(rhs); // Solve for joint accelerations using the articulated body inertia matrix - if constexpr (std::is_same_v) { - LOG_INFO_ONCE("free a_prop=[%.3f %.3f %.3f | %.3f %.3f %.3f] qdd=[%.3f %.3f %.3f | %.3f %.3f %.3f]", - a_prop(0),a_prop(1),a_prop(2),a_prop(3),a_prop(4),a_prop(5), - qdd_blk(0),qdd_blk(1),qdd_blk(2),qdd_blk(3),qdd_blk(4),qdd_blk(5)); - } qdd_out.segment(off[i], 6) = qdd_blk; a_out[i].v += qdd_blk; continue; @@ -338,7 +337,6 @@ namespace physics { int nv = 0; for (const auto& j : model.joints) { nv += j.nfDOF; } mathlib::VecX_T qdd = mathlib::VecX_T::Zero(nv); - mathlib::SpatialVec_T a0; // base acceleration (gravity) a0.v << scratch.spatial.g.template segment<3>(0), @@ -374,7 +372,6 @@ namespace physics { a0, scratch.spatial.dblk, scratch.spatial.ublk, scratch.spatial.a, qdd ); - return qdd; // [rad/s^2], joint accelerations computed using the Articulated Body Algorithm (ABA) } } // namespace physics \ No newline at end of file From d90dfb957c2bbc1098f278654a585a9cd80c04c0 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:23:35 +0100 Subject: [PATCH 107/114] feat: Added "setVelocity" command to syntax highlighter for enhanced command recognition --- DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp index f2e5bace..57a16af0 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/DSL/DSLSyntaxHighlighter.cpp @@ -21,7 +21,8 @@ namespace widgets { "rotateJointTo", "rotateJointBy", "parallel", - "set" + "set", + "setVelocity" }; for (const auto& cmd : commands) { _rules.push_back({ QRegularExpression("\\b" + cmd + "\\b"), cmdFmt }); } From 1d5a0f1a1d7c32622b347aa65bfdc12f5ec40a90 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:25:04 +0100 Subject: [PATCH 108/114] refactor: Reordered parameters in SetVelocityCmd constructor and update method for alignment with rest of code * I did some research and realised most engines use linear then angular in storage, I want to make it as easy as possible for anyone to use DSL so I refactored it. * Also stupid oversight on my part, it was causing bad transformations. --- .../src/DSL/Commands/SetVelocityCmd.cpp | 27 ++++++++----------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp index fd31e9eb..885a5a53 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp @@ -18,15 +18,12 @@ namespace commands { bool SetVelocityCmd::hasStarted() const { return _started; } // Constructor - SetVelocityCmd::SetVelocityCmd(std::string link, double wx, double wy, double wz, double vx, double vy, double vz) - : _link(std::move(link)), _wx(wx), _wy(wy), _wz(wz), _vx(vx), _vy(vy), _vz(vz) { + SetVelocityCmd::SetVelocityCmd(std::string link, double vx, double vy, double vz, double wx, double wy, double wz) + : _link(std::move(link)), _vx(wx), _vy(wy), _vz(wz), _wx(vx), _wy(vy), _wz(vz) { _result = { CmdState::NotStarted, {}, "" }; } CmdResult SetVelocityCmd::update(CommandContext& cntx, double dt) { - if (_done) { return { CmdState::Executed, {}, "setVelocity completed" }; } - if (!_started) { return { CmdState::NotStarted, {}, "setVelocity not started" }; } - auto* core = cntx.Core(); if (!core) { SIM_FAIL("setVelocity: SimulationCore is null."); @@ -36,15 +33,13 @@ namespace commands { auto& body = cntx.RigidBody(); mathlib::VecX v = mathlib::VecX::Zero(6); - v << _wx, _wy, _wz, _vx, _vy, _vz; // [Angular Velocity (rad/s), Linear Velocity (m/s)] + v << _vx, _vy, _vz, _wx, _wy, _wz; // [Angular Velocity (rad/s), Linear Velocity (m/s)] if (!body.trySetFreeVelocity(_link, v)) { SIM_FAIL("setVelocity: '%s' is not a free body.", _link.c_str()); markFailed("setVelocity: target is not a free body."); return { CmdState::Failed, {}, "setVelocity failed" }; } - - _done = true; markCompleted(); return { CmdState::Executed, {}, "setVelocity executed" }; } @@ -61,17 +56,17 @@ namespace commands { return nullptr; } if (args.size() != 6) { - D_FAIL("setVelocity expects 6 args: wx, wy, wz, vx, vy, vz."); + D_FAIL("setVelocity expects 6 args: vx, vy, vz, wx, wy, wz."); return nullptr; } const std::string link = id; - double wx = utils::parseDouble(args[0]); - double wy = utils::parseDouble(args[1]); - double wz = utils::parseDouble(args[2]); - double vx = utils::parseDouble(args[3]); - double vy = utils::parseDouble(args[4]); - double vz = utils::parseDouble(args[5]); + double vx = utils::parseDouble(args[0]); + double vy = utils::parseDouble(args[1]); + double vz = utils::parseDouble(args[2]); + double wx = utils::parseDouble(args[3]); + double wy = utils::parseDouble(args[4]); + double wz = utils::parseDouble(args[5]); - return std::make_unique(link, wx, wy, wz, vx, vy, vz); + return std::make_unique(link, vx, vy, vz, wx, wy, wz); } } // namespace commands \ No newline at end of file From 7638b69aecfb90fc4416722d753730975b540061 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:25:14 +0100 Subject: [PATCH 109/114] fixes: Adjusted acceleration computation for free joints and added logging for debugging --- .../DSFE_Core/include/Physics/SpatialDynamics.inl | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 6c7af317..52436f49 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -309,13 +309,24 @@ namespace physics { for (size_t i = 0; i < n; ++i) { const systems::SpatialJoint& j = model.joints[i]; - if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } + if (j.parent < 0) { + if (j.nfDOF == 6) { a_out[i] = Xup[i] * (a0 * Scalar(-1)) + c[i]; } // For free joints, negate the base acceleration to account for the free motion of the base link + else { a_out[i] = Xup[i] * a0 + c[i]; } // For non-free joints, use the base acceleration directly + } else { a_out[i] = Xup[i] * a_out[j.parent] + c[i]; } if (j.nfDOF == 0) { continue; } // Skip fixed joints if (j.nfDOF == 6) { mathlib::VecX_T a_prop = a_out[i].v; mathlib::VecX_T rhs = ublk[i] - dblk[i] * a_prop; mathlib::VecX_T qdd_blk = dblk[i].ldlt().solve(rhs); // Solve for joint accelerations using the articulated body inertia matrix + if constexpr (std::is_same_v) { + static int s_freeAcc = 0; + if (++s_freeAcc % 500 == 0) { + LOG_INFO("free ABA: a_prop=[%.4f %.4f %.4f | %.4f %.4f %.4f] qdd=[%.4f %.4f %.4f | %.4f %.4f %.4f]", + a_prop(0), a_prop(1), a_prop(2), a_prop(3), a_prop(4), a_prop(5), + qdd_blk(0), qdd_blk(1), qdd_blk(2), qdd_blk(3), qdd_blk(4), qdd_blk(5)); + } + } qdd_out.segment(off[i], 6) = qdd_blk; a_out[i].v += qdd_blk; continue; From df88c9767c8ee7e7f0783d0ee30af8ab82ce3c32 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:25:20 +0100 Subject: [PATCH 110/114] fixes: Corrected order of angular and linear mask parsing in parseSpatialMask function --- DSFE_App/DSFE_Core/src/DSL/Utils.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/DSL/Utils.cpp b/DSFE_App/DSFE_Core/src/DSL/Utils.cpp index 14749432..755830dd 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Utils.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Utils.cpp @@ -203,17 +203,17 @@ namespace utils { std::vector parseSpatialMask(const std::string& args) { std::string s = stripBraces(args); - AxisMask angular_mask{}; AxisMask linear_mask{}; + AxisMask angular_mask{}; for (char c : s) { char c_lower = static_cast(std::tolower(static_cast(c))); switch (c) { - case 'wx': angular_mask.x = true; break; - case 'wy': angular_mask.y = true; break; - case 'wz': angular_mask.z = true; break; case 'vx': linear_mask.x = true; break; case 'vy': linear_mask.y = true; break; case 'vz': linear_mask.z = true; break; + case 'wx': angular_mask.x = true; break; + case 'wy': angular_mask.y = true; break; + case 'wz': angular_mask.z = true; break; default: break; } } @@ -223,11 +223,11 @@ namespace utils { angular_mask.z = true; } std::vector spatial_mask; - spatial_mask.push_back(angular_mask); spatial_mask.push_back(linear_mask); - SIM_RUNTIME("Parsed spatial mask: {wx: %d, wy: %d, wz: %d, vx: %d, vy: %d, vz: %d}", - angular_mask.x, angular_mask.y, angular_mask.z, - linear_mask.x, linear_mask.y, linear_mask.z + spatial_mask.push_back(angular_mask); + SIM_RUNTIME("Parsed spatial mask: {vx: %d, vy: %d, vz: %d, wx: %d, wy: %d, wz: %d}", + linear_mask.x, linear_mask.y, linear_mask.z, + angular_mask.x, angular_mask.y, angular_mask.z ); return spatial_mask; } From 4e2af4dba4b228e085b04d65e9946bd77b48c94b Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:25:27 +0100 Subject: [PATCH 111/114] refactor: Commented out logging for free joint positions and enhance logging in trySetFreeVelocity method --- .../DSFE_Core/src/Systems/RigidBodySystem.cpp | 37 ++++++++++--------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index c0cdd5db..847dd605 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -421,18 +421,18 @@ namespace systems { clearExtForces(); unpackState(result.stepOut.x_next); - { - static int s_freeLogCount = 0; - const bool logNow = (++s_freeLogCount % 60 == 0); - for (const auto& j : _body.joints) { - if (j.type == eJointType::FREE && logNow) { - LOG_INFO("free pos=(%.4f %.4f %.4f) w=(%.5f %.5f %.5f) v=(%.5f %.5f %.5f)", - j.free_pos.x(), j.free_pos.y(), j.free_pos.z(), - j.free_vel(0), j.free_vel(1), j.free_vel(2), // angular (the NaN one) - j.free_vel(3), j.free_vel(4), j.free_vel(5)); // linear - } - } - } + // { + // static int s_freeLogCount = 0; + // const bool logNow = (++s_freeLogCount % 60 == 0); + // for (const auto& j : _body.joints) { + // if (j.type == eJointType::FREE && logNow) { + // LOG_INFO("free pos=(%.4f %.4f %.4f) w=(%.5f %.5f %.5f) v=(%.5f %.5f %.5f)", + // j.free_pos.x(), j.free_pos.y(), j.free_pos.z(), + // j.free_vel(0), j.free_vel(1), j.free_vel(2), // angular (the NaN one) + // j.free_vel(3), j.free_vel(4), j.free_vel(5)); // linear + // } + // } + // } _dynamics->setDt(result.stepOut.dt_taken); const auto scratchCopy = _dynScratch; @@ -810,7 +810,8 @@ namespace systems { } /* - * + * Method to get the free-floating joint velocity of a specific rigidBody joint + * @param linkName: The name of the link associated with the free joint */ bool RigidBodySystem::tryGetFreeVelocity(const std::string& linkName, mathlib::VecX& outVel) const { if (!_hasBody) { return false; } @@ -829,13 +830,15 @@ namespace systems { */ bool RigidBodySystem::trySetFreeVelocity(const std::string& linkName, const mathlib::VecX& vel) { if (!_hasBody) { return false; } - for (auto& joint : _body.joints) { - if (joint.type == eJointType::FREE && joint.child == linkName) { - if (vel.size() != 6) { return false; } - joint.free_vel = vel; + for (auto& j : _body.joints) { + if (j.type == eJointType::FREE && j.child == linkName) { + j.free_vel = vel.head<6>(); + LOG_INFO("Set '%s' free_vel=[%.3f %.3f %.3f %.3f %.3f %.3f]", linkName.c_str(), vel(0),vel(1),vel(2),vel(3),vel(4),vel(5)); return true; } + LOG_INFO("trySetFreeVelocity: joint '%s' is not free or does not match linkName='%s'", j.child.c_str(), linkName.c_str()); } + LOG_WARN("No free joint for link '%s'", linkName.c_str()); return false; } From 40fa04fa854bc81a799ae884e8df208568f62352 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:55:50 +0100 Subject: [PATCH 112/114] refactor: Reverted previous spatialparameter change in SetVelocityCmd constructor for consistency and updated logging format in parseSpatialMask function --- .../src/DSL/Commands/SetVelocityCmd.cpp | 22 +++++++++---------- DSFE_App/DSFE_Core/src/DSL/Utils.cpp | 16 +++++++------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp index 885a5a53..0f5de29c 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp @@ -18,8 +18,8 @@ namespace commands { bool SetVelocityCmd::hasStarted() const { return _started; } // Constructor - SetVelocityCmd::SetVelocityCmd(std::string link, double vx, double vy, double vz, double wx, double wy, double wz) - : _link(std::move(link)), _vx(wx), _vy(wy), _vz(wz), _wx(vx), _wy(vy), _wz(vz) { + SetVelocityCmd::SetVelocityCmd(std::string link, double wx, double wy, double wz, double vx, double vy, double vz) + : _link(std::move(link)), _wx(wx), _wy(wy), _wz(wz), _vx(vx), _vy(vy), _vz(vz) { _result = { CmdState::NotStarted, {}, "" }; } @@ -33,7 +33,7 @@ namespace commands { auto& body = cntx.RigidBody(); mathlib::VecX v = mathlib::VecX::Zero(6); - v << _vx, _vy, _vz, _wx, _wy, _wz; // [Angular Velocity (rad/s), Linear Velocity (m/s)] + v << _wx, _wy, _wz, _vx, _vy, _vz; // [Angular Velocity (rad/s), Linear Velocity (m/s)] if (!body.trySetFreeVelocity(_link, v)) { SIM_FAIL("setVelocity: '%s' is not a free body.", _link.c_str()); @@ -56,17 +56,17 @@ namespace commands { return nullptr; } if (args.size() != 6) { - D_FAIL("setVelocity expects 6 args: vx, vy, vz, wx, wy, wz."); + D_FAIL("setVelocity expects 6 args: wx, wy, wz, vx, vy, vz."); return nullptr; } const std::string link = id; - double vx = utils::parseDouble(args[0]); - double vy = utils::parseDouble(args[1]); - double vz = utils::parseDouble(args[2]); - double wx = utils::parseDouble(args[3]); - double wy = utils::parseDouble(args[4]); - double wz = utils::parseDouble(args[5]); + double wx = utils::parseDouble(args[0]); + double wy = utils::parseDouble(args[1]); + double wz = utils::parseDouble(args[2]); + double vx = utils::parseDouble(args[3]); + double vy = utils::parseDouble(args[4]); + double vz = utils::parseDouble(args[5]); - return std::make_unique(link, vx, vy, vz, wx, wy, wz); + return std::make_unique(link, wx, wy, wz, vx, vy, vz); } } // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/DSL/Utils.cpp b/DSFE_App/DSFE_Core/src/DSL/Utils.cpp index 755830dd..14749432 100644 --- a/DSFE_App/DSFE_Core/src/DSL/Utils.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/Utils.cpp @@ -203,17 +203,17 @@ namespace utils { std::vector parseSpatialMask(const std::string& args) { std::string s = stripBraces(args); - AxisMask linear_mask{}; AxisMask angular_mask{}; + AxisMask linear_mask{}; for (char c : s) { char c_lower = static_cast(std::tolower(static_cast(c))); switch (c) { - case 'vx': linear_mask.x = true; break; - case 'vy': linear_mask.y = true; break; - case 'vz': linear_mask.z = true; break; case 'wx': angular_mask.x = true; break; case 'wy': angular_mask.y = true; break; case 'wz': angular_mask.z = true; break; + case 'vx': linear_mask.x = true; break; + case 'vy': linear_mask.y = true; break; + case 'vz': linear_mask.z = true; break; default: break; } } @@ -223,11 +223,11 @@ namespace utils { angular_mask.z = true; } std::vector spatial_mask; - spatial_mask.push_back(linear_mask); spatial_mask.push_back(angular_mask); - SIM_RUNTIME("Parsed spatial mask: {vx: %d, vy: %d, vz: %d, wx: %d, wy: %d, wz: %d}", - linear_mask.x, linear_mask.y, linear_mask.z, - angular_mask.x, angular_mask.y, angular_mask.z + spatial_mask.push_back(linear_mask); + SIM_RUNTIME("Parsed spatial mask: {wx: %d, wy: %d, wz: %d, vx: %d, vy: %d, vz: %d}", + angular_mask.x, angular_mask.y, angular_mask.z, + linear_mask.x, linear_mask.y, linear_mask.z ); return spatial_mask; } From b03c96aa5af466556125f36dc21e1e9a2fa13c7f Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:55:56 +0100 Subject: [PATCH 113/114] fixes: Corrected order of free joint position and rotation vector in computeSpatialKinematicsAndBias function and removed logging for free joint accelerations --- .../DSFE_Core/include/Physics/SpatialDynamics.inl | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 52436f49..245c093c 100644 --- a/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -40,8 +40,8 @@ namespace physics { if (j.type == systems::eJointType::FREE) { if constexpr (std::is_same_v) { - mathlib::Vec3_T pos = q.template segment<3>(off[i]); // Free Joint Position - mathlib::Vec3_T rv = q.template segment<3>(off[i] + 3); // Free Joint Rotation Vector + mathlib::Vec3_T rv = q.template segment<3>(off[i]); // Free Joint Rotation Vector + mathlib::Vec3_T pos = q.template segment<3>(off[i] + 3); // Free Joint Position mathlib::Quat_T q_full = (j.free_qref * expToQuat(rv)).normalized(); // Free Joint Orientation with Reference mathlib::Mat3_T R = q_full.toRotationMatrix(); mathlib::Vec3_T zero_R = mathlib::Vec3_T::Zero(); @@ -319,14 +319,6 @@ namespace physics { mathlib::VecX_T a_prop = a_out[i].v; mathlib::VecX_T rhs = ublk[i] - dblk[i] * a_prop; mathlib::VecX_T qdd_blk = dblk[i].ldlt().solve(rhs); // Solve for joint accelerations using the articulated body inertia matrix - if constexpr (std::is_same_v) { - static int s_freeAcc = 0; - if (++s_freeAcc % 500 == 0) { - LOG_INFO("free ABA: a_prop=[%.4f %.4f %.4f | %.4f %.4f %.4f] qdd=[%.4f %.4f %.4f | %.4f %.4f %.4f]", - a_prop(0), a_prop(1), a_prop(2), a_prop(3), a_prop(4), a_prop(5), - qdd_blk(0), qdd_blk(1), qdd_blk(2), qdd_blk(3), qdd_blk(4), qdd_blk(5)); - } - } qdd_out.segment(off[i], 6) = qdd_blk; a_out[i].v += qdd_blk; continue; From e44d14a28d383c020a7f6519caf5ef7f6dc42541 Mon Sep 17 00:00:00 2001 From: saltyjoss Date: Sat, 1 Aug 2026 11:56:05 +0100 Subject: [PATCH 114/114] fixes: Corrected order of free joint position and rotation vector in RigidBodySystem methods --- DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp index 847dd605..55f65ca7 100644 --- a/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -193,9 +193,9 @@ namespace systems { x[off + nv] = j.qd; } else { - x.segment(off, 3) = j.free_pos; // Position - x.segment(off + 3, 3) = j.free_rot_v; // Euler angles - x.segment(nv + off, 6) = j.free_vel; // linear + angular velocity + x.segment(off, 3) = j.free_rot_v; // Euler angles + x.segment(off + 3, 3) = j.free_pos; // Position + x.segment(nv + off, 6) = j.free_vel; // linear + angular velocity } off += dof; // increment offset by the DOF of the joint } @@ -250,9 +250,9 @@ namespace systems { j.qd = omega_out; } else { - j.free_pos = x.segment(off, 3); // Position - mathlib::Vec3 rot_v = x.segment(off + 3, 3); // Euler angles - j.free_vel = x.segment(nv + off, 6); // linear + angular velocity + mathlib::Vec3 rot_v = x.segment(off, 3); // Euler angles + j.free_pos = x.segment(off + 3, 3); // Position + j.free_vel = x.segment(nv + off, 6); // linear + angular velocity j.free_qref = (j.free_qref * expToQuat(rot_v)).normalized(); // Update quaternion based on Euler angles j.free_rot_v = mathlib::Vec3::Zero(); // Reset Euler angles to zero after conversion _clampTheta[i] = 0;