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) diff --git a/DSFE_App/DSFE_Core/CMakeLists.txt b/DSFE_App/DSFE_Core/CMakeLists.txt index 947efe43..a43baa12 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) @@ -32,17 +48,22 @@ set(DEP_CORE_SRC src/Numerics/IntegrationService.cpp ) -set(SINGLE_BODY_SYS_SRC +set(LOADING_SRC + src/Systems/RigidBodyLoader.cpp + src/Systems/RigidBodyLoaderJSON.cpp + src/Systems/RigidBodyLoaderURDF.cpp +) + +set(SYSTEMS_SRC + src/Systems/RigidBodySystem.cpp + src/Systems/TrajectoryManager.cpp + src/Systems/RigidBodySnapshot.cpp 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(PHYSICS_SRC + src/Physics/RigidBodyDynamics.cpp + src/Physics/RigidBodyKinematics.cpp ) set(PLATFORM_SRC @@ -54,30 +75,31 @@ 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/SetVelocityCmd.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) @@ -85,8 +107,9 @@ 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} - ${MULTI_BODY_SYS_SRC} + ${SYSTEMS_SRC} + ${LOADING_SRC} + ${PHYSICS_SRC} ${PLATFORM_SRC} ${DSL_SRC} ${SIM_CORE_SRC} diff --git a/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h b/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h index 793d4406..6f53de4a 100644 --- a/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h +++ b/DSFE_App/DSFE_Core/include/Analysis/MetricLogger.h @@ -1,9 +1,13 @@ +/* + * File: Analysis/MetricLogger.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #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/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; diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Command.h b/DSFE_App/DSFE_Core/include/DSL/Command.h similarity index 55% rename from DSFE_App/DSFE_Core/include/Interpreter/Command.h rename to DSFE_App/DSFE_Core/include/DSL/Command.h index 6b223879..4a626e94 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Command.h +++ b/DSFE_App/DSFE_Core/include/DSL/Command.h @@ -1,10 +1,11 @@ -// DSFE_Core Command.h +/* + * File: DSL/Command.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "ICommand.h" -#include "MainContext.h" - -using namespace interpreter; +#include "DSL/ICommand.h" +#include "DSL/MainContext.h" namespace commands { // Class representing a generic command @@ -13,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; @@ -21,11 +22,11 @@ namespace commands { void markCompleted() override; bool hasStarted() const override; - interpreter::IStoredProgram* getProgram() const override { return _program; } - void setProgram(interpreter::IStoredProgram* program) override { _program = program; } + dsl::IStoredProgram* getProgram() const override { return _program; } + void setProgram(dsl::IStoredProgram* program) override { _program = program; } protected: - IStoredProgram* _program = nullptr; + dsl::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/DSL/CommandContext.h similarity index 90% rename from DSFE_App/DSFE_Core/include/Interpreter/CommandContext.h rename to DSFE_App/DSFE_Core/include/DSL/CommandContext.h index dd85661e..ccd4bee7 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/CommandContext.h +++ b/DSFE_App/DSFE_Core/include/DSL/CommandContext.h @@ -1,9 +1,12 @@ -// DSFE_Core CommandContext.h +/* + * File: DSL/CommandContext.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "SimFwd.h" -#include "Interpreter/Utils.h" +#include "DSL/SimFwd.h" +#include "DSL/Utils.h" #include "Platform/Logger.h" @@ -35,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 --- @@ -50,15 +52,15 @@ namespace commands { // Gets the current omega clamp value double getOmegaClamp() const; - // Stops all angular velocity for the robot - utils::OpResult stopAllOmega(); // stops all angular velocity + // Stops all angular velocity for the body + 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); // --- HELPER METHODS --- core::ISimulationCore* Core() const; - robots::RobotSystem& Robot() const; + systems::RigidBodySystem& RigidBody() const; // --- ROTATION COMMAND METHODS --- diff --git a/DSFE_App/DSFE_Core/include/Interpreter/CommandFactory.h b/DSFE_App/DSFE_Core/include/DSL/CommandFactory.h similarity index 92% rename from DSFE_App/DSFE_Core/include/Interpreter/CommandFactory.h rename to DSFE_App/DSFE_Core/include/DSL/CommandFactory.h index b4a2dc54..b332a1e3 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/CommandFactory.h +++ b/DSFE_App/DSFE_Core/include/DSL/CommandFactory.h @@ -1,8 +1,11 @@ -// DSFE_Core CommandFactory.h +/* + * File: DSL/CommandFactory.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "ICommand.h" +#include "DSL/ICommand.h" #include #include diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/LoadCmd.h similarity index 67% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/LoadCmd.h index 08fe298d..e3c01b51 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/LoadCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/LoadCmd.h @@ -1,22 +1,24 @@ -// DSFE_Core LoadCmd.h +/* + * File: DSL/Commands/LoadCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { // Enum for load target type enum class LoadTargetType { - SingleBody, - MultiBody + RigidBody }; // Struct for load target struct DSFE_API LoadTarget { - LoadTargetType type = LoadTargetType::SingleBody; + LoadTargetType type = LoadTargetType::RigidBody; std::string path; }; @@ -29,18 +31,18 @@ namespace commands { // 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; } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } // Get current result - program_data::CmdResult currentResult() const override { return getResult(); } + CmdResult currentResult() const override { return getResult(); } // Execute the command void execute() override; private: LoadTarget _target{}; - program_data::CmdResult _result = { CmdState::NotStarted, {}, "" }; + CmdResult _result = { CmdState::NotStarted, {}, "" }; std::string _path; protected: diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/ParallelGroupCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/ParallelGroupCmd.h similarity index 91% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/ParallelGroupCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/ParallelGroupCmd.h index b8f2d169..ea6baae8 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/ParallelGroupCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/ParallelGroupCmd.h @@ -1,11 +1,14 @@ -// DSFE_Core ParallelGroupCmd.h +/* + * File: DSL/Commands/ParallelGroupCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include #include -#include "Interpreter/Command.h" +#include "DSL/Command.h" namespace commands { class DSFE_API ParallelGroupCmd final : public Command { diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h similarity index 65% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h index f53ed1d1..995eaf4a 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateByCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateByCmd.h @@ -1,11 +1,14 @@ -// DSFE_Core RotateByCmd.h +/* + * File: DSL/Commands/RotateByCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { @@ -17,13 +20,13 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; + CmdResult update(CommandContext& cntx, double dt) override; utils::AxisMask _axes{}; double _deltaDeg; @@ -31,7 +34,7 @@ namespace commands { double _totalRotated = 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/Interpreter/Commands/RotateJointByCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointByCmd.h similarity index 74% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointByCmd.h index fa78218d..73cbf795 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointByCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointByCmd.h @@ -1,11 +1,14 @@ -// DSFE_Core RotateJointByCmd.h +/* + * File: DSL/Commands/RotateJointByCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { class DSFE_API RotateJointByCmd final : public Command { @@ -15,13 +18,13 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; + CmdResult update(CommandContext& cntx, double dt) override; std::string _link; double _deltaDeg = 0.0; diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointToCmd.h similarity index 74% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointToCmd.h index 9445eaa6..3685a966 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateJointToCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateJointToCmd.h @@ -1,11 +1,14 @@ -// DSFE_Core RotateJointToCmd.h +/* + * File: DSL/Commands/RotateJointToCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { // Class representing the ROTATE command @@ -16,13 +19,13 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; + CmdResult update(CommandContext& cntx, double dt) override; std::string _link; double _angleDeg; // angle relative to the start position double _maxOmegaDeg; diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateToCmd.h similarity index 69% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/RotateToCmd.h index 8585ff19..aceedcbb 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/RotateToCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/RotateToCmd.h @@ -1,11 +1,14 @@ -// DSFE_Core RotateToCmd.h +/* + * File: DSL/Commands/RotateToCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { @@ -17,13 +20,13 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; + CmdResult update(CommandContext& cntx, double dt) override; utils::AxisMask _axes; double _angleDeg = 0.0; diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SaveCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SaveCmd.h similarity index 74% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/SaveCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/SaveCmd.h index df36d0c8..5ec0e7c1 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SaveCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SaveCmd.h @@ -1,4 +1,7 @@ -// DSFE_Core SaveCmd.h +/* + * File: DSL/Commands/SaveCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -6,8 +9,8 @@ #include #include -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" +#include "DSL/SimFwd.h" +#include "DSL/Command.h" namespace commands { // Types of saves that can be performed by the SaveCmd @@ -30,9 +33,9 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: void execute() override; @@ -43,7 +46,7 @@ namespace commands { bool _integratorName = false; // Whether to include integrator name in the filename 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/Interpreter/Commands/SelectCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h similarity index 63% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/SelectCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h index ec055999..e07969c7 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SelectCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SelectCmd.h @@ -1,4 +1,7 @@ -// DSFE_Core SelectCmd.h +/* + * File: DSL/Commands/SelectCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -6,8 +9,8 @@ #include #include -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" +#include "DSL/SimFwd.h" +#include "DSL/Command.h" namespace commands { class DSFE_API SelectCmd final : public Command { @@ -18,16 +21,15 @@ namespace commands { 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(); } + 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, {}, "" }; + CmdResult _result = { CmdState::NotStarted, {}, "" }; protected: void markFailed(const std::string& message) override; diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h similarity index 69% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h index 9f2c81a7..09ac2b3c 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SetCmd.h @@ -1,4 +1,7 @@ -// DSFE_Core SetCmd.h +/* + * File: DSL/Commands/SetCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -7,8 +10,8 @@ #include #include -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" #include "Platform/Logger.h" @@ -16,7 +19,7 @@ namespace commands { enum class SetTargetType { IntegratorMethod, - Omega, + Velocity, FixedDt, Gravity }; @@ -24,28 +27,23 @@ 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 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(); } - + 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; } @@ -54,15 +52,11 @@ namespace commands { private: SetTarget _target{}; - std::string _id; std::string _tokens; - IntegratorMethod _method = IntegratorMethod::RK4; - 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/Interpreter/Commands/SetOmegaCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SetOmegaCmd.h similarity index 59% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/SetOmegaCmd.h index e4503089..76f8d159 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SetOmegaCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SetOmegaCmd.h @@ -1,12 +1,15 @@ -// DSFE_Core SetOmegaCmd.h +/* + * File: DSL/Commands/SetOmegaCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { class SetOmegaCmd final : public Command { @@ -16,20 +19,18 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: - program_data::CmdResult update(CommandContext& cntx, double dt) override; + CmdResult update(CommandContext& cntx, double dt) override; void execute() override; std::string _link; double _omega; - 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/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/include/Interpreter/Commands/SpinCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h similarity index 69% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h index 3d786c31..e97ff188 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/SpinCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/SpinCmd.h @@ -1,11 +1,14 @@ -// DSFE_Core SpinCmd.h +/* + * 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" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { @@ -17,20 +20,19 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) 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: diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/StartCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/StartCmd.h similarity index 61% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/StartCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/StartCmd.h index ddc11d29..91c72d38 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/StartCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/StartCmd.h @@ -1,4 +1,7 @@ -// DSFE_Core StartCmd.h +/* + * File: DSL/Commands/StartCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -6,9 +9,9 @@ #include #include -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { class DSFE_API StartCmd final : public Command { @@ -19,17 +22,16 @@ namespace commands { 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(); } + 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; - - 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/Interpreter/Commands/StopCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/StopCmd.h similarity index 60% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/StopCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/StopCmd.h index 2a9b8916..36e44a56 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/StopCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/StopCmd.h @@ -1,4 +1,7 @@ -// DSFE_Core StopCmd.h +/* + * File: DSL/Commands/StopCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -6,9 +9,9 @@ #include #include -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { class DSFE_API StopCmd final : public Command { @@ -19,16 +22,15 @@ namespace commands { 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(); } + 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; - - 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/Interpreter/Commands/TrajClearCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajClearCmd.h similarity index 60% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/TrajClearCmd.h index 3f89ed4c..36042656 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajClearCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajClearCmd.h @@ -1,14 +1,16 @@ -// DSFE_Core TrajClearCmd.h +/* + * File: DSL/Commands/TrajClearCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { - class TrajClearCmd final : public Command { public: explicit TrajClearCmd() = default; @@ -16,20 +18,18 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: - program_data::CmdResult update(CommandContext& cntx, double dt) override; + 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, {}, "" }; + CmdResult _result{ CmdState::NotStarted, {}, "" }; protected: void markFailed(const std::string& message) override; diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajSetCmd.h similarity index 64% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/TrajSetCmd.h index 3b667d60..b211b7ee 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/TrajSetCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/TrajSetCmd.h @@ -1,14 +1,16 @@ -// DSFE_Core TrajSetCmd.h +/* + * File: DSL/Commands/TrajSetCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.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); @@ -16,22 +18,20 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: - program_data::CmdResult update(CommandContext& cntx, double dt) override; + 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, {}, "" }; + CmdResult _result{ CmdState::NotStarted, {}, "" }; static std::string upperCopy(std::string s); diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Commands/WaitCmd.h b/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h similarity index 58% rename from DSFE_App/DSFE_Core/include/Interpreter/Commands/WaitCmd.h rename to DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h index afc062a1..d42fd0f9 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Commands/WaitCmd.h +++ b/DSFE_App/DSFE_Core/include/DSL/Commands/WaitCmd.h @@ -1,4 +1,7 @@ -// DSFE_Core WaitCmd.h +/* + * File: DSL/Commands/WaitCmd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -6,9 +9,9 @@ #include #include -#include "Interpreter/SimFwd.h" -#include "Interpreter/Command.h" -#include "Interpreter/CommandContext.h" +#include "DSL/SimFwd.h" +#include "DSL/Command.h" +#include "DSL/CommandContext.h" namespace commands { class DSFE_API WaitCmd final : public Command { @@ -19,20 +22,18 @@ namespace commands { 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(); } + CmdResult getResult() const { return _result; } + void setResult(const CmdResult& result) { _result = result; } + CmdResult currentResult() const override { return getResult(); } private: void execute() override; - program_data::CmdResult update(CommandContext& cntx, double dt) override; + CmdResult update(CommandContext& cntx, double dt) override; 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/Interpreter/ICommand.h b/DSFE_App/DSFE_Core/include/DSL/ICommand.h similarity index 68% rename from DSFE_App/DSFE_Core/include/Interpreter/ICommand.h rename to DSFE_App/DSFE_Core/include/DSL/ICommand.h index ae2c882f..562ad0a0 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/ICommand.h +++ b/DSFE_App/DSFE_Core/include/DSL/ICommand.h @@ -1,11 +1,16 @@ -// DSFE_Core ICommand.h +/* + * File: DSL/ICommand.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "IStoredProgram.h" +#include "DSL/IStoredProgram.h" #include #include +using namespace dsl; + namespace commands { // Forward declaration of ICommand for use in IStoredProgram class CommandContext; @@ -20,16 +25,16 @@ 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; - virtual interpreter::IStoredProgram* getProgram() const = 0; - virtual void setProgram(interpreter::IStoredProgram* program) = 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; diff --git a/DSFE_App/DSFE_Core/include/Interpreter/IStoredProgram.h b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h similarity index 79% rename from DSFE_App/DSFE_Core/include/Interpreter/IStoredProgram.h rename to DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h index 7007dfa1..646b7a7d 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/IStoredProgram.h +++ b/DSFE_App/DSFE_Core/include/DSL/IStoredProgram.h @@ -1,19 +1,20 @@ -// DSFE_Core IStoredProgram.h +/* + * File: DSL/IStoredProgram.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "ProgramData.h" -#include "Interpreter/Utils.h" +#include "DSL/ProgramData.h" +#include "DSL/Utils.h" #include #include // Forward declarations -namespace commands { class DSFE_API ICommand; } -namespace scene { class DSFE_API Object; } +namespace commands { class ICommand; } +namespace scene { class Object; } -using namespace program_data; - -namespace interpreter { +namespace dsl { // IStoredProgram interface class DSFE_API IStoredProgram { public: @@ -69,8 +70,9 @@ namespace interpreter { 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; @@ -79,5 +81,7 @@ namespace interpreter { // 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/Interpreter/MainContext.h b/DSFE_App/DSFE_Core/include/DSL/MainContext.h similarity index 80% rename from DSFE_App/DSFE_Core/include/Interpreter/MainContext.h rename to DSFE_App/DSFE_Core/include/DSL/MainContext.h index e01e8ef3..a2ff018b 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/MainContext.h +++ b/DSFE_App/DSFE_Core/include/DSL/MainContext.h @@ -1,9 +1,12 @@ -// DSFE_Core MainContext.h +/* + * File: DSL/MainContext.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "SimFwd.h" -#include "Interpreter/CommandContext.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) diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Parser.h b/DSFE_App/DSFE_Core/include/DSL/Parser.h similarity index 80% rename from DSFE_App/DSFE_Core/include/Interpreter/Parser.h rename to DSFE_App/DSFE_Core/include/DSL/Parser.h index 9246bd66..81371f5b 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Parser.h +++ b/DSFE_App/DSFE_Core/include/DSL/Parser.h @@ -1,17 +1,20 @@ -// DSFE_Core Parser.h +/* + * File: DSL/Parser.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "ProgramData.h" -#include "IStoredProgram.h" -#include "Token.h" +#include "DSL/ProgramData.h" +#include "DSL/IStoredProgram.h" +#include "DSL/Token.h" #include #include #include #include "Platform/Logger.h" -namespace interpreter { +namespace dsl { // Class representing a parsed command class DSFE_API Parser { public: @@ -20,9 +23,9 @@ namespace interpreter { 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/Interpreter/ProgramData.h b/DSFE_App/DSFE_Core/include/DSL/ProgramData.h similarity index 90% rename from DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h rename to DSFE_App/DSFE_Core/include/DSL/ProgramData.h index 5ac25d7d..dfbafb33 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/ProgramData.h +++ b/DSFE_App/DSFE_Core/include/DSL/ProgramData.h @@ -1,4 +1,7 @@ -// DSFE_Core ProgramData.h +/* + * File: DSL/ProgramData.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include @@ -6,7 +9,7 @@ #include #include -namespace program_data { +namespace dsl { // Struct representing source location struct DSFE_API SrcLocation { std::string filename; // Name of the source file @@ -51,21 +54,6 @@ namespace program_data { 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, diff --git a/DSFE_App/DSFE_Core/include/Interpreter/RegisterCommand.h b/DSFE_App/DSFE_Core/include/DSL/RegisterCommand.h similarity index 58% rename from DSFE_App/DSFE_Core/include/Interpreter/RegisterCommand.h rename to DSFE_App/DSFE_Core/include/DSL/RegisterCommand.h index e2a42dda..c5c469fb 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/RegisterCommand.h +++ b/DSFE_App/DSFE_Core/include/DSL/RegisterCommand.h @@ -1,7 +1,10 @@ -// DSFE_Core RegisterCommand.h +/* + * File: DSL/RegisterCommand.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once -#include "CommandFactory.h" +#include "DSL/CommandFactory.h" namespace commands { // Free function to register all commands diff --git a/DSFE_App/DSFE_Core/include/Interpreter/RunWrapper.h b/DSFE_App/DSFE_Core/include/DSL/RunWrapper.h similarity index 72% rename from DSFE_App/DSFE_Core/include/Interpreter/RunWrapper.h rename to DSFE_App/DSFE_Core/include/DSL/RunWrapper.h index 589bddbb..3cb4d8dd 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/RunWrapper.h +++ b/DSFE_App/DSFE_Core/include/DSL/RunWrapper.h @@ -1,11 +1,14 @@ -// DSFE_Core RunWrapper.h +/* + * File: DSL/RunWrapper.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "Parser.h" -#include "IStoredProgram.h" +#include "DSL/Parser.h" +#include "DSL/IStoredProgram.h" -namespace interpreter { +namespace dsl { // Class that wraps the parsing and storing of a program class DSFE_API RunWrapper { public: diff --git a/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h b/DSFE_App/DSFE_Core/include/DSL/SimFwd.h similarity index 72% rename from DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h rename to DSFE_App/DSFE_Core/include/DSL/SimFwd.h index fbc718fe..91e19228 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/SimFwd.h +++ b/DSFE_App/DSFE_Core/include/DSL/SimFwd.h @@ -1,4 +1,7 @@ -// DSFE_Core SimFwd.h +/* + * File: DSL/SimFwd.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -13,4 +16,4 @@ // Forward declarations for the main classes used in the interpreter namespace core { struct ISimulationCore; } -namespace robots { class RobotSystem; } +namespace systems { class RigidBodySystem; } diff --git a/DSFE_App/DSFE_Core/include/Interpreter/StoredProgram.h b/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h similarity index 84% rename from DSFE_App/DSFE_Core/include/Interpreter/StoredProgram.h rename to DSFE_App/DSFE_Core/include/DSL/StoredProgram.h index 60ce74d2..367244f0 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/StoredProgram.h +++ b/DSFE_App/DSFE_Core/include/DSL/StoredProgram.h @@ -1,13 +1,16 @@ -// DSFE_Core StoredProgram.h +/* + * File: DSL/StoredProgram.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "IStoredProgram.h" -#include "ICommand.h" -#include "Interpreter/MainContext.h" +#include "DSL/IStoredProgram.h" +#include "DSL/ICommand.h" +#include "DSL/MainContext.h" #include #include -namespace interpreter { +namespace dsl { // Class representing a stored program in the interpreter. class DSFE_API StoredProgram : public IStoredProgram { public: @@ -61,8 +64,9 @@ namespace interpreter { 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; @@ -71,6 +75,8 @@ namespace interpreter { // 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; @@ -91,8 +97,7 @@ namespace interpreter { 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/include/Interpreter/Token.h b/DSFE_App/DSFE_Core/include/DSL/Token.h similarity index 84% rename from DSFE_App/DSFE_Core/include/Interpreter/Token.h rename to DSFE_App/DSFE_Core/include/DSL/Token.h index b1991bd7..ba6d5d21 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Token.h +++ b/DSFE_App/DSFE_Core/include/DSL/Token.h @@ -1,10 +1,13 @@ -// DSFE_Core Token.h +/* + * File: DSL/Token.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -namespace interpreter { +namespace dsl { enum class TokenType { Unknown, Comment, diff --git a/DSFE_App/DSFE_Core/include/Interpreter/Utils.h b/DSFE_App/DSFE_Core/include/DSL/Utils.h similarity index 92% rename from DSFE_App/DSFE_Core/include/Interpreter/Utils.h rename to DSFE_App/DSFE_Core/include/DSL/Utils.h index 5c5d1e01..e7b2517f 100644 --- a/DSFE_App/DSFE_Core/include/Interpreter/Utils.h +++ b/DSFE_App/DSFE_Core/include/DSL/Utils.h @@ -1,8 +1,11 @@ -// DSFE_Core Utils.h +/* + * File: DSL/Utils.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "SimFwd.h" +#include "DSL/SimFwd.h" #include #include "Platform/Logger.h" @@ -57,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 --- 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/Robots/DynamicsTypes.h b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h similarity index 85% rename from DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h rename to DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h index 43777486..28cedd2c 100644 --- a/DSFE_App/DSFE_Core/include/Robots/DynamicsTypes.h +++ b/DSFE_App/DSFE_Core/include/Physics/DynamicsTypes.h @@ -1,13 +1,16 @@ -// DSFE_Core DynamicsTypes.h +/* + * File: Physics/DynamicsTypes.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include #include -#include "Robots/RobotMetrics.h" +#include "Systems/RigidBodyMetrics.h" -namespace robots { +namespace physics { // Scratch buffers for dense dynamics template struct DenseDynamicsScratch { @@ -15,6 +18,7 @@ namespace robots { 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; @@ -36,6 +40,7 @@ namespace robots { 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); @@ -52,6 +57,7 @@ namespace robots { 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(); @@ -63,6 +69,7 @@ namespace robots { rhs.resize(0); h.resize(0); tau.resize(0); + tau_g.resize(0); I_eff_controller.resize(0); T_world.clear(); jointWorldPoses.clear(); @@ -77,13 +84,18 @@ namespace robots { 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 + + 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 @@ -110,6 +122,11 @@ namespace robots { a.resize(nJoints); 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); @@ -131,9 +148,14 @@ namespace robots { a.clear(); 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(); @@ -150,7 +172,7 @@ namespace robots { mathlib::VecX_T dxdt; mathlib::VecX_T qdd; - RobotMetrics metrics; + systems::RigidBodyMetrics metrics; void resize(size_t n) { dxdt.resize(2 * n); @@ -173,7 +195,7 @@ namespace robots { 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 @@ -183,4 +205,4 @@ namespace robots { g.resize(0); } }; -} \ No newline at end of file +} // 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/Robots/RobotDynamics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h similarity index 61% rename from DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h rename to DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h index 0de235ba..b73275f4 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.h +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.h @@ -1,18 +1,21 @@ -// DSFE_Core RobotDynamics.h +/* + * File: Physics/RigidBodyDynamics.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include -#include "Robots/DynamicsTypes.h" -#include "Robots/RobotMetrics.h" +#include "Physics/DynamicsTypes.h" +#include "Systems/RigidBodyMetrics.h" -#include "Robots/SpatialDynamics.h" -#include "Robots/SpatialModel.h" -#include "Robots/RobotKinematics.h" -#include "Robots/RobotSimSnapshot.h" +#include "Physics/SpatialDynamics.h" +#include "Systems/SpatialModel.h" +#include "Physics/RigidBodyKinematics.h" +#include "Systems/RigidBodySnapshot.h" -#include "Robots/TrajectoryManager.h" +#include "Systems/TrajectoryManager.h" #include #include @@ -23,124 +26,129 @@ namespace control { class TrajectoryManager; } namespace integration { class IntegrationService; enum class eIntegrationMethod; } -namespace robots { +namespace systems { // Forward declarations - struct RobotLink; - struct RobotJoint; + 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 RobotDynamics { + class DSFE_API RigidBodyDynamics { public: // Constructor - RobotDynamics(); + RigidBodyDynamics(); - // Computes the inertia tensor of a robot link + // Computes the inertia tensor of a body link template - mathlib::Mat3_T computeLinkInertiaTensor(const RobotLink& 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 Scalar computeJointInertiaContribution( - const RobotJoint& joint, - const RobotLink& link, + 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 robot configuration + // Computes the full mass matrix M(q) based on the current state and body configuration template void computeMassMatrix( - const RobotConstModel& robot, + 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 robot configuration + // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and body configuration template mathlib::VecX_T computeCoriolisVector( - const RobotConstModel& robot, + 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 robot configuration + // Computes the gravity torque for a joint based on the current state and body configuration template mathlib::VecX_T computeGravityTorque( - const RobotConstModel& robot, + const systems::RigidBodyConstModel& body, 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 + // Computes the analytical Jacobian matrix J(q) for the body based on the current state and body configuration template void analyticalJacobian( - const RobotConstModel& robot, + 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 robot configuration + // 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 RobotSimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ); template mathlib::VecX_T derivative_spatial( - const robots::SpatialModel& model, + const systems::SpatialModel& model, Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ); template void jacobian_spatial( - const robots::SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, + 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 robot configurations + // 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 RobotSimSnapshot_T& snap, + 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 robot configuration + // 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 RobotSimSnapshot_T& snap, + 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 robot system - void setGravity(double gravity) { _gravity = gravity; } - const double getGravity() const { return _gravity; } + // Set the gravity strength for the body system + 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; } @@ -148,21 +156,20 @@ namespace robots { private: // References and pointers - std::unique_ptr _kinematics = nullptr; + 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 - double _gravity{ 0.0 }; + mathlib::Vec3 _gravity{ 0.0, 0.0, 0.0 }; bool _baseIsFree = false; double _lastBaseForwardForce{ 0.0 }; }; -} // namespace robots +} // namespace physics - -#include "Robots/RobotDynamics.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/Robots/RobotDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl similarity index 77% rename from DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl rename to DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl index 3469ab99..2aae5209 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyDynamics.inl @@ -1,11 +1,14 @@ -// DSFE_Core RobotDynamics.inl +/* + * File: Physics/RigidBodyDynamics.inl + * Created by: Joss Salton, 26-07-2026 + */ #pragma once -namespace robots { - // Computes the inertia tensor of a robot link +namespace physics { + // Computes the inertia tensor of a rigidbody link template - mathlib::Mat3_T RobotDynamics::computeLinkInertiaTensor(const RobotLink& link) const { - const robots::Inertia& I = link.inertial.inertia; + mathlib::Mat3_T RigidBodyDynamics::computeLinkInertiaTensor(const systems::RigidBodyLink& link) const { + const systems::Inertia& I = link.inertial.inertia; // Construct the inertia tensor matrix mathlib::Mat3_T M = mathlib::Mat3_T::Zero(); @@ -19,9 +22,9 @@ namespace robots { // 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, + Scalar RigidBodyDynamics::computeJointInertiaContribution( + const systems::RigidBodyJoint& joint, + const systems::RigidBodyLink& link, const mathlib::Pose_T& jointWorldPose, const mathlib::Pose_T& linkWorldPose ) const { @@ -57,21 +60,21 @@ namespace robots { 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 + // Computes the full mass matrix M(q) based on the current state and body configuration template - void RobotDynamics::computeMassMatrix( - const RobotConstModel& robot, + void RigidBodyDynamics::computeMassMatrix( + const systems::RigidBodyConstModel& body, const std::vector>& T_world, const std::vector>& jointWorldPoses, mathlib::MatX_T& M_out ) const { - const size_t n = robot.joints.size(); + 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 < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; + for (size_t k = 0; k < body.links.size(); ++k) { + const systems::RigidBodyLink& link = body.links[k]; const Scalar m = link.inertial.mass; if (m <= Scalar(0)) { continue; } @@ -85,9 +88,9 @@ namespace robots { // 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 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 @@ -101,10 +104,10 @@ namespace robots { // 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; } + const systems::RigidBodyJoint& j_j = body.joints[j]; + if (j_j.type == systems::eJointType::FIXED) { continue; } - if (!robot.jointAffectsLink(j, k)) { continue; } // skip if joint j does not affect link k + 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 @@ -121,16 +124,16 @@ namespace robots { } } - // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and robot configuration + // Computes the Coriolis and centrifugal bias vector h(q, qd) based on the current state and body configuration template - mathlib::VecX_T RobotDynamics::computeCoriolisVector( - const RobotConstModel& robot, + mathlib::VecX_T RigidBodyDynamics::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 { - const size_t n = robot.joints.size(); + 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; @@ -138,7 +141,7 @@ namespace robots { 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()); + T_world_eps.resize(body.links.size()); std::vector> jointWorldPoses_eps; jointWorldPoses_eps.resize(n); @@ -157,9 +160,9 @@ namespace robots { } // 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 + _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"); } @@ -177,24 +180,23 @@ namespace robots { } } } - return h; // [Nm], Coriolis and centrifugal bias vector for the robot at configuration q and velocity qd + 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 robot configuration + // Computes the gravity torque for a joint based on the current state and body configuration template - mathlib::VecX_T RobotDynamics::computeGravityTorque( - const RobotConstModel& robot, + mathlib::VecX_T RigidBodyDynamics::computeGravityTorque( + const systems::RigidBodyConstModel& body, const std::vector>& T_world, const std::vector>& jointWorldPoses ) const { - const size_t n = robot.joints.size(); + 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 RobotJoint& j = robot.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 @@ -202,23 +204,22 @@ namespace robots { 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); + 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 < robot.links.size(); ++k) { - const RobotLink& link = robot.links[k]; + for (size_t k = 0; k < body.links.size(); ++k) { + const systems::RigidBodyLink& link = body.links[k]; const Scalar m = (Scalar)link.inertial.mass; if (m <= Scalar(0)) { continue; } - if (!robot.jointAffectsLink(i, k)) { 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 + 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] @@ -230,22 +231,22 @@ namespace robots { 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 + // Computes the analytical Jacobian matrix J(q) for the body based on the current state and body configuration template - void RobotDynamics::analyticalJacobian( - const RobotConstModel& robot, + void RigidBodyDynamics::analyticalJacobian( + const systems::RigidBodyConstModel& body, const mathlib::VecX_T& x, mathlib::MatX_T& J_out, DenseDynamicsScratch& scratch ) { - const size_t n = robot.joints.size(); + 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(robot, x, scratch.T_world); - scratch.jointWorldPoses = _kinematics->calcJointWorldPoses(scratch.T_world, robot); - computeMassMatrix(robot, scratch.T_world, scratch.jointWorldPoses, scratch.M); + _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(); @@ -254,8 +255,8 @@ namespace robots { 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 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 @@ -286,12 +287,12 @@ namespace robots { 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 + // Computes the Coriolis and centrifugal torque for a joint based on the current state and body configuration template - mathlib::VecX_T RobotDynamics::derivative_dense( + mathlib::VecX_T RigidBodyDynamics::derivative_dense( Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, DynamicsScratch& scratch, DynamicsResult& out ) { @@ -306,21 +307,20 @@ namespace robots { 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 + 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); } + 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) { - const RobotJoint& 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; @@ -344,7 +344,7 @@ namespace robots { 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; @@ -362,7 +362,7 @@ namespace robots { } // 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); @@ -389,30 +389,29 @@ namespace robots { } template - mathlib::VecX_T RobotDynamics::derivative_spatial( - const robots::SpatialModel& model, + mathlib::VecX_T RigidBodyDynamics::derivative_spatial( + const systems::SpatialModel& model, Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, 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 += systems::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,67 +423,60 @@ namespace robots { ); 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 SpatialJoint& joint = model.joints[i]; - if (!isControlledJoint(joint.type)) { - scratch.dense.tau[i] = Scalar(0); - continue; + const systems::SpatialJoint& joint = model.joints[i]; + 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)) { + 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)); - - 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; - - out.metrics.q[i] = mathlib::real(q[i]); - out.metrics.qd[i] = mathlib::real(qd[i]); + scratch.dense.tau[off] = tau_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); + off += dof; // I forgot to add this in the previous version } 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; } template - void RobotDynamics::jacobian_spatial( - const robots::SpatialModel& model, + void RigidBodyDynamics::jacobian_spatial( + const systems::SpatialModel& model, const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, @@ -511,7 +503,7 @@ namespace robots { 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 @@ -541,12 +533,12 @@ namespace robots { 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 + // Computes the derivative of the state vector with control gains based on the current state and body configurations template - mathlib::VecX_T RobotDynamics::derivative_with_gains( + mathlib::VecX_T RigidBodyDynamics::derivative_with_gains( Scalar t, const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, DynamicsScratch& scratch, @@ -564,30 +556,29 @@ namespace robots { 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); + 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) { - const RobotJoint& 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 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; @@ -596,11 +587,11 @@ namespace robots { return dx; } - // Computes the Jacobian matrix with control gains based on the current state and robot configuration + // Computes the Jacobian matrix with control gains based on the current state and body configuration template - void RobotDynamics::jacobian_with_gains( + void RigidBodyDynamics::jacobian_with_gains( const mathlib::VecX_T& x, - const RobotSimSnapshot_T& snap, + const systems::RigidBodySnapshot_T& snap, const mathlib::VecX_T& kp, const mathlib::VecX_T& kd, mathlib::MatX_T& F_out, @@ -623,8 +614,8 @@ namespace robots { 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; + const systems::RigidBodyJoint& joint = snap.model->joints[i]; + if (joint.type == systems::eJointType::FIXED) continue; dTau_dq(i, i) = -kp[i]; @@ -641,4 +632,4 @@ namespace robots { 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 +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h similarity index 66% rename from DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h rename to DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h index d462dd03..4e500dfc 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.h +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.h @@ -1,37 +1,39 @@ -// DSFE_Core RobotKinematics.h +// DSFE_Core RigidBodyKinematics.h #pragma once #include "EngineCore.h" #include #include -#include "Robots/RobotSimSnapshot.h" +#include "Systems/RigidBodySnapshot.h" #include "EngineLib/LogMacros.h" -namespace robots { - // Forward declarations - struct RobotLink; - struct RobotJoint; +// Forward declarations +namespace systems { + struct RigidBodyLink; + struct RigidBodyJoint; +} +namespace physics { // Kinematics class responsible for computing forward kinematics and related transformations - class DSFE_API RobotKinematics { + class DSFE_API RigidBodyKinematics { public: // Constructor - RobotKinematics(); + RigidBodyKinematics(); - // Computes the forward kinematics for the robot based on the current state and robot configuration + // Computes the forward kinematics for the body based on the current state and body configuration template void computeForwardKinematics_fromState( - const RobotConstModel& robot, + 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 robot configuration + // 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 RobotConstModel& robot + const systems::RigidBodyConstModel& body ); // Computes the forward kinematics for a single joint motion based on the joint axis and angle @@ -45,6 +47,6 @@ namespace robots { template mathlib::Quat_T rpyRadToQuat(const mathlib::Vec3_T& rpyRad); }; -} +} // namespace physics -#include "Robots/RobotKinematics.inl" \ No newline at end of file +#include "Physics/RigidBodyKinematics.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl similarity index 74% rename from DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl rename to DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl index 84ed0d79..f5a7f9fd 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotKinematics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/RigidBodyKinematics.inl @@ -1,19 +1,22 @@ -// DSFE_Core RobotKinematics.inl +/* + * File: Physics/RigidBodyKinematics.inl + * Created by: Joss Salton, 26-07-2026 + */ #pragma once -namespace robots { - // Computes the forward kinematics for the robot based on the current state and robot configuration +namespace physics { + // Computes the forward kinematics for the body based on the current state and body configuration template - void RobotKinematics::computeForwardKinematics_fromState( - const RobotConstModel& robot, + void RigidBodyKinematics::computeForwardKinematics_fromState( + const systems::RigidBodyConstModel& body, const mathlib::VecX_T& x, std::vector>& T_world_out ) const { - const auto& joints = robot.joints; - const auto& links = robot.links; + const auto& joints = body.joints; + const auto& links = body.links; const size_t n = joints.size(); - T_world_out.resize(robot.links.size()); + T_world_out.resize(body.links.size()); if (T_world_out.empty()) { LOG_ERROR("T_world_out is empty"); @@ -36,17 +39,17 @@ namespace robots { // 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 } // compose transforms T = T * T_origin * T_motion; // parent -> joint -> motion -> child - int childIdx = robot.linkIndex(joint.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; @@ -56,17 +59,17 @@ namespace robots { } } - // Computes the joint world poses for all joints based on the current state and robot configuration + // Computes the joint world poses for all joints based on the current state and body configuration template - std::vector> RobotKinematics::calcJointWorldPoses( + std::vector> RigidBodyKinematics::calcJointWorldPoses( const std::vector>& T_world, - const RobotConstModel& robot + const systems::RigidBodyConstModel& body ) { - std::vector> jointWorldPoses(robot.joints.size()); + std::vector> jointWorldPoses(body.joints.size()); - for (size_t i = 0; i < robot.joints.size(); ++i) { - const RobotJoint& joints = robot.joints[i]; - int childIdx = robot.linkIndex(joints.child); + for (size_t i = 0; i < body.joints.size(); ++i) { + const systems::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); @@ -80,7 +83,7 @@ namespace robots { // Computes the forward kinematics for a single joint motion based on the joint axis and angle template - mathlib::Pose_T RobotKinematics::jointMotionTransform( + mathlib::Pose_T RigidBodyKinematics::jointMotionTransform( const mathlib::Vec3_T& axis_joint, Scalar q ) const { @@ -91,7 +94,7 @@ namespace robots { // Converts roll-pitch-yaw angles (in radians) to a quaternion representation template - mathlib::Quat_T RobotKinematics::rpyRadToQuat(const mathlib::Vec3_T& rpyRad) { + 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(); diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h similarity index 74% rename from DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h rename to DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h index 9963d617..825c804f 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.h +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.h @@ -1,16 +1,20 @@ -// DSFE_Core SpatialDynamics.h +/* + * File: Physics/SpatialDynamics.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "Robots/SpatialModel.h" -#include "Robots/DynamicsTypes.h" +#include +#include "Systems/SpatialModel.h" +#include "Physics/DynamicsTypes.h" -namespace robots { +namespace physics { class DSFE_API SpatialDynamics { public: template static void computeSpatialKinematicsAndBias( - const SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, std::vector>& Xup_out, @@ -20,7 +24,7 @@ namespace robots { 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, @@ -30,7 +34,7 @@ namespace robots { template static void computeBackwardForces_RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& a, @@ -39,7 +43,7 @@ namespace robots { 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, @@ -48,14 +52,14 @@ namespace robots { 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, @@ -65,31 +69,35 @@ namespace robots { 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 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, 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 ); 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, DynamicsScratch& scratch ); }; -} +} // namespace physics -#include "Robots/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/Robots/SpatialDynamics.inl b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl similarity index 62% rename from DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl rename to DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl index 5382736c..245c093c 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialDynamics.inl +++ b/DSFE_App/DSFE_Core/include/Physics/SpatialDynamics.inl @@ -1,10 +1,19 @@ -// DSFE_Core SpatialDynamics.inl +/* + * File: Physics/SpatialDynamics.inl + * Created by: Joss Salton, 26-07-2026 + */ #pragma once -namespace robots { +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 SpatialModel& model, + const systems::SpatialModel& model, const mathlib::VecX_T& q, const mathlib::VecX_T& qd, std::vector>& Xup_out, @@ -12,23 +21,46 @@ namespace robots { 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 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::FREE) { + if constexpr (std::is_same_v) { + 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(); + 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); + 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); 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(); @@ -51,7 +83,7 @@ namespace robots { 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, @@ -67,7 +99,7 @@ namespace robots { 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 @@ -82,7 +114,7 @@ namespace robots { template void SpatialDynamics::computeBackwardForces_RNEA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& a, @@ -95,7 +127,7 @@ namespace robots { // 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; @@ -103,7 +135,7 @@ namespace robots { // 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]; } @@ -112,7 +144,7 @@ namespace robots { 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, @@ -130,7 +162,7 @@ namespace robots { // 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); @@ -139,7 +171,7 @@ namespace robots { template mathlib::MatX_T SpatialDynamics::CRBA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, DynamicsScratch& scratch ) { @@ -152,8 +184,8 @@ namespace robots { // 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(); @@ -163,8 +195,8 @@ namespace robots { // 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); @@ -183,7 +215,7 @@ namespace robots { template void SpatialDynamics::computeArticulatedBodies_ABA( - const SpatialModel& model, + const systems::SpatialModel& model, const std::vector>& Xup, const std::vector>& v, const std::vector>& c, @@ -193,10 +225,11 @@ namespace robots { 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); @@ -204,12 +237,18 @@ namespace robots { 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 SpatialJoint& j = model.joints[i]; - - if (j.type == eJointType::FIXED) { + 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) { mathlib::SpatialMat_T XupT = Xup[i].transpose(); @@ -218,19 +257,20 @@ namespace robots { } 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; + continue; + } + // 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]; @@ -241,51 +281,69 @@ namespace robots { 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, 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(); + 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); + { + 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 SpatialJoint& j = model.joints[i]; - - if (j.parent < 0) { a_out[i] = Xup[i] * a0 + c[i]; } + const systems::SpatialJoint& j = model.joints[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.type == eJointType::FIXED) { - qdd_out[i] = Scalar(0); + 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 + 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]]; } } 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, 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 << - 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); computeSpatialKinematicsAndBias( model, q, qd, @@ -294,9 +352,11 @@ namespace robots { 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 @@ -304,16 +364,17 @@ namespace robots { 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) } -} \ No newline at end of file +} // namespace physics \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Platform/DataManager.h b/DSFE_App/DSFE_Core/include/Platform/DataManager.h index 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..8fb21b37 100644 --- a/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h +++ b/DSFE_App/DSFE_Core/include/Platform/ISimulationCore.h @@ -2,17 +2,17 @@ #pragma once #include "EngineCore.h" - +#include #include #include // 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; @@ -53,32 +53,43 @@ 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 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; + 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; 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; + // 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 void clearExternalForces() = 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/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/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/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/Scene/SimulationCore.h b/DSFE_App/DSFE_Core/include/Scene/SimulationCore.h index 759b0ce1..a8454ae2 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,9 +24,9 @@ // Forward Declarations namespace control { class TrajectoryManager; } -namespace robots { class RobotSystem; } +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) @@ -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 @@ -74,10 +77,15 @@ 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 - robots::RobotSystem& robotSystem() override; + systems::RigidBodySystem& rigidBodySystem() override; single_body_system::SingleBodySystem& singleBodySystem() override; control::TrajectoryManager& trajectoryManager() override; @@ -86,31 +94,32 @@ 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 + 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(interpreter::IStoredProgram* program, integration::eIntegrationMethod method) override; + bool runScriptToCompletion(dsl::IStoredProgram* program, integration::eIntegrationMethod method) override; // Telemetry diagnostics::TelemetryRecorder& telemetry() override; 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) @@ -132,32 +141,39 @@ 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; } - bool robotPresentationDirty() const override { return _robotPresentationDirty; } - void clearRobotPresentationDirty() override { _robotPresentationDirty = 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; + void clearExternalForces() override; + const std::vector& linkWorldTransforms() const override; + std::vector linkNames() const override; private: // Export thread management void exportThreadMain(); - void scriptParallelisation(interpreter::IStoredProgram* program); + void scriptParallelisation(dsl::IStoredProgram* program); 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; @@ -172,6 +188,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; @@ -181,14 +198,14 @@ namespace core { std::string _runTag; // Active Script Program - interpreter::IStoredProgram* _activeProgram = nullptr; - bool _robotPresentationDirty = false; + dsl::IStoredProgram* _activeProgram = nullptr; + 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..458dd949 --- /dev/null +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyLoader.h @@ -0,0 +1,17 @@ +/* + * 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 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 diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h similarity index 85% rename from DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h rename to DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h index 0e1d1608..d22cd3f0 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotMetrics.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyMetrics.h @@ -4,10 +4,10 @@ #include "EngineCore.h" -namespace robots { +namespace systems { // Per-joint metrics template - struct RobotMetrics { + struct RigidBodyMetrics { // State mathlib::VecX_T q; mathlib::VecX_T qd; @@ -19,6 +19,7 @@ namespace robots { // Dynamics mathlib::VecX_T I_eff; mathlib::VecX_T tau; + mathlib::VecX_T tau_g; // Constraints / realism mathlib::VecX_T tau_barrier; @@ -36,10 +37,10 @@ namespace robots { 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); } }; -} \ No newline at end of file +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h similarity index 76% rename from DSFE_App/DSFE_Core/include/Robots/RobotModel.h rename to DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h index ee2dd43e..8543ba67 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodyModel.h @@ -1,4 +1,7 @@ -// DSFE_Core RobotModel.h +/* + * File: Systems/RigidBodyModel.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" @@ -9,8 +12,8 @@ #include "Platform/Logger.h" #include "EngineLib/LogMacros.h" -namespace robots { - // --- Robot Model Kinematic Models --- +namespace systems { + // --- RigidBody Kinematic Models --- enum class eKinematicsModel { URDF, DH @@ -36,7 +39,21 @@ namespace robots { CONTROLLED // Full physics simulation with active control (e.g., for testing control algorithms, trajectory tracking, or simulating real-world behavior) }; - // --- Robot Model Links --- + /* + * 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 struct Inertia { double ixx = 0, ixy = 0, ixz = 0, iyy = 0, iyz = 0, izz = 0; }; @@ -84,17 +101,23 @@ namespace robots { std::vector meshEntries; // for multiple visual meshes with per-mesh material }; - // RobotLink struct, representing a single link in the robot model - struct RobotLink { + // 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{}; + + // 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; }; - // --- Robot Model Joints --- + // --- RigidBody Model Joints --- // Joint limits struct, representing the physical limits of a joint struct JointLimit { @@ -113,8 +136,8 @@ namespace robots { double friction = 0.0; }; - // RobotJoint struct, representing a single joint in the robot model - struct RobotJoint { + // 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 = ""; @@ -132,6 +155,13 @@ namespace robots { 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 }; @@ -159,16 +189,16 @@ namespace robots { mathlib::Mat4 parentToJoint = mathlib::Mat4::Identity(); }; - // --- Robot Model --- + // --- RigidBody Model --- - // RobotModel struct, representing the entire robot model - struct RobotModel { - std::string name = "UnnamedRobot"; + // 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; + std::vector links; + std::vector joints; // Torque Mode for simulation eTorqueMode torqueMode = eTorqueMode::CONTROLLED; @@ -180,7 +210,7 @@ namespace robots { // 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 + mathlib::Mat4 baseFrame = mathlib::Mat4::Identity(); // transform from world frame to rigidbody base frame, can be set in JSON bool baseFrameIsEngineAligned = false; @@ -207,4 +237,4 @@ namespace robots { } } }; -} // namespace robots \ No newline at end of file +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h similarity index 64% rename from DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h rename to DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h index bb176b7f..7b19dda8 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSimSnapshot.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySnapshot.h @@ -1,13 +1,16 @@ -// DSFE_Core RobotSimSnapshot.h +/* + * File: Systems/RigidBodySnapshot.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "Robots/RobotModel.h" +#include "Systems/RigidBodyModel.h" #include -namespace robots { - // Immutable robot data needed by solver threads - struct DSFE_API RobotConstModel { +namespace systems { + // Immutable system data needed by solver threads + struct DSFE_API RigidBodyConstModel { std::string name; bool baseFrameIsAligned = false; double scale = 1.0; @@ -15,8 +18,8 @@ namespace robots { bool jointAffectsLink(size_t jIdx, size_t lIdx) const; - std::vector links; - std::vector joints; + std::vector links; + std::vector joints; std::unordered_map linkNameToIndex; int linkIndex(const std::string& linkName) const; @@ -24,9 +27,9 @@ namespace robots { // Runtime snapshot for one integration/derivative step template - struct RobotSimSnapshot_T { + struct RigidBodySnapshot_T { - const RobotConstModel* model = nullptr; + const RigidBodyConstModel* model = nullptr; mathlib::VecX_T q; // joint angles mathlib::VecX_T qd; // joint velocities @@ -34,26 +37,26 @@ namespace robots { 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 robotRootPose = mathlib::Mat4_T::Identity(); + 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 RobotSimSnapshot = RobotSimSnapshot_T; + using RigidBodySnapshot = RigidBodySnapshot_T; template - inline RobotSimSnapshot_T castSnapshot( - const RobotSimSnapshot_T& src + inline RigidBodySnapshot_T castSnapshot( + const RigidBodySnapshot_T& src ) { - RobotSimSnapshot_T dst; + RigidBodySnapshot_T dst; dst.model = src.model; @@ -63,19 +66,16 @@ namespace robots { 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.robotRootPose = src.robotRootPose.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 robots \ No newline at end of file +} // namespace rigidbodys \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h similarity index 56% rename from DSFE_App/DSFE_Core/include/Robots/RobotSystem.h rename to DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h index 8bb9ba53..467b66f7 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystem.h +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystem.h @@ -1,17 +1,20 @@ -// DSFE_Core RobotSystem.h +/* + * File: Systems/RigidBodySystem.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" -#include "Robots/RobotModel.h" +#include "Systems/RigidBodyModel.h" -#include "Robots/SpatialModel.h" -#include "Robots/RobotSimSnapshot.h" -#include "Robots/DynamicsTypes.h" +#include "Systems/SpatialModel.h" +#include "Systems/RigidBodySnapshot.h" +#include "Physics/DynamicsTypes.h" #include -#include "Robots/RobotKinematics.h" -#include "Robots/RobotDynamics.h" -#include "Robots/SpatialDynamics.h" +#include "Physics/RigidBodyKinematics.h" +#include "Physics/RigidBodyDynamics.h" +#include "Physics/SpatialDynamics.h" #include "Analysis/MetricLogger.h" #include "Numerics/IntegrationService.h" @@ -19,7 +22,7 @@ // Forward declarations namespace control { class TrajectoryManager; } -namespace robots { +namespace systems { // Forward declarations enum class eTorqueMode; @@ -37,68 +40,91 @@ namespace robots { // Step Result struct template - struct RobotStepResult_T { + struct RigidBodyStepResult_T { integration::StepOut_T stepOut; - RobotSimSnapshot_T snap; - DynamicsResult dynamics; + RigidBodySnapshot_T snap; + physics::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 { + 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: - RobotSystem(); - ~RobotSystem(); + RigidBodySystem(); + ~RigidBodySystem(); // --- Utility Methods --- - static double clampJointAngle(const RobotJoint& joint, double angleRad); + static double clampJointAngle(const RigidBodyJoint& joint, double angleRad); template - static T clampJointAngle_T(const RobotJoint& joint, T angleRad); + static T clampJointAngle_T(const RigidBodyJoint& joint, T angleRad); // ---- Accessors --- - const robots::RobotModel& model() const; + const systems::RigidBodyModel& 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; } + 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 _robot.links.size(); } - std::size_t jointCount() const { return _robot.joints.size(); } + std::vector linkNames() const; + 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(); } + bool hasFreeJoint() const; + + int jointStateOffset(size_t joint_idx) const; + int totalDOF() 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; } + const std::string& rigidBodyName() const { return _body.name; } + 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; } - void resetNaturalFrequencyToTarget() { for (auto& joint : _robot.joints) { joint.wn_target = _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 : _robot.joints) { joint.zeta_target = _zeta; } + for (auto& joint : _body.joints) { joint.zeta_target = _zeta; } } - // Get pointer to this RobotSystem - const RobotSystem& getRobot() const { return *this; } + // Get pointer to this Systemsystem + 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 setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldForce); + void clearExtForces(); // ---- Joint State Methods --- - void computeRobotKinematics(std::vector& world); + void computeRigidBodyKinematics(std::vector& world); 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); @@ -127,25 +153,25 @@ namespace robots { // --- SIMULATION STEP METHOD --- template - RobotSimSnapshot_T takeSnapshot(T simTime) const; + 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); - // --- ROBOT LOADING AND RESET METHODS --- + // --- RIGIDBODY LOADING AND RESET METHODS --- - void loadRobot(const std::string& name); - void resetRobot(); + void loadRigidBody(const std::string& name); + void resetRigidBody(); void stopAll(); - // --- ROBOT LINK AND ROOT POSE METHODS --- + // --- RIGIDBODY 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 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; } @@ -172,16 +198,16 @@ namespace robots { std::shared_ptr runtimeIntegratorState(); std::shared_ptr runtimeIntegratorState() const; - void setRefBuffer(robots::TrajRefBuffer* buf) { _refBuffer = buf; } - void setLogBuffer(robots::JointLogBuffer* buf) { _logBuffer = buf; } + 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 robot system + // Setter and getter the torque mode for the rigidbody system void setTorqueMode(eTorqueMode mode); - eTorqueMode getTorqueMode() const { return _robot.torqueMode; } + eTorqueMode getTorqueMode() const { return _body.torqueMode; } // Swap for the current log buffer, returning a ptr to new active buffer - std::unique_ptr claimExportLogBuffer(); + std::unique_ptr claimExportLogBuffer(); // Method to enable or disable the use of internal log buffers void useInternalLogBuffer(bool enable); @@ -194,17 +220,20 @@ namespace robots { void buildSpatialModel(); template - RobotStepResult_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 RobotStepResult_T& result); + 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; + std::unique_ptr _kinematics; + std::unique_ptr _dynamics; std::unique_ptr _integrator; integration::eIntegrationMethod _curIntMethod{}; @@ -219,11 +248,11 @@ namespace robots { bool _useAutoDiff = false; - // Compute the forward drive (velocity) of the robot's root link based on the current state and robot configuration + // 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 robot configuration + // 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 robot configuration + // Integrate the floating base rotation (yaw-only for now) based on the current state and rigidbody configuration void updateBaseRootPose(); // State packing and unpacking @@ -242,38 +271,43 @@ namespace robots { void unpackRefState(const mathlib::VecX& xr); // Enforce joint limits after integration - void enforceJointLimits(RobotJoint& j); + void enforceJointLimits(RigidBodyJoint& j); // Simulation time double _simTime = 0.0; - // Robot model, and robot mode - RobotModel _robot; - eTorqueMode _torqueMode = _robot.torqueMode; + // RigidBody model, and rigidbody mode + RigidBodyModel _body; + eTorqueMode _torqueMode = _body.torqueMode; SpatialModel _spatialModel; - RobotConstModel _constModel; + RigidBodyConstModel _constModel; + + std::vector> _pendingExtForces; + std::vector _prevLinkY; // last-step link heights for floor damping + + double linkWorldMinY(size_t linkIdx, const Mat4& T) const; - 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 robot base transform (meters) + // World to rigidbody 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 + 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 _linkIndex; - std::unordered_map _jointIndex; - // List of joint indices that correspond to the robot's degrees of freedom (excluding fixed joints) + 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; @@ -289,7 +323,7 @@ namespace robots { 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; @@ -311,15 +345,15 @@ namespace robots { double _lastBaseForwardForce = 0.0; // Double-buffer design - std::array _logBuffers{}; + 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; + // Pointers to external log and reference buffers (not owned by Systemsystem) + systems::JointLogBuffer* _logBuffer = nullptr; + systems::TrajRefBuffer* _refBuffer = nullptr; }; -} // namespace robot -#include "RobotSystemStep.inl" \ No newline at end of file +} // namespace rigidbody +#include "Systems/RigidBodySystemStep.inl" \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl similarity index 60% rename from DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl rename to DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl index 55f85b73..650c0c2b 100644 --- a/DSFE_App/DSFE_Core/include/Robots/RobotSystemStep.inl +++ b/DSFE_App/DSFE_Core/include/Systems/RigidBodySystemStep.inl @@ -1,19 +1,22 @@ -// DSFE_Core RobotSystemStep.inl +/* + * File: Systems/RigidBodySystemStep.inl + * Created by: Joss Salton, 26-07-2026 + */ #pragma once -namespace robots { +namespace systems { template - T RobotSystem::clampJointAngle_T(const RobotJoint& 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)); } } - // Method to take a snapshot of the current robot state + // Method to take a snapshot of the current rigidbody state template - RobotSimSnapshot_T RobotSystem::takeSnapshot(T simTime) const { - RobotSimSnapshot_T snap; + RigidBodySnapshot_T RigidBodySystem::takeSnapshot(T simTime) const { + RigidBodySnapshot_T snap; snap.model = &_constModel; - const size_t n = (size_t)_robot.joints.size(); + const size_t n = (size_t)_body.joints.size(); snap.q.resize(n); snap.qd.resize(n); @@ -23,22 +26,29 @@ namespace robots { 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; + const auto& j = _body.joints[i]; + 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.robotRootPose = _robotRootPose.template cast(); + snap.root_pose = _root_pose.template cast(); snap.baseIsFree = _baseIsFree; snap.lastBaseForwardForce = T(_lastBaseForwardForce); - snap.gravity = T(_gravity); + snap.gravity = _gravity.template cast(); - snap.torqueMode = _robot.torqueMode; + snap.torqueMode = _body.torqueMode; snap.dt = T(_dynamics->dt()); snap.simTime = simTime; @@ -47,12 +57,12 @@ namespace robots { } template - RobotStepResult_T RobotSystem::step_impl( + 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 ) { - RobotStepResult_T result; + RigidBodyStepResult_T result; result.snap = takeSnapshot(t); auto& snap = result.snap; const size_t n = snap.model->joints.size(); @@ -61,7 +71,7 @@ namespace robots { 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; } + 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); @@ -71,7 +81,7 @@ namespace robots { auto& dynScratch = dynamicScratch; auto& dynResult = dynamicResult; - SpatialDynamics::computeSpatialKinematicsAndBias( + physics::SpatialDynamics::computeSpatialKinematicsAndBias( spatialModel, q, qd, dynScratch.spatial.Xup, @@ -79,7 +89,7 @@ namespace robots { ); // 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 @@ -100,7 +110,7 @@ namespace robots { } // 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 @@ -150,14 +160,15 @@ namespace robots { } template - void RobotSystem::postStepUpdate(const mathlib::VecX& x, const DynamicsScratch& dynScratch, const RobotStepResult_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); - 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 : _robot.joints) { enforceJointLimits(j); }*/ + /*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()); @@ -176,44 +187,46 @@ namespace robots { 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 + 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(); + auto g = _dynamics->getGravityVec(); - for (size_t k = 0; k < _robot.links.size(); ++k) { - const RobotLink& link = _robot.links[k]; + 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; } - 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(); + 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.dot(com_world); } 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; + 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; + int off = 0; for (size_t i = 0; i < n; ++i) { - const RobotJoint& j = _robot.joints[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]; + const double err = q_ref_real[i] - q_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); 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 = 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]); @@ -223,13 +236,59 @@ namespace robots { } } + template + 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; } + 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 RobotSystem::step_AD(double dt, double simTime) { - if (!hasRobot()) { return; } + void RigidBodySystem::step_AD(double dt, double simTime) { + if (!hasRigidBody()) { return; } using Dual = mathlib::DualNumber_T; _simTime = simTime; - const size_t n = _robot.joints.size(); + 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; } @@ -242,11 +301,11 @@ namespace robots { postStepUpdate(x_real, _dynScratch_AD, result); // Update base pose if free-floating - if (_baseIsFree) { - integrateBaseTranslation(dt); - updateBaseRootPose(); - } + // if (_baseIsFree) { + // integrateBaseTranslation(dt); + // updateBaseRootPose(); + // } // Update kinematics - computeRobotKinematics(_worldTransforms); + computeRigidBodyKinematics(_worldTransforms); } } \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h similarity index 58% rename from DSFE_App/DSFE_Core/include/Robots/SpatialModel.h rename to DSFE_App/DSFE_Core/include/Systems/SpatialModel.h index 3775f8d4..2b28aec2 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModel.h +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModel.h @@ -1,11 +1,14 @@ -// DSFE_Core SpatialModel.h +/* + * File: Systems/SpatialModel.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include #include -#include "Robots/RobotModel.h" +#include "Systems/RigidBodyModel.h" -namespace robots { +namespace systems { // Spatial joint struct template struct SpatialJoint { @@ -16,6 +19,8 @@ namespace robots { 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; }; @@ -29,5 +34,5 @@ namespace robots { template SpatialModel cast() const; }; -} // namespace robots -#include "SpatialModelCast.inl" \ No newline at end of file +} // namespace systems +#include "Systems/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/Systems/SpatialModelCast.inl similarity index 71% rename from DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl rename to DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl index 8ceb6c06..12afbed5 100644 --- a/DSFE_App/DSFE_Core/include/Robots/SpatialModelCast.inl +++ b/DSFE_App/DSFE_Core/include/Systems/SpatialModelCast.inl @@ -1,7 +1,10 @@ -// DSFE_Core SpatialModelCast.inl +/* + * File: Systems/SpatialModelCast.inl + * Created by: Joss Salton, 26-07-2026 + */ #pragma once -namespace robots { +namespace systems { template template SpatialModel SpatialModel::cast() const { @@ -16,7 +19,9 @@ namespace robots { 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; } -} \ No newline at end of file +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h b/DSFE_App/DSFE_Core/include/Systems/TrajectoryManager.h similarity index 85% rename from DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h rename to DSFE_App/DSFE_Core/include/Systems/TrajectoryManager.h index 03c440fd..a3bdcf5b 100644 --- a/DSFE_App/DSFE_Core/include/Robots/TrajectoryManager.h +++ b/DSFE_App/DSFE_Core/include/Systems/TrajectoryManager.h @@ -1,10 +1,13 @@ -// DSFE_Core TrajectoryManager.h +/* + * File: Systems/TrajectoryManager.h + * Created by: Joss Salton, 26-07-2026 + */ #pragma once #include "EngineCore.h" #include #include -namespace robots { class DSFE_API RobotSystem; } +namespace systems { class RigidBodySystem; } namespace control { class DSFE_API TrajectoryManager { @@ -26,7 +29,7 @@ namespace control { bool hasActive(const std::string& link) const; void set(const std::string& link, std::unique_ptr traj); - void apply(robots::RobotSystem& robot, double t); + void apply(systems::RigidBodySystem& sys, double t); std::size_t activeCount() const { return _active.size(); } 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/DSL/Command.cpp b/DSFE_App/DSFE_Core/src/DSL/Command.cpp new file mode 100644 index 00000000..66f27a9f --- /dev/null +++ b/DSFE_App/DSFE_Core/src/DSL/Command.cpp @@ -0,0 +1,20 @@ +/* + * File: DSL/Command.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "DSL/Command.h" + +namespace commands { + // Execute command + 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) {} + // Mark the command as completed + void Command::markCompleted() {} + // Check if the command has started + 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/DSL/CommandContext.cpp similarity index 86% rename from DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp rename to DSFE_App/DSFE_Core/src/DSL/CommandContext.cpp index 4c3ecda8..035123a1 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/CommandContext.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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" @@ -37,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 robot by name and updates the context with the new robot 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(); + // Loads a rigidBody by name and updates the context with the new rigidBody system + 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); + auto& rb = _core->rigidBodySystem(); return OpResult::Success(true); } @@ -62,15 +58,15 @@ namespace commands { utils::OpResult CommandContext::setJointOmega(const std::string& childLink, double omegaDegPerSec) { double omegaRadPerSec = degToRad(omegaDegPerSec); - auto& rs = _core->robotSystem(); - rs.trySetJointOmegaRad(childLink, omegaRadPerSec); + auto& rb = _core->rigidBodySystem(); + rb.trySetJointOmegaRad(childLink, omegaRadPerSec); return OpResult::Success(true); } 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,9 +82,9 @@ namespace commands { } double CommandContext::getJointAngleRad(const std::string& link) const { - auto& rs = _core->robotSystem(); + 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; } @@ -96,17 +92,17 @@ namespace commands { // --- JOINT ANGLE METHODS --- utils::OpResult CommandContext::setJointTargetRad(const std::string& link, double thetaTargetRad) { - auto& rs = _core->robotSystem(); - 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->robotSystem(); + 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; @@ -114,9 +110,9 @@ namespace commands { } utils::OpResult CommandContext::setJointMaxOmegaRad(const std::string& link, double maxqd) { - auto& rs = _core->robotSystem(); + 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); @@ -124,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->robotSystem(); - 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); @@ -133,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->robotSystem(); - 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); @@ -143,8 +139,8 @@ namespace commands { utils::OpResult CommandContext::updateJointRotateTo(double /*dt*/) { if (!_jnt.active) { return OpResult::Success(true); } - auto& rs = _core->robotSystem(); - 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()); @@ -153,7 +149,7 @@ namespace commands { } utils::OpResult CommandContext::beginJointRotateTo(const std::string& link, double maxOmegaDegPerSec, double angleDeg) { - auto& rs = _core->robotSystem(); + auto& rb = _core->rigidBodySystem(); if (link.empty()) return OpResult::Failure("beginJointRotateTo -> empty link."); const double current = getJointAngleRad(link); @@ -174,8 +170,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,10 +352,10 @@ 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(); - 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 89% rename from DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp rename to DSFE_App/DSFE_Core/src/DSL/CommandFactory.cpp index ef7964fe..6c2b901f 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/CommandFactory.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/LoadCmd.cpp similarity index 66% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/LoadCmd.cpp index 9d79d3d2..cf40d26c 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/LoadCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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,25 @@ namespace commands { // constructor LoadCmd::LoadCmd(const std::string& id, const std::vector& tokens) { - if (id == "robot") { _target.type = LoadTargetType::MultiBody; } + 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; } - - 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]; } } // Execute the command @@ -44,8 +50,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 +66,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; - return std::make_unique(id, tokens); + if (tokens.empty()) { + std::string errMsg = "load() command requires a path argument."; + D_FAIL(errMsg.c_str()); + return nullptr; + } + 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 91% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/ParallelGroupCmd.cpp index e039bb23..cddf4066 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/ParallelGroupCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/RotateByCmd.cpp similarity index 96% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateByCmd.cpp index d907b40b..59a97550 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateByCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/RotateJointByCmd.cpp similarity index 91% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateJointByCmd.cpp index 047200c7..149b239e 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointByCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/RotateJointToCmd.cpp similarity index 92% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateJointToCmd.cpp index 3719b0c2..a16508ba 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateJointToCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/RotateToCmd.cpp similarity index 95% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/RotateToCmd.cpp index 7a30acbb..bc2c0f5d 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/RotateToCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/SelectCmd.cpp similarity index 93% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SelectCmd.cpp index 10f446c0..60a012ab 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SelectCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/SetCmd.cpp similarity index 52% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SetCmd.cpp index 523fa20e..8f8c22fa 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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; } @@ -35,30 +45,41 @@ 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), "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; } // Constructor SetCmd::SetCmd(const std::string& id, const std::string& tokens) - : _id(id), _tokens(tokens) { + : _id(toLower(id)), _tokens(tokens) + { _result = { CmdState::NotStarted, {}, "" }; } @@ -79,28 +100,28 @@ 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; } - markFailed("Unknown set target: " + _id); } diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetOmegaCmd.cpp similarity index 81% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SetOmegaCmd.cpp index 218dab86..d0de2e15 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SetOmegaCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/SetVelocityCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp new file mode 100644 index 00000000..0f5de29c --- /dev/null +++ b/DSFE_App/DSFE_Core/src/DSL/Commands/SetVelocityCmd.cpp @@ -0,0 +1,72 @@ +/* + * 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) { + 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" }; + } + 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 diff --git a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp b/DSFE_App/DSFE_Core/src/DSL/Commands/SpinCmd.cpp similarity index 93% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/SpinCmd.cpp index e13a7b87..9de54931 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/SpinCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/StartCmd.cpp similarity index 91% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/StartCmd.cpp index a223a078..03ed997a 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StartCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/StopCmd.cpp similarity index 91% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/StopCmd.cpp index 8c2dff9e..f1a42596 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/StopCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Commands/TrajClearCmd.cpp similarity index 79% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/TrajClearCmd.cpp index 53d05047..3c2a4198 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajClearCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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 --- @@ -25,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(); @@ -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/DSL/Commands/TrajSetCmd.cpp similarity index 91% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/TrajSetCmd.cpp index f3f0c79a..deaf7edf 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/TrajSetCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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,22 @@ 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(); + 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 (!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 +83,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 +107,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 +157,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 +216,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/DSL/Commands/WaitCmd.cpp similarity index 88% rename from DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp rename to DSFE_App/DSFE_Core/src/DSL/Commands/WaitCmd.cpp index b557b663..676432b4 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Commands/WaitCmd.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Parser.cpp similarity index 97% rename from DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp rename to DSFE_App/DSFE_Core/src/DSL/Parser.cpp index e5770ba3..03b1aa4a 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Parser.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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 @@ -30,6 +31,7 @@ namespace interpreter { s == "set" || s == "select" || s == "setomega" || + s == "setvelocity" || s == "load"; } @@ -201,7 +203,7 @@ namespace interpreter { } // inner commands - std::vector innerCmds; + std::vector innerCmds; int braceDepth = 1; // consume subsequent lines until matching '}' @@ -223,7 +225,7 @@ namespace interpreter { 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(); @@ -263,7 +265,7 @@ namespace interpreter { return; } - program_data::Command par; + dsl::Command par; par.cmdName = "parallel"; par.rawLine = std::string(line); par.lineNumber = _program->getCurrentLineNumber(); @@ -419,5 +421,5 @@ namespace interpreter { buildProgram(); } -} // namespace interpreter +} // namespace dsl diff --git a/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp b/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp new file mode 100644 index 00000000..156a206e --- /dev/null +++ b/DSFE_App/DSFE_Core/src/DSL/RegisterCommand.cpp @@ -0,0 +1,50 @@ +/* + * File: DSL/RegisterCommand.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "DSL/RegisterCommand.h" + +// Motion commands +#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" +#include "DSL/Commands/SetVelocityCmd.h" + +// Primary function commands +#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 + void RegisterAllCommands(CommandFactory& factory) { + // Motion commands + factory.registerCommand("spin", &commands::CreateSpinCmd); // spin command + factory.registerCommand("rotateto", &commands::CreateRotateToCmd); // rotate command + factory.registerCommand("rotateby", &commands::CreateRotateByCmd); // rotate command + factory.registerCommand("rotatejointto", &commands::CreateRotateJointToCmd); // rotateJoint command + factory.registerCommand("rotatejointby", &commands::CreateRotateJointByCmd); // rotateJoint command + 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 + // New commands later + } +} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp b/DSFE_App/DSFE_Core/src/DSL/RunWrapper.cpp similarity index 89% rename from DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp rename to DSFE_App/DSFE_Core/src/DSL/RunWrapper.cpp index b5e37efc..228b0779 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/RunWrapper.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/StoredProgram.cpp similarity index 77% rename from DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp rename to DSFE_App/DSFE_Core/src/DSL/StoredProgram.cpp index 4fc7854c..511f8482 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/StoredProgram.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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,9 +152,10 @@ namespace interpreter { void StoredProgram::setIntegratorMethod(IntegratorMethod method) { _integratorMethod = method; if (_core) { - if (_core->hasRobot()) { - auto& rs = _core->robotSystem(); - if (method == IntegratorMethod::AD_ImplicitEuler || method == IntegratorMethod::AD_ImplicitMidpoint || method == IntegratorMethod::AD_GLRK2 || method == IntegratorMethod::AD_GLRK3) { + if (_core->hasRigidBody()) { + auto& rs = _core->rigidBodySystem(); + 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 { @@ -164,10 +169,14 @@ namespace interpreter { 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 @@ -178,22 +187,29 @@ namespace interpreter { // Set Gravity void StoredProgram::setGravity(double gravity) { - _gravity = gravity; + _gravity.z() = gravity; if (_core) { - if (_core->hasRobot()) { - auto& rs = _core->robotSystem(); - rs.setGravity(gravity); - } + if (_core->hasRigidBody()) { auto& rs = _core->rigidBodySystem(); rs.setGravity(gravity); } } } // Get Gravity double StoredProgram::getGravity() const { if (_core) { - if (_core->hasRobot()) { - const auto& rs = _core->robotSystem(); - 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; } -} // 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/DSL/UIContext.cpp similarity index 90% rename from DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp rename to DSFE_App/DSFE_Core/src/DSL/UIContext.cpp index ed1d066a..a9b0a40f 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/UIContext.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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/DSL/Utils.cpp similarity index 85% rename from DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp rename to DSFE_App/DSFE_Core/src/DSL/Utils.cpp index 773cf221..14749432 100644 --- a/DSFE_App/DSFE_Core/src/Interpreter/Utils.cpp +++ b/DSFE_App/DSFE_Core/src/DSL/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; @@ -199,18 +200,38 @@ 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); + std::vector parseSpatialMask(const std::string& args) { + std::string s = stripBraces(args); - // 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; - //} + 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); } mathlib::Vec3 degToRad(mathlib::Vec3& degrees) { 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/Interpreter/Command.cpp b/DSFE_App/DSFE_Core/src/Interpreter/Command.cpp deleted file mode 100644 index 24892639..00000000 --- a/DSFE_App/DSFE_Core/src/Interpreter/Command.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "pch.h" -// File: Command.cpp -#include "Interpreter/Command.h" - -namespace commands { - // Execute command - void Command::execute() { - // No base implementation - } - 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 - } - // Mark the command as completed - void Command::markCompleted() { - // Base implementation (if any) can go here - } - // Check if the command has started - bool Command::hasStarted() const { - return false; // Base implementation (if any) can go here - } -} // namespace commands \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp b/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp deleted file mode 100644 index 47c17d0e..00000000 --- a/DSFE_App/DSFE_Core/src/Interpreter/RegisterCommand.cpp +++ /dev/null @@ -1,45 +0,0 @@ -// DSFE_Core RegisterCommand.cpp -#include "pch.h" - -#include "Interpreter/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" - -// 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" - -namespace commands { - // Register all commands with the factory - void RegisterAllCommands(CommandFactory& factory) { - // Motion commands - factory.registerCommand("spin", &commands::CreateSpinCmd); // spin command - factory.registerCommand("rotateto", &commands::CreateRotateToCmd); // rotate command - factory.registerCommand("rotateby", &commands::CreateRotateByCmd); // rotate command - factory.registerCommand("rotatejointto", &commands::CreateRotateJointToCmd); // rotateJoint command - factory.registerCommand("rotatejointby", &commands::CreateRotateJointByCmd); // rotateJoint command - factory.registerCommand("trajset", &commands::CreateTrajSetCmd); // trajSet command - factory.registerCommand("trajclear", &commands::CreateTrajClearCmd); // trajClear command - factory.registerCommand("setomega", &commands::CreateSetOmegaCmd); // setOmega 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 - // New commands later - } -} // namespace commands \ No newline at end of file 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..b9123ece --- /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 "Physics/RigidBodyDynamics.h" + +namespace physics { + // 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..47e4c203 --- /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 "Physics/RigidBodyKinematics.h" + +using namespace mathlib; +using namespace constants; + +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 c9dbc7ff..a164d614 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" @@ -694,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/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/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/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/Scene/SimulationCore.cpp b/DSFE_App/DSFE_Core/src/Scene/SimulationCore.cpp index 8f6288c0..a8f65351 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,30 @@ 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 + // 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(); @@ -140,26 +147,29 @@ 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); } } + 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 } @@ -182,15 +192,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 +209,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 +243,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 +257,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 +284,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 +324,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 + 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 +349,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 +384,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 +398,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,21 +438,37 @@ 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 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); + } + // Resets the rigidBody system to its initial state + void SimulationCore::resetRigidBody() { + if (!_rigidBody) { LOG_ERROR("Cannot reset rigidBody: RigidBodySystem not set"); return; } + _rigidBody->resetRigidBody(); } - // 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); + + // 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(); } + // 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; } @@ -466,8 +492,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 +501,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 +526,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 +549,7 @@ namespace core { } } - void SimulationCore::enqueueExportBuffer(std::unique_ptr buf) { + void SimulationCore::enqueueExportBuffer(std::unique_ptr buf) { { std::lock_guard lock(_expMutex); ++_exportsInFlight; @@ -537,4 +563,19 @@ namespace core { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } + + 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(); + _accum = 0.0; + } + _manipulating.store(on); + if (!on) { _rigidBody->clearExtForces(); } // drop any residual drag force + } } \ No newline at end of file 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..91474df1 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoader.cpp @@ -0,0 +1,27 @@ +/* + * 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)); } + } + 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()); + return RigidBodyModel(); + } +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp similarity index 83% rename from DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp rename to DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp index d6501e38..6a8441fd 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotLoader.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderJSON.cpp @@ -1,7 +1,10 @@ -// DSFE_Core RobotLoader.cpp +/* + * File: Systems/RigidBodyLoaderJSON.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Robots/RobotLoader.h" +#include "Systems/RigidBodyLoader.h" #include #include "EngineLib/LogMacros.h" @@ -13,19 +16,14 @@ using kinematics::DH_Params; using namespace constants; -namespace robots { +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))); - + 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(); } @@ -43,13 +41,13 @@ namespace robots { return Vec3(j[key][0].get(), j[key][1].get(), j[key][2].get()); } - // --- RobotLoader Link and Joint Parsing --- + // --- 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, RobotModel& robot) { + static void loadMaterials(const json& data, RigidBodyModel& rigidBody) { if (!data.contains("material")) { return; } const auto& mat = data["material"]; @@ -57,7 +55,7 @@ namespace robots { 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( + rigidBody.materials[name] = Vec4( col[0].get(), col[1].get(), col[2].get(), @@ -73,7 +71,7 @@ namespace robots { // 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( + rigidBody.materials[name] = Vec4( col[0].get(), col[1].get(), col[2].get(), @@ -107,7 +105,7 @@ namespace robots { } // 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) { + static void parseVisual(const json& linkData, const std::unordered_map& materials, RigidBodyLink& link) { if (!linkData.contains("visual")) { return; } const auto& v = linkData["visual"]; @@ -172,18 +170,18 @@ namespace robots { } // Parse collision geometry material properties - static void parseCollisionMaterial(const json& collisionData, const RobotModel& robot, CollisionShape& shape) { + 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, robot.materials, "Collision", + 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 RobotModel& robot, RobotLink& 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()) { @@ -203,13 +201,13 @@ namespace robots { 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); + 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, RobotLink& link) { + 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); @@ -229,7 +227,7 @@ namespace robots { // 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) { + 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); @@ -242,7 +240,7 @@ namespace robots { } // 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) { + 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"]; @@ -280,7 +278,7 @@ namespace robots { } // Parse joint limits, including continuous revolute joints and prismatic joints - static void parseJointLimits(const json& jointData, RobotJoint& joint) { + static void parseJointLimits(const json& jointData, RigidBodyJoint& joint) { joint.limits.continuous = false; joint.limits.minAngle = 0.0f; joint.limits.maxAngle = 0.0f; @@ -311,7 +309,7 @@ namespace robots { } // Parse joint dynamics parameters - static void parseJointDynamics(const json& jointData, RobotJoint& joint) { + static void parseJointDynamics(const json& jointData, RigidBodyJoint& joint) { joint.dynamics.damping = 0.0; joint.dynamics.friction = 0.0; @@ -357,42 +355,42 @@ namespace robots { return eKinematicsModel::DH; } - // --- RobotLoader Loading, Public API --- + // --- RigidBodyLoader 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()); + 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 robot; + return rigidBody; } json data = json::parse(file); - robot.name = data["name"].get(); - loadMaterials(data, robot); + 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") { robot.visualFrame = eVisualFrame::JOINT; } - else if (vf == "link") { robot.visualFrame = eVisualFrame::LINK; } - else if (vf == "world") { robot.visualFrame = eVisualFrame::WORLD; } + 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 { - robot.visualFrame = eVisualFrame::JOINT; + rigidBody.visualFrame = eVisualFrame::JOINT; } - // Load robot scale (default 1.0) - robot.scale = data.value("scale", 1.0f); + // Load rigidBody scale (default 1.0) + rigidBody.scale = data.value("scale", 1.0f); // Load base frame if present if (data.contains("base_frame")) { - robot.baseFrameIsEngineAligned = false; + rigidBody.baseFrameIsEngineAligned = false; const auto& bf = data["base_frame"]; // Read translation and rotation (RPY) from JSON, with defaults @@ -400,45 +398,45 @@ namespace robots { 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; + 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 { - robot.baseFrameIsEngineAligned = true; - robot.baseFrame = Mat4::Identity(); + rigidBody.baseFrameIsEngineAligned = true; + rigidBody.baseFrame = Mat4::Identity(); LOG_INFO("No base frame specified in JSON, using identity (engine-aligned) by default."); } - if (robot.name == "Z1") { - robot.baseFrameIsEngineAligned = true; + if (rigidBody.name == "Z1") { + rigidBody.baseFrameIsEngineAligned = true; } // Load links for (auto& linkData : data["links"]) { - RobotLink link; + RigidBodyLink link; link.name = linkData.value("name", ""); - parseVisual(linkData, robot.materials, link); - parseCollisions(linkData, robot, link); + parseVisual(linkData, rigidBody.materials, link); + parseCollisions(linkData, rigidBody, link); parseInertial(linkData, link); - robot.links.push_back(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 - robot.kinematicsModel = decideKinematicsModel(data); + rigidBody.kinematicsModel = decideKinematicsModel(data); // Load joints for (auto& jointData : data["joints"]) { - RobotJoint joint; + RigidBodyJoint joint; // Load basic joint info joint.name = jointData["name"].get(); @@ -461,18 +459,18 @@ namespace robots { parseJointDynamics(jointData, joint); } - robot.joints.push_back(joint); + rigidBody.joints.push_back(joint); - // If robot is DH-mode, also parse DH table - if (robot.kinematicsModel == eKinematicsModel::DH) { + // 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()); - robot.kinematicsModel = eKinematicsModel::URDF; - robot.dhParams.clear(); + rigidBody.kinematicsModel = eKinematicsModel::URDF; + rigidBody.dhParams.clear(); } else { - robot.dhParams.push_back(dh); + rigidBody.dhParams.push_back(dh); } } @@ -490,11 +488,11 @@ namespace robots { } } - if (robot.kinematicsModel == eKinematicsModel::URDF) { robot.dhParams.clear(); } + if (rigidBody.kinematicsModel == eKinematicsModel::URDF) { rigidBody.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()); + 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 robot; + return rigidBody; } } \ No newline at end of file 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..e8b98890 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodyLoaderURDF.cpp @@ -0,0 +1,267 @@ +/* + * File: Systems/RigidBodyLoaderURDF.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "pch.h" + +#include "Systems/RigidBodyLoader.h" +#include + +#include "Platform/Paths.h" +#include "EngineLib/LogMacros.h" +#include + +#include +#include +#include +#include +#include + +using namespace tinyxml2; +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 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, const std::string& meshdir) { + std::string p = raw; + // Remove "package://" prefix if present + const std::string pkg = "package://"; + if (p.rfind(pkg, 0) == 0) { p = p.substr(pkg.size()); } + // Replace forward slashes with platform-specific separators + 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, const std::string& meshdir) { + 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 + 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, meshdir); + link.visual.meshEntries.push_back(entry); + } + } + } + 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")) { + // 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 = jEl->Attribute("type") ? jEl->Attribute("type") : "revolute"; + 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("effort", &joint.limits.maxEffort); + l->QueryDoubleAttribute("lower", &joint.limits.minAngle); + l->QueryDoubleAttribute("upper", &joint.limits.maxAngle); + l->QueryDoubleAttribute("velocity", &joint.limits.maxqd); + if (!continuous) { + 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 = 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. + // 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 + 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, 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 + } + // Parse links and joints from the URDF + for (XMLElement* lEl = robot->FirstChildElement("link"); lEl; lEl = lEl->NextSiblingElement("link")) { + RigidBodyLink link; + urdf_parseLink(lEl, link, meshDirRel); + 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 | 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 + ); + } + // 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; + } +} // namespace systems \ No newline at end of file diff --git a/DSFE_App/DSFE_Core/src/Robots/RobotSimSnapshot.cpp b/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp similarity index 77% rename from DSFE_App/DSFE_Core/src/Robots/RobotSimSnapshot.cpp rename to DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp index 947611d4..1974f36b 100644 --- a/DSFE_App/DSFE_Core/src/Robots/RobotSimSnapshot.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySnapshot.cpp @@ -1,11 +1,14 @@ -// DSFE_Core RobotSimSnapshot.cpp +/* + * File: Systems/RigidBodySnapshot.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Robots/RobotSimSnapshot.h" +#include "Systems/RigidBodySnapshot.h" -namespace robots { +namespace systems { // Method to check if a joint affects a link - bool RobotConstModel::jointAffectsLink(size_t jIdx, size_t lIdx) const { + 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; @@ -31,7 +34,7 @@ namespace robots { } // Method to get the index of a link by name, returns -1 if not found - int RobotConstModel::linkIndex(const std::string& linkName) const { + int RigidBodyConstModel::linkIndex(const std::string& linkName) const { auto it = linkNameToIndex.find(linkName); if (it != linkNameToIndex.end()) { return it->second; 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..55f65ca7 --- /dev/null +++ b/DSFE_App/DSFE_Core/src/Systems/RigidBodySystem.cpp @@ -0,0 +1,1178 @@ +/* + * 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; +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), + _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; } + std::vector RigidBodySystem::linkNames() const { + std::vector names; + for (const auto& link : _body.links) { names.push_back(link.name); } + return names; + } + + /* + * 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; + } + /* + * 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 --- + + // 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); + // 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; + } + } + // 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()); + 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; + } + sj.free_qref = j.free_qref; // Store the free joint reference orientation + } + 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 { + 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 (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_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 + } + 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); } + + 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); + } + } + + // 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; + } + else { + 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; + _clampOmega[i] = 0; // No clamping for free joints + } + off += dof; // increment offset by the DOF of the joint + } + } + + // 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; }} + } + + 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; } + + 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; + } + + _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 = 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; + 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 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::load(bodyPath.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.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(); + + 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 get the world origin of a specific rigidBody link by name + bool RigidBodySystem::linkWorldOrigin(const std::string& linkName, mathlib::Vec3& outOrigin) const { + 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 + 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; } + 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 < _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) + + _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::setLinkExtForce(const std::string& linkName, const mathlib::Vec3& worldForce) { + mathlib::Vec3 o; + if (!linkWorldOrigin(linkName, o)) { return false; } + return setLinkExtForce(linkName, o, worldForce); + } + // 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) { + 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; + + 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()); + 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; } + if (hasFreeJoint()) { 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; } + if (hasFreeJoint()) { 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 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; } + 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& 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; + } + + // 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 = 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; } + + // 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/Robots/TrajectoryManager.cpp b/DSFE_App/DSFE_Core/src/Systems/TrajectoryManager.cpp similarity index 66% rename from DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp rename to DSFE_App/DSFE_Core/src/Systems/TrajectoryManager.cpp index ce546e2b..89df0cec 100644 --- a/DSFE_App/DSFE_Core/src/Robots/TrajectoryManager.cpp +++ b/DSFE_App/DSFE_Core/src/Systems/TrajectoryManager.cpp @@ -1,20 +1,23 @@ -// DSFE_Core TrajectoryManager.cpp +/* + * File: Systems/TrajectoryManager.cpp + * Created by: Joss Salton, 26-07-2026 + */ #include "pch.h" -#include "Robots/TrajectoryManager.h" -#include "Robots/RobotSystem.h" +#include "Systems/TrajectoryManager.h" +#include "Systems/RigidBodySystem.h" #include #include #include namespace control { - // Clear trajectory for a specific robot link + // 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 robot link at time t, returning the desired state in out + // 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; } @@ -25,14 +28,14 @@ namespace control { return true; } - // Check if a trajectory is active for a specific robot link + // 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 robot link + // Set a trajectory for a specific rigidBody link void TrajectoryManager::set(const std::string& link, std::unique_ptr traj) { if (!traj) { _active.erase(link); @@ -41,8 +44,8 @@ namespace control { _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) { + // 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; @@ -50,14 +53,14 @@ namespace control { // 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); + // 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 = robot.trySetJointOmegaRefRad(link, (float)ref.qd); + 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 = robot.trySetJointAlphaRefRad(link, (float)ref.qdd); + 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 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/cube/cube.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf new file mode 100644 index 00000000..eb4c9205 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/cube.urdf @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ 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 00000000..0d4af35d Binary files /dev/null and b/DSFE_App/DSFE_Engine/assets/rigidbody_models/cube/meshes/cube.fbx differ 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 diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/CITATION.cff b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/CITATION.cff new file mode 100644 index 00000000..c15ff00c --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/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/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/vispa/LICENSE.txt 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/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/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/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/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/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/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/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/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/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/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/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/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/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/vispa/meshes/Link6-DHReference-PublicRelease.stl diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/package.xml b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/meshes/package.xml new file mode 100644 index 00000000..f274c0c1 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/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/vispa/urdf/VISPA_modifiedDH.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/urdf/VISPA_modifiedDH.urdf new file mode 100644 index 00000000..19e3efcf --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/urdf/VISPA_modifiedDH.urdf @@ -0,0 +1,142 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/DSFE_App/DSFE_Engine/assets/objects/Robotic_Arm_Models/VISPA/VISPA.json b/DSFE_App/DSFE_Engine/assets/rigidbody_models/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/vispa/vispa.json diff --git a/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf new file mode 100644 index 00000000..1bd16932 --- /dev/null +++ b/DSFE_App/DSFE_Engine/assets/rigidbody_models/vispa/vispa.urdf @@ -0,0 +1,149 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/DSFE_App/DSFE_GUI/CMakeLists.txt b/DSFE_App/DSFE_GUI/CMakeLists.txt index ba03493f..4f357157 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 ) @@ -138,11 +140,10 @@ set(WIDGETS_SRC include/MainWindow/Widgets/FractionSelectorWidget.h src/MainWindow/DSL/DSLSyntaxHighlighter.cpp include/MainWindow/DSL/DSLSyntaxHighlighter.h -) - -set(ROBOT_SRC - src/Robots/RobotPresentationBuilder.cpp - src/Robots/RobotRenderer.cpp + src/MainWindow/Widgets/GravityVectorWidget.cpp + include/MainWindow/Widgets/GravityVectorWidget.h + src/MainWindow/style/RecentItemDelegate.cpp + include/MainWindow/style/RecentItemDelegate.h ) set(OBJECT_SRC @@ -179,7 +180,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/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..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.06799895316362381, + "pitch": -0.4689960479736328, "pos": [ - 2.5499985218048096, - 1.1508084535598755, - 3.396512269973755 + 1.2224150896072388, + 0.6096836924552917, + -0.11915026605129242 ], - "yaw": -2.219787359237671 + "yaw": -9.512792587280273 }, "content": { - "robot": "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/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/include/MainWindow/Widgets/ControlPanelWidget.h b/DSFE_App/DSFE_GUI/include/MainWindow/Widgets/ControlPanelWidget.h index 963ad0f4..cd23a34d 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; } @@ -35,6 +35,7 @@ class QSlider; namespace widgets { class FractionSelectorWidget; + class GravityVectorWidget; class ControlPanelWidget : public QWidget { public: @@ -87,6 +88,9 @@ namespace widgets { void simPropertiesPanel(); void buildIntegratorCombos(); + void buildTimestepSelectors(QVBoxLayout* layout); + + void worldPropertiesPanel(); void jointInfoPanel(); void updateTelemetryInfo(const diagnostics::JointTelemetry& j); @@ -99,6 +103,10 @@ namespace widgets { void updateSimClock(); + QLabel* _jointChainLabel = nullptr; + QLabel* _jointIndexLabel = nullptr; + void refreshJointChainLabel(int idx); + gui::SimulationManager* _sim = nullptr; QVBoxLayout* _contentLayout = nullptr; @@ -107,8 +115,19 @@ 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; + + QLabel* _gravityLabel = nullptr; + GravityVectorWidget* _grav = nullptr; QGroupBox* _jointInfoGroup = nullptr; QSlider* _jointIdxSlider = nullptr; @@ -154,7 +173,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..7b4ef76a 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/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/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/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/include/MainWindow/Workspace/Workspace.h b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h index 5ef9323b..88cec419 100644 --- a/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h +++ b/DSFE_App/DSFE_GUI/include/MainWindow/Workspace/Workspace.h @@ -12,17 +12,18 @@ namespace gui { struct WorkspaceData { int version = 1; QString name; - - QString robotName; // empty = no robot + 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) // Simulation properties 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 simDt = 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 }; @@ -35,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 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/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"; } } diff --git a/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h b/DSFE_App/DSFE_GUI/include/Simulation/SimulationManager.h index db079534..e11cf18d 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" @@ -40,9 +44,9 @@ namespace scene { // Forward Declarations for Simulation Core namespace core { class ISimulationCore; } -// Forward Declarations for Physics, Robots, Control, and Integration -namespace interpreter { class IStoredProgram; } -namespace robots { class RobotSystem; struct RobotModel; } +// Forward Declarations for Physics, RigidBodys, Control, and Integration +namespace dsl { class IStoredProgram; } +namespace systems { class RigidBodySystem; struct RigidBodyModel; } namespace control { class TrajectoryManager; } namespace integration { enum class eIntegrationMethod; } @@ -51,8 +55,7 @@ namespace gui { enum class ViewID { Manual = 0, Top, Right, Front, Follow, COUNT }; class SimulationRenderer; - //class SimulationSystemController; - + // Forward Declarations for eKeyCode enum class eKeyCode; @@ -132,11 +135,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); @@ -153,7 +156,7 @@ namespace gui { void tick(double dt); void setDisplaySize(uint32_t w, uint32_t h); - void syncRobotToScene(); + void syncRigidBodyToScene(); void syncBodyToScene(); // Scene Objects Management @@ -167,21 +170,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; @@ -218,9 +221,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); @@ -238,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(); } @@ -263,9 +268,20 @@ 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; } + const std::string& currentRigidBodyPath() const { return _currentRigidBodyPath; } + + 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; @@ -299,6 +315,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 @@ -308,8 +329,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 @@ -338,12 +359,13 @@ namespace gui { double _simTime = 0.0; double _fixedDt = 1.0 / 180.0; double _telemetryHz = 100.0; - std::string _currentRobotName; + std::string _currentRigidBodyName; + std::string _currentRigidBodyPath; 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/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(); } diff --git a/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h b/DSFE_App/DSFE_GUI/include/Systems/MultiBodySystem.h index 9243390c..78c019f5 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,7 +12,7 @@ #include "Platform/Logger.h" -namespace robots { struct RobotModel; } +namespace systems { struct RigidBodyModel; } namespace assets { class MeshLoader; } namespace gui { @@ -21,7 +21,7 @@ namespace gui { class MultiBodySystem : public ISimulationSystem { public: - MultiBodySystem(const robots::RobotModel& 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::RobotModel& _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/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/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/DSFE_MainWindow.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/DSFE_MainWindow.cpp index 49051c2b..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,12 @@ 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(); std::unordered_map familyMenus; @@ -205,9 +216,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_robot(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/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 }); } diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp index 63164a25..37d2a6fe 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ControlPanelWidget.cpp @@ -16,11 +16,12 @@ #include #include "Widgets/FractionSelectorWidget.h" +#include "Widgets/GravityVectorWidget.h" #include "Scene/Camera.h" #include "Simulation/SimulationManager.h" -#include "Robots/RobotSystem.h" +#include "Systems/RigidBodySystem.h" #include "Analysis/Telemetry.h" #include "Platform/Paths.h" @@ -41,6 +42,7 @@ namespace widgets { rootLayout->addWidget(scrollArea); simPropertiesPanel(); + worldPropertiesPanel(); jointInfoPanel(); auto* timer = new QTimer(this); @@ -66,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); @@ -77,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()); + layout->addSpacing(8); - dtHeaderRow->addWidget(simDtLabel); - dtHeaderRow->addWidget(telemetryDtLabel); + buildTimestepSelectors(layout); - layout->addLayout(dtHeaderRow); - - auto* dtValueRow = new QHBoxLayout(); - - dtValueRow->addWidget(_simDtSelector); - dtValueRow->addWidget(_telemetryDtSelector); - layout->addLayout(dtValueRow); layout->addSpacing(8); layout->addWidget(_simTimeLabel); - _contentLayout->addWidget(_simPropertiesGroup); buildIntegratorCombos(); @@ -124,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() { @@ -149,65 +140,176 @@ 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); + + _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); + + _contentLayout->addWidget(_worldPropertiesGroup); + } + 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")); + + // --- 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->hasRobot()) { - _jointInfoGroup->setVisible(false); - return; - } - auto& rs = _sim->robotSystem(); - auto& joints = rs.joints(); - auto& links = rs.links(); - - _jointInfoGroup->setVisible(!joints.empty() && !links.empty()); + if (!_sim || !_sim->hasRigidBody()) { _jointInfoGroup->setVisible(false); return; } + auto& body = _sim->rigidBodySystem(); + auto& joints = body.joints(); + auto& links = body.links(); 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); + const double e = 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)); - - 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) { @@ -215,120 +317,119 @@ 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); + } - layout->addSpacing(8); - layout->addWidget(t.referenceHeader); - layout->addLayout(refGrid); + 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.err, + t.qRef, t.qdRef, t.qddRef, + t.qTraj, t.qdTraj, t.qddTraj, + t.qClamped, t.qdClamped, + t.damping, t.friction + } + ) { + valueLabel(v); + } - layout->addSpacing(8); - layout->addWidget(t.trajectoryHeader); - layout->addLayout(trajGrid); + 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.clampedHeader); - layout->addLayout(limitGrid); + // θ (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.constantsHeader); - layout->addLayout(physicalGrid); + auto* refGrid = makeGrid({ + { symLabel("\u03B8ref"), t.qRef }, + { symLabel("\u03C9ref"), t.qdRef }, + { symLabel("\u03B1ref"), t.qddRef }, // α target accel + { symLabel("\u0394\u03B8"), t.err }, // Δθ error + }); + + auto* trajGrid = makeGrid({ + { symLabel("\u03B8traj"), t.qTraj }, + { symLabel("\u03C9traj"), t.qdTraj }, + { symLabel("\u03B1traj"), t.qddTraj }, + }); + + 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() { @@ -352,11 +453,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 +469,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() { @@ -404,7 +505,13 @@ 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); + _grav->setValue(_sim->gravity()); + } } // namespace widgets \ No newline at end of file diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/DSLEditorWidget.cpp index 5bd9a201..15d38bbb 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" @@ -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; } @@ -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/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 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..94632780 --- /dev/null +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/GravityVectorWidget.cpp @@ -0,0 +1,78 @@ +/* + * 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\u2009=\u2009[ %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 diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/RobotSelectorWidget.cpp index 70b646b7..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_robot(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 diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp index 9428e611..0050ca2c 100644 --- a/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp +++ b/DSFE_App/DSFE_GUI/src/MainWindow/Widgets/ViewportWidget.cpp @@ -134,6 +134,15 @@ 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); + _sim->setLinkHighlight(_dragLink, true); + } + } } void ViewportWidget::mouseReleaseEvent(QMouseEvent* event) { @@ -142,14 +151,33 @@ namespace widgets { releaseMouse(); unsetCursor(); } + else if (event->button() == Qt::LeftButton && _dragging) { + _dragging = false; + if (_sim && !_dragLink.empty()) { _sim->setLinkHighlight(_dragLink, false); } + _dragLink.clear(); + if (_sim) { _sim->clearExternalForces(); } + } } 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. + 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 = 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); + } } void ViewportWidget::wheelEvent(QWheelEvent* event) { @@ -157,4 +185,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 diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/HomePage.cpp index 350fc164..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 @@ -303,8 +306,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()) { diff --git a/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp b/DSFE_App/DSFE_GUI/src/MainWindow/Workspace/Workspace.cpp index 66040898..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["robot"] = robotName; + content["rigid_body"] = rigidBodyName.toLower(); + content["rigid_body_path"] = rigidBodyPath; content["script_text"] = scriptText; content["script_path"] = scriptPath; o["content"] = content; @@ -23,6 +24,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; @@ -40,7 +42,8 @@ namespace gui { w.name = o["name"].toString(); const QJsonObject content = o["content"].toObject(); - w.robotName = content["robot"].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(); @@ -50,6 +53,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/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 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. diff --git a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp index fa806431..c6eaef63 100644 --- a/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp +++ b/DSFE_App/DSFE_GUI/src/Simulation/SimulationManager.cpp @@ -8,14 +8,14 @@ #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" -#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 @@ -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) { @@ -122,36 +130,42 @@ 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; + _currentRigidBodyPath = name; + LOG_INFO("RigidBody loaded: %s", model.name.c_str()); } - - void SimulationManager::clearRobot() { _systems.clear_all(_scene); _scene.clear(); } + // 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(); } // -------------------------------------------------- // SIMULATION TICK & RENDER @@ -214,7 +228,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(); } @@ -249,9 +263,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(); } @@ -274,18 +288,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) { @@ -296,7 +305,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" }; @@ -306,9 +315,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); @@ -344,7 +353,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 +363,8 @@ namespace gui { _scene.clear(); _renderer.destroy_all_meshes(); _mesh_store.clear(); - _currentRobotName.clear(); + _currentRigidBodyName.clear(); + _currentRigidBodyPath.clear(); _core->setScriptRunning(false); _core->stopSimulation(); @@ -370,25 +380,64 @@ 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); _camera.setPitch(w.cameraPitch); - if (!w.robotName.isEmpty()) { - load_robot(w.robotName.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 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.rigidBodyPath = QString::fromStdString(_currentRigidBodyPath); w.integrationMethod = static_cast(integrationMethod()); w.adIntegrationMethod = static_cast(autoDiffIntegrationMethod()); 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(); } + + 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)); + } + 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 = 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) { + 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; + } + } } diff --git a/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp b/DSFE_App/DSFE_GUI/src/Systems/MultiBodySystem.cpp index e202a1b9..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" @@ -6,7 +9,7 @@ #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 +26,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) { @@ -31,17 +34,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; + 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; - } + 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 +65,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; + } } } @@ -79,7 +91,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 68% rename from DSFE_App/DSFE_GUI/src/Robots/RobotPresentationBuilder.cpp rename to DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp index b8e04226..d1a972c4 100644 --- a/DSFE_App/DSFE_GUI/src/Robots/RobotPresentationBuilder.cpp +++ b/DSFE_App/DSFE_GUI/src/Systems/RigidBodyPresentationBuilder.cpp @@ -1,7 +1,10 @@ -// DSFE_GUI RobotPresentationBuilder.cpp -#include "Robots/RobotPresentationBuilder.h" +/* + * File: Systems/RigidBodyPresentationBuilder.cpp + * Created by: Joss Salton, 26-07-2026 + */ +#include "Systems/RigidBodyPresentationBuilder.h" -#include "Robots/RobotModel.h" +#include "Systems/RigidBodyModel.h" #include "Assets/MeshLoader.h" #include "Scene/Object.h" @@ -9,29 +12,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; +RigidBodyRenderBinding RigidBodyPresentationBuilder::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; - + fs::path fullPath = paths::assets() / 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