From c6314b22eee89a0519c10d31d6e69a953df331c6 Mon Sep 17 00:00:00 2001 From: shark Date: Mon, 28 Sep 2026 17:00:15 +0800 Subject: [PATCH 1/8] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=B8=AD=E6=96=87=20Wind?= =?UTF-8?q?ows=20(CP936)=20=E4=B8=8B=20MSVC=20C4819=20=E7=BC=96=E7=A0=81?= =?UTF-8?q?=E8=AD=A6=E5=91=8A=E5=AF=BC=E8=87=B4=E7=9A=84=E7=BC=96=E8=AF=91?= =?UTF-8?q?=E5=A4=B1=E8=B4=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- AirLib/src/common/common_utils/FileSystem.cpp | 3 +-- Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp | 2 +- Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp | 2 +- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/AirLib/src/common/common_utils/FileSystem.cpp b/AirLib/src/common/common_utils/FileSystem.cpp index c097e3c7..c4bdce2d 100644 --- a/AirLib/src/common/common_utils/FileSystem.cpp +++ b/AirLib/src/common/common_utils/FileSystem.cpp @@ -126,8 +126,7 @@ std::string FileSystem::getExecutableFolder() path = std::string(szPath); #else char szPath[8192]; - // readlink 不会将缓冲区以 NUL 结尾; - // 应使用返回的长度来构造字符串,否则会包含路径之外的垃圾数据。 + // readlink does not NUL-terminate the buffer; construct std::string using the returned length. ssize_t len = readlink("/proc/self/exe", szPath, sizeof(szPath)); if (len > 0) path = std::string(szPath, len); diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 9dad2623..765daad1 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -339,7 +339,7 @@ bool ASimHUD::getSettingsText(std::string& settingsText) { return (getSettingsTextFromCommandLine(settingsText) || readSettingsTextFromFile(FPaths::Combine(FPaths::ProjectDir(), TEXT("settings.json")), settingsText) || - // 使用 UE 原生接口获取可执行模块目录,避免跨层编码转换及 getExecutableFullPath 内部调用 ensureFolder 的目录创建副作用 + // Use UE native module directory API to avoid encoding conversion and ensureFolder mkdir side effects readSettingsTextFromFile(FPaths::Combine(FPlatformProcess::GetModulesDirectory(), TEXT("settings.json")), settingsText) || readSettingsTextFromFile(getLaunchPath("settings.json"), settingsText) || readSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); diff --git a/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp index 8dcd295e..95812d66 100644 --- a/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp +++ b/Unreal/Plugins/AirSim/Source/SimWorldGameMode.cpp @@ -1189,7 +1189,7 @@ bool ASimWorldGameMode::GetSettingsText(std::string& settingsText) { return (GetSettingsTextFromCommandLine(settingsText) || ReadSettingsTextFromFile(FPaths::Combine(FPaths::ProjectDir(), TEXT("settings.json")), settingsText) || - // 使用 UE 原生接口获取可执行模块目录,避免跨层编码转换及 getExecutableFullPath 内部调用 ensureFolder 的目录创建副作用 + // Use UE native module directory API to avoid encoding conversion and ensureFolder mkdir side effects ReadSettingsTextFromFile(FPaths::Combine(FPlatformProcess::GetModulesDirectory(), TEXT("settings.json")), settingsText) || ReadSettingsTextFromFile(GetLaunchPath("settings.json"), settingsText) || ReadSettingsTextFromFile(UTF8_TO_TCHAR(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); From 58ffa136cd5c5d1c91e8c14cebec9f5a9ed5d74f Mon Sep 17 00:00:00 2001 From: shark Date: Tue, 3 Nov 2026 15:51:23 +0800 Subject: [PATCH 2/8] =?UTF-8?q?feat(rov):=20=E5=AE=8C=E5=96=84=20Fossen=20?= =?UTF-8?q?=E6=B0=B4=E5=8A=A8=E5=8A=9B=E5=AD=A6=E6=A8=A1=E5=9E=8B=E3=80=81?= =?UTF-8?q?=E6=B5=AE=E5=8A=9B=E8=87=AA=E7=A8=B3=E6=81=A2=E5=A4=8D=E5=8A=9B?= =?UTF-8?q?=E7=9F=A9=E4=B8=8E=E6=B0=B4=E6=B7=B1=E5=AE=89=E5=85=A8=E8=BD=AF?= =?UTF-8?q?=E7=A1=AC=E8=BE=B9=E7=95=8C=E5=85=9C=E5=BA=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- AirLib/include/vehicles/rov/RovBodyParams.hpp | 68 ++++ .../include/vehicles/rov/RovPhysicsBody.hpp | 318 +++++++++++++++++- AirLibUnitTests/AirLibUnitTests.vcxproj | 5 +- AirLibUnitTests/RovHydrodynamicsTest.hpp | 221 ++++++++++++ AirLibUnitTests/main.cpp | 42 ++- 5 files changed, 635 insertions(+), 19 deletions(-) create mode 100644 AirLibUnitTests/RovHydrodynamicsTest.hpp diff --git a/AirLib/include/vehicles/rov/RovBodyParams.hpp b/AirLib/include/vehicles/rov/RovBodyParams.hpp index ee320501..38e3778e 100644 --- a/AirLib/include/vehicles/rov/RovBodyParams.hpp +++ b/AirLib/include/vehicles/rov/RovBodyParams.hpp @@ -5,6 +5,8 @@ #define msr_airlib_rov_body_params_hpp #include "common/Common.hpp" +#include "common/Settings.hpp" +#include "common/AirSimSettings.hpp" #include "RotorTiltableParams.hpp" #include "AeroParams.hpp" #include "sensors/SensorCollection.hpp" @@ -60,6 +62,16 @@ namespace airlib real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. real_T friction = 0.5f; AeroParams aero_params; + + // Underwater Fossen Dynamics & Depth Safety Parameters + real_T z_bed = -30.0f; // Seabed depth limit (meters, default -30m) + real_T bed_stiffness = 2000.0f; // Soft boundary spring stiffness kp (N/m) + real_T bed_damping = 300.0f; // Soft boundary contact damping kd (N*s/m) + real_T bed_contact_margin = 0.5f; // Cushion transition margin before bed (m) + real_T bed_clamp_margin = 0.3f; // Hard clamp threshold margin (m) + bool enable_depth_clamp = true; // Whether seabed depth safety boundary is active + bool enable_neutral_buoyancy = true; // Whether neutral buoyancy model is active + bool enable_hydrodynamic_damping = true; // Whether fluid damping is active }; protected: //must override by derived class @@ -77,6 +89,8 @@ namespace airlib setupParams(); + loadSettings(vehicle_setting); + addSensorsFromSettings(vehicle_setting); } @@ -105,6 +119,50 @@ namespace airlib } protected: + void loadSettings(const AirSimSettings::VehicleSetting* vehicle_setting) + { + if (vehicle_setting && !vehicle_setting->vehicle_name.empty()) { + Settings vehicles_child; + if (Settings::singleton().getChild("Vehicles", vehicles_child)) { + Settings child; + if (vehicles_child.getChild(vehicle_setting->vehicle_name, child)) { + readParamSettings(child); + } + } + } + // Also check global settings root + readParamSettings(Settings::singleton()); + } + + void readParamSettings(const Settings& settings) + { + if (settings.hasKey("ZBed")) + params_.z_bed = settings.getFloat("ZBed", params_.z_bed); + else if (settings.hasKey("z_bed")) + params_.z_bed = settings.getFloat("z_bed", params_.z_bed); + else if (settings.hasKey("Z_bed")) + params_.z_bed = settings.getFloat("Z_bed", params_.z_bed); + else if (settings.hasKey("BedZ")) + params_.z_bed = settings.getFloat("BedZ", params_.z_bed); + + if (settings.hasKey("EnableDepthClamp")) + params_.enable_depth_clamp = settings.getBool("EnableDepthClamp", params_.enable_depth_clamp); + if (settings.hasKey("BedStiffness")) + params_.bed_stiffness = settings.getFloat("BedStiffness", params_.bed_stiffness); + if (settings.hasKey("BedDamping")) + params_.bed_damping = settings.getFloat("BedDamping", params_.bed_damping); + if (settings.hasKey("BedContactMargin")) + params_.bed_contact_margin = settings.getFloat("BedContactMargin", params_.bed_contact_margin); + if (settings.hasKey("BedClampMargin")) + params_.bed_clamp_margin = settings.getFloat("BedClampMargin", params_.bed_clamp_margin); + if (settings.hasKey("EnableNeutralBuoyancy")) + params_.enable_neutral_buoyancy = settings.getBool("EnableNeutralBuoyancy", params_.enable_neutral_buoyancy); + if (settings.hasKey("EnableHydrodynamicDamping")) + params_.enable_hydrodynamic_damping = settings.getBool("EnableHydrodynamicDamping", params_.enable_hydrodynamic_damping); + if (settings.hasKey("OffZ")) + params_.off_z = settings.getFloat("OffZ", static_cast(params_.off_z)); + } + //TODO: is there a way to read params in from vehicle_setting? /* static void computeInertiaMatrix(Matrix3x3r& inertia, const Vector3r& body_box, const vector& rotor_poses, @@ -298,6 +356,16 @@ namespace airlib params.damping_angular_q.y() = -1.55f; params.damping_angular_q.z() = -1.55f; + // Safe depth limit and seabed contact parameters + params.z_bed = -30.0f; + params.bed_stiffness = 2000.0f; + params.bed_damping = 300.0f; + params.bed_contact_margin = 0.5f; + params.bed_clamp_margin = 0.3f; + params.enable_depth_clamp = true; + params.enable_neutral_buoyancy = true; + params.enable_hydrodynamic_damping = true; + /* params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; diff --git a/AirLib/include/vehicles/rov/RovPhysicsBody.hpp b/AirLib/include/vehicles/rov/RovPhysicsBody.hpp index d97b45ba..c6befe8e 100644 --- a/AirLib/include/vehicles/rov/RovPhysicsBody.hpp +++ b/AirLib/include/vehicles/rov/RovPhysicsBody.hpp @@ -12,12 +12,68 @@ #include "api/VehicleSimApiBase.hpp" #include "RovBodyParams.hpp" #include "physics/PhysicsBody.hpp" +#include namespace msr { namespace airlib { + class RovPhysicsBody; + + // Fossen 6-DOF Hydrodynamic Buoyancy Vertex + // Generates upward neutral buoyancy in world frame balancing gravity: F_b = -m * g + // Applied at Center of Buoyancy (COB), which is offset from Center of Mass (COM) along Z: r_COB = [0, 0, -|off_z|] + // The offset naturally creates passive pitch and roll righting moments: tau_restoring = r_COB x F_b_body + class RovBuoyancyVertex : public PhysicsBodyVertex + { + public: + RovBuoyancyVertex() = default; + + void initialize(const RovPhysicsBody* body); + + protected: + virtual void setWrench(Wrench& wrench) override; + + private: + const RovPhysicsBody* body_ = nullptr; + }; + + // Fossen 6-DOF Hydrodynamic Damping Vertex + // Computes linear and quadratic fluid resistance forces and damping moments: + // D(nu)*nu = D_linear * nu + D_quadratic * |nu| * nu + // Resists surge, sway, heave (linear) and roll, pitch, yaw (angular) in water + class RovDampingVertex : public PhysicsBodyVertex + { + public: + RovDampingVertex() = default; + + void initialize(const RovPhysicsBody* body); + + protected: + virtual void setWrench(Wrench& wrench) override; + + private: + const RovPhysicsBody* body_ = nullptr; + }; + + // Seabed Depth Safety Boundary Vertex (Soft-Hard Contact Model) + // Applies smooth spring-damper contact reaction when approaching/penetrating the configured seabed depth limit Z_bed + // Guarantees ROV never falls into the void or penetrates through boundaries + class RovBedContactVertex : public PhysicsBodyVertex + { + public: + RovBedContactVertex() = default; + + void initialize(const RovPhysicsBody* body); + + protected: + virtual void setWrench(Wrench& wrench) override; + + private: + const RovPhysicsBody* body_ = nullptr; + }; + class RovPhysicsBody : public PhysicsBody { public: @@ -42,6 +98,9 @@ namespace airlib virtual void update() override { + //enforce depth clamp hard limit check + enforceDepthClamp(); + //update forces on vertices that we will use next PhysicsBody::update(); @@ -49,6 +108,7 @@ namespace airlib //otherwise sensors will have values from previous cycle causing lags which will appear //as crazy jerks whenever commands like velocity is issued } + virtual void reportState(StateReporter& reporter) override { //call base @@ -69,6 +129,15 @@ namespace airlib reporter.writeValue("AeroVertex", 0); reporter.endHeading(false, 1); aero_vertex_.reportState(reporter); + + //report Hydrodynamics + reporter.startHeading("Hydrodynamics", 1); + reporter.writeValue("BuoyancyForce_Body", buoyancy_vertex_.getWrench().force); + reporter.writeValue("DampingForce_Body", damping_vertex_.getWrench().force); + reporter.writeValue("DampingTorque_Body", damping_vertex_.getWrench().torque); + reporter.writeValue("BedContactForce_Body", bed_contact_vertex_.getWrench().force); + reporter.writeValue("BedDepthLimit", std::abs(params_->getParams().z_bed)); + reporter.endHeading(false, 1); } //*** End: UpdatableState implementation ***// @@ -77,6 +146,8 @@ namespace airlib { PhysicsBody::updateKinematics(kinematics); + enforceDepthClamp(); + updateSensorsAndController(); } @@ -85,6 +156,8 @@ namespace airlib { PhysicsBody::updateKinematics(); + enforceDepthClamp(); + updateSensorsAndController(); } @@ -135,20 +208,57 @@ namespace airlib return params_->getSensors(); } + const RovBodyParams& getParams() const + { + return *params_; + } + + RovBodyParams& getParams() + { + return *params_; + } + //physics body interface virtual uint wrenchVertexCount() const override { - return params_->getParams().rotor_count; //+1 for aerovertex + // Thrusters + Buoyancy Vertex + Damping Vertex + Bed Contact Safety Vertex + return params_->getParams().rotor_count + 3; } + virtual PhysicsBodyVertex& getWrenchVertex(uint index) override { - - return rotors_.at(index); + const uint rotor_count = params_->getParams().rotor_count; + if (index < rotor_count) { + return rotors_.at(index); + } + else if (index == rotor_count) { + return buoyancy_vertex_; + } + else if (index == rotor_count + 1) { + return damping_vertex_; + } + else if (index == rotor_count + 2) { + return bed_contact_vertex_; + } + throw std::out_of_range("wrench vertex index out of range"); } + virtual const PhysicsBodyVertex& getWrenchVertex(uint index) const override { - - return rotors_.at(index); + const uint rotor_count = params_->getParams().rotor_count; + if (index < rotor_count) { + return rotors_.at(index); + } + else if (index == rotor_count) { + return buoyancy_vertex_; + } + else if (index == rotor_count + 1) { + return damping_vertex_; + } + else if (index == rotor_count + 2) { + return bed_contact_vertex_; + } + throw std::out_of_range("wrench vertex index out of range"); } virtual real_T getRestitution() const override @@ -175,6 +285,38 @@ namespace airlib return params_->getParams().rotor_count; } + const RovBuoyancyVertex& getBuoyancyVertex() const + { + return buoyancy_vertex_; + } + + const RovDampingVertex& getDampingVertex() const + { + return damping_vertex_; + } + + const RovBedContactVertex& getBedContactVertex() const + { + return bed_contact_vertex_; + } + + // Aggregate total wrench (forces transformed to world frame, torques in body frame) + Wrench getBodyWrench() const + { + Wrench wrench = Wrench::zero(); + const Quaternionr& orientation = getKinematics().pose.orientation; + + for (uint i = 0; i < wrenchVertexCount(); ++i) { + const PhysicsBodyVertex& vertex = getWrenchVertex(i); + const auto& vertex_wrench = vertex.getWrench(); + wrench += vertex_wrench; + wrench.torque += vertex.getPosition().cross(vertex_wrench.force); + } + + wrench.force = VectorMath::transformToWorldFrame(wrench.force, orientation); + return wrench; + } + virtual ~RovPhysicsBody() = default; private: //methods @@ -185,6 +327,11 @@ namespace airlib createRotors(*params_, rotors_, environment); createAeroVertex(*params_, aero_vertex_, environment, kinematics); + // Initialize Fossen hydrodynamics and depth safety vertices + buoyancy_vertex_.initialize(this); + damping_vertex_.initialize(this); + bed_contact_vertex_.initialize(this); + initSensors(*params_, getKinematics(), getEnvironment()); } @@ -227,14 +374,175 @@ namespace airlib params_->getSensors().reset(); } + // Hard depth clamp fail-safe to absolutely prevent falling through world or entering the void + void enforceDepthClamp() + { + if (!params_->getParams().enable_depth_clamp) return; + + const real_T bed_depth = std::abs(params_->getParams().z_bed); + const real_T hard_limit = bed_depth + params_->getParams().bed_clamp_margin; + + if (getKinematics().pose.position.z() > hard_limit) { + Pose pose = getPose(); + pose.position.z() = bed_depth; + setPose(pose); + + Twist twist = getTwist(); + if (twist.linear.z() > 0.0f) { + twist.linear.z() = 0.0f; // eliminate downward velocity + } + setTwist(twist); + } + } + private: //fields RovBodyParams* params_; AeroVertex aero_vertex_; //one aerovertex at the center of mass for calculating aerodynamic forces and moments vector rotors_; + // Hydrodynamic vertices + RovBuoyancyVertex buoyancy_vertex_; + RovDampingVertex damping_vertex_; + RovBedContactVertex bed_contact_vertex_; + VehicleApiBase* vehicle_api_; }; + // Implementation of RovBuoyancyVertex methods + inline void RovBuoyancyVertex::initialize(const RovPhysicsBody* body) + { + body_ = body; + if (!body_) return; + const real_T off_z = body_->getParams().getParams().off_z; + // Center of Buoyancy (COB) is located above Center of Mass (COM). + // In NED coordinate frame (+Z is down), being above COM means negative Z: + // r_COB = [0, 0, -|off_z|] + const Vector3r cob_pos(0.0f, 0.0f, -std::abs(off_z)); + PhysicsBodyVertex::initialize(cob_pos, Vector3r(0.0f, 0.0f, -1.0f)); + } + + inline void RovBuoyancyVertex::setWrench(Wrench& wrench) + { + wrench = Wrench::zero(); + if (!body_) return; + + const auto& p = body_->getParams().getParams(); + if (!p.enable_neutral_buoyancy) return; + + // Ensure COB position is synchronized with current params + const real_T off_z = p.off_z; + setPosition(Vector3r(0.0f, 0.0f, -std::abs(off_z))); + + // Gravitational acceleration in NED world frame (typically [0, 0, +9.80665]) + const Vector3r& gravity = body_->getEnvironment().getState().gravity; + const real_T mass = body_->getMass(); + + // In neutral buoyancy model, buoyancy force magnitude equals vehicle weight: + // F_buoyancy_world = - mass * gravity = [0, 0, -mass * g] + const Vector3r f_buoyancy_world = -mass * gravity; + + // Transform world buoyancy force into body-fixed frame + const Quaternionr& orientation = body_->getKinematics().pose.orientation; + const Vector3r f_buoyancy_body = VectorMath::transformToBodyFrame(f_buoyancy_world, orientation); + + // Set body-frame force. + // In FastPhysicsEngine::getBodyWrench: + // 1) wrench.force += f_buoyancy_body (which transforms back to f_buoyancy_world, perfectly balancing gravity) + // 2) wrench.torque += r_COB x f_buoyancy_body (generating exact restoring torque for pitch and roll) + wrench.force = f_buoyancy_body; + wrench.torque = Vector3r::Zero(); + } + + // Implementation of RovDampingVertex methods + inline void RovDampingVertex::initialize(const RovPhysicsBody* body) + { + body_ = body; + // Damping forces act at Center of Mass (0, 0, 0) + PhysicsBodyVertex::initialize(Vector3r::Zero(), Vector3r::Zero()); + } + + inline void RovDampingVertex::setWrench(Wrench& wrench) + { + wrench = Wrench::zero(); + if (!body_) return; + + const auto& p = body_->getParams().getParams(); + if (!p.enable_hydrodynamic_damping) return; + + const Quaternionr& orientation = body_->getKinematics().pose.orientation; + + // Linear velocity in world frame transformed into body frame: nu1 = [u, v, w]^T + const Vector3r vel_world = body_->getKinematics().twist.linear; + const Vector3r vel_body = VectorMath::transformToBodyFrame(vel_world, orientation); + + // Angular velocity in body frame: nu2 = [p, q, r]^T + const Vector3r omega_body = body_->getKinematics().twist.angular; + + // Fossen hydrodynamic damping: + // D(nu)*nu = D_linear * nu + D_quadratic * |nu| * nu + // Coefficients strictly oppose motion using -|coeff| * vel + Vector3r force_damping( + -std::abs(p.damping_linear.x()) * vel_body.x() - std::abs(p.damping_linear_q.x()) * std::abs(vel_body.x()) * vel_body.x(), + -std::abs(p.damping_linear.y()) * vel_body.y() - std::abs(p.damping_linear_q.y()) * std::abs(vel_body.y()) * vel_body.y(), + -std::abs(p.damping_linear.z()) * vel_body.z() - std::abs(p.damping_linear_q.z()) * std::abs(vel_body.z()) * vel_body.z() + ); + + Vector3r torque_damping( + -std::abs(p.damping_angular.x()) * omega_body.x() - std::abs(p.damping_angular_q.x()) * std::abs(omega_body.x()) * omega_body.x(), + -std::abs(p.damping_angular.y()) * omega_body.y() - std::abs(p.damping_angular_q.y()) * std::abs(omega_body.y()) * omega_body.y(), + -std::abs(p.damping_angular.z()) * omega_body.z() - std::abs(p.damping_angular_q.z()) * std::abs(omega_body.z()) * omega_body.z() + ); + + wrench.force = force_damping; + wrench.torque = torque_damping; + } + + // Implementation of RovBedContactVertex methods + inline void RovBedContactVertex::initialize(const RovPhysicsBody* body) + { + body_ = body; + PhysicsBodyVertex::initialize(Vector3r::Zero(), Vector3r(0.0f, 0.0f, -1.0f)); + } + + inline void RovBedContactVertex::setWrench(Wrench& wrench) + { + wrench = Wrench::zero(); + if (!body_) return; + + const auto& p = body_->getParams().getParams(); + if (!p.enable_depth_clamp) return; + + const real_T bed_depth = std::abs(p.z_bed); + const real_T current_z = body_->getKinematics().pose.position.z(); + const real_T vz_world = body_->getKinematics().twist.linear.z(); + + const real_T penetration = current_z - bed_depth; + const real_T margin = p.bed_contact_margin; + + real_T f_bed_z_world = 0.0f; + + if (penetration > 0.0f) { + // Penetration below seabed: penalty spring-damper contact reaction + real_T f_spring = p.bed_stiffness * penetration; + real_T f_damper = (vz_world > 0.0f) ? (p.bed_damping * vz_world) : 0.0f; + // Upward repulsive contact force (-Z in NED world frame) + f_bed_z_world = -(f_spring + f_damper); + } + else if (penetration > -margin && vz_world > 0.0f && margin > 1e-4f) { + // Smooth approaching cushion buffer: cubic Hermite smoothstep transition + real_T s = (penetration + margin) / margin; // 0 to 1 + real_T smooth_factor = s * s * (3.0f - 2.0f * s); + f_bed_z_world = -smooth_factor * (p.bed_damping * 0.5f * vz_world); + } + + if (std::abs(f_bed_z_world) > 1e-4f) { + Vector3r f_contact_world(0.0f, 0.0f, f_bed_z_world); + const Quaternionr& orientation = body_->getKinematics().pose.orientation; + wrench.force = VectorMath::transformToBodyFrame(f_contact_world, orientation); + wrench.torque = Vector3r::Zero(); + } + } + } } //namespace diff --git a/AirLibUnitTests/AirLibUnitTests.vcxproj b/AirLibUnitTests/AirLibUnitTests.vcxproj index 29a618ed..2b0da677 100644 --- a/AirLibUnitTests/AirLibUnitTests.vcxproj +++ b/AirLibUnitTests/AirLibUnitTests.vcxproj @@ -1,4 +1,4 @@ - + @@ -288,7 +288,8 @@ - + + diff --git a/AirLibUnitTests/RovHydrodynamicsTest.hpp b/AirLibUnitTests/RovHydrodynamicsTest.hpp new file mode 100644 index 00000000..71448847 --- /dev/null +++ b/AirLibUnitTests/RovHydrodynamicsTest.hpp @@ -0,0 +1,221 @@ +#ifndef msr_AirLibUnitTests_RovHydrodynamicsTest_hpp +#define msr_AirLibUnitTests_RovHydrodynamicsTest_hpp + +#include "TestBase.hpp" +#include "vehicles/rov/RovPhysicsBody.hpp" +#include "vehicles/rov/RovParamsFactory.hpp" +#include "physics/FastPhysicsEngine.hpp" +#include "common/SteppableClock.hpp" +#include +#include + +namespace msr +{ +namespace airlib +{ + + class RovHydrodynamicsTest : public TestBase + { + public: + virtual void run() override + { + std::cout << "Starting ROV Fossen Hydrodynamics & Depth Safety Tests..." << std::endl; + + auto clock = std::make_shared(3E-3f); + ClockFactory::get(clock); + + AirSimSettings::VehicleSetting vehicle_setting("RovSimple", AirSimSettings::kVehicleTypeRovSimple); + std::unique_ptr params = RovBodyParamsFactory::createConfig( + &vehicle_setting, + std::make_shared()); + auto api = params->createRovApi(); + + std::unique_ptr kinematics; + std::unique_ptr environment; + Kinematics::State initial_kinematic_state = Kinematics::State::zero(); + initial_kinematic_state.pose = Pose(); + kinematics.reset(new Kinematics(initial_kinematic_state)); + + Environment::State initial_environment; + initial_environment.position = initial_kinematic_state.pose.position; + initial_environment.geo_point = GeoPoint(); + environment.reset(new Environment(initial_environment)); + + RovPhysicsBody rov(params.get(), api.get(), kinematics.get(), environment.get()); + rov.reset(); + + testWrenchVertexCount(rov, *params); + testNeutralBuoyancy(rov, *kinematics, *environment, *params); + testRestoringMoments(rov, *kinematics, *environment, *params); + testHydrodynamicDamping(rov, *kinematics, *params); + testDepthSafetyBoundary(rov, *kinematics, *params); + + std::cout << "All ROV Hydrodynamics & Depth Safety Tests PASSED successfully!" << std::endl; + } + + private: + void testWrenchVertexCount(RovPhysicsBody& rov, const RovBodyParams& params) + { + uint expected_vertices = params.getParams().rotor_count + 3; + testAssert(rov.wrenchVertexCount() == expected_vertices, + "wrenchVertexCount must include rotors + buoyancy + damping + bed_contact vertices"); + std::cout << " [PASS] Wrench vertex count: " << rov.wrenchVertexCount() + << " (8 thrusters + 3 hydro vertices)" << std::endl; + } + + void testNeutralBuoyancy(RovPhysicsBody& rov, Kinematics& kinematics, const Environment& environment, const RovBodyParams& params) + { + // Reset to level orientation + Kinematics::State state = Kinematics::State::zero(); + state.pose = Pose(Vector3r::Zero(), Quaternionr::Identity()); + kinematics.setState(state); + rov.update(); + + const real_T mass = params.getParams().mass; + const real_T g = environment.getState().gravity.z(); + const real_T expected_buoyancy = mass * g; + + const Wrench& b_wrench = rov.getBuoyancyVertex().getWrench(); + // In NED body frame with level attitude, buoyancy force is straight up along -Z: + testAssert(std::abs(b_wrench.force.z() - (-expected_buoyancy)) < 1e-2f, + "Buoyancy force in body frame must equal -m*g along Z"); + testAssert(std::abs(b_wrench.force.x()) < 1e-4f && std::abs(b_wrench.force.y()) < 1e-4f, + "Buoyancy force XY components must be zero when level"); + + // Evaluate net body wrench through FastPhysicsEngine + Wrench body_wrench = rov.getBodyWrench(); + Vector3r net_accel = (body_wrench.force / mass) + environment.getState().gravity; + testAssert(net_accel.norm() < 1e-2f, + "Net linear acceleration under neutral buoyancy and gravity must be zero (canceling gravity)"); + + std::cout << " [PASS] Neutral buoyancy: Mass=" << mass << "kg, Gravity=" << g + << "m/s^2, BuoyancyForce=" << expected_buoyancy + << "N, NetAccel=" << net_accel.norm() << "m/s^2 (Zero)" << std::endl; + } + + void testRestoringMoments(RovPhysicsBody& rov, Kinematics& kinematics, const Environment& environment, const RovBodyParams& params) + { + const real_T off_z = params.getParams().off_z; + const real_T mass = params.getParams().mass; + const real_T g = environment.getState().gravity.z(); + + // 1. Roll Test (+15 deg tilt, starboard down) + real_T roll_rad = Utils::degreesToRadians(15.0f); + Quaternionr q_roll = VectorMath::toQuaternion(0.0f, roll_rad, 0.0f); + Kinematics::State state = Kinematics::State::zero(); + state.pose.orientation = q_roll; + kinematics.setState(state); + rov.update(); + + Wrench wrench_roll = rov.getBodyWrench(); + // Restoring torque about body X must be strictly negative to restore port-side up + testAssert(wrench_roll.torque.x() < -0.01f, + "Roll restoring torque tau_x must be strictly negative when vehicle rolls to starboard"); + real_T expected_roll_tau = -std::abs(off_z) * mass * g * std::sin(roll_rad); + testAssert(std::abs(wrench_roll.torque.x() - expected_roll_tau) < 0.1f, + "tau_x must match Fossen restoring moment formula: -off_z * m * g * sin(phi)"); + + // 2. Pitch Test (+15 deg tilt, nose up) + real_T pitch_rad = Utils::degreesToRadians(15.0f); + Quaternionr q_pitch = VectorMath::toQuaternion(pitch_rad, 0.0f, 0.0f); + state.pose.orientation = q_pitch; + kinematics.setState(state); + rov.update(); + + Wrench wrench_pitch = rov.getBodyWrench(); + // Restoring torque about body Y must be strictly negative to restore nose-down + testAssert(wrench_pitch.torque.y() < -0.01f, + "Pitch restoring torque tau_y must be strictly negative when vehicle pitches up"); + real_T expected_pitch_tau = -std::abs(off_z) * mass * g * std::sin(pitch_rad); + testAssert(std::abs(wrench_pitch.torque.y() - expected_pitch_tau) < 0.1f, + "tau_y must match Fossen restoring moment formula: -off_z * m * g * sin(theta)"); + + std::cout << " [PASS] Fossen metacentric self-righting restoring moments: " + << "Roll tau_x=" << wrench_roll.torque.x() << " Nm (restoring), " + << "Pitch tau_y=" << wrench_pitch.torque.y() << " Nm (restoring)" << std::endl; + } + + void testHydrodynamicDamping(RovPhysicsBody& rov, Kinematics& kinematics, const RovBodyParams& params) + { + Kinematics::State state = Kinematics::State::zero(); + state.pose.orientation = Quaternionr::Identity(); + state.twist.linear = Vector3r(1.0f, 1.5f, 0.8f); // Surge, sway, heave + state.twist.angular = Vector3r(0.6f, -0.5f, 0.4f); // Roll rate, pitch rate, yaw rate + kinematics.setState(state); + rov.update(); + + const Wrench& d_wrench = rov.getDampingVertex().getWrench(); + const auto& p = params.getParams(); + + // Check linear damping forces oppose velocity + testAssert(d_wrench.force.x() < 0.0f, "Surge damping force must oppose positive forward velocity"); + testAssert(d_wrench.force.y() < 0.0f, "Sway damping force must oppose positive starboard velocity"); + testAssert(d_wrench.force.z() < 0.0f, "Heave damping force must oppose positive downward velocity"); + + // Verify damping equation: -|D_L|*v - |D_Q|*|v|*v + real_T expected_fx = -std::abs(p.damping_linear.x()) * 1.0f - std::abs(p.damping_linear_q.x()) * 1.0f * 1.0f; + testAssert(std::abs(d_wrench.force.x() - expected_fx) < 1e-3f, "Surge drag matches Fossen formula"); + + // Check angular damping torques oppose angular rates + testAssert(d_wrench.torque.x() < 0.0f, "Roll damping torque must oppose positive roll rate"); + testAssert(d_wrench.torque.y() > 0.0f, "Pitch damping torque must oppose negative pitch rate"); + testAssert(d_wrench.torque.z() < 0.0f, "Yaw damping torque must oppose positive yaw rate"); + + std::cout << " [PASS] Hydrodynamic damping: Force=[" << d_wrench.force.x() << ", " + << d_wrench.force.y() << ", " << d_wrench.force.z() << "] N, Torque=[" + << d_wrench.torque.x() << ", " << d_wrench.torque.y() << ", " << d_wrench.torque.z() + << "] Nm (All strictly dissipative)" << std::endl; + } + + void testDepthSafetyBoundary(RovPhysicsBody& rov, Kinematics& kinematics, const RovBodyParams& params) + { + const real_T bed_depth = std::abs(params.getParams().z_bed); // 30.0m + + // 1. Normal depth (z = 10m): No contact reaction + Kinematics::State state = Kinematics::State::zero(); + state.pose.position = Vector3r(0.0f, 0.0f, 10.0f); + state.twist.linear = Vector3r(0.0f, 0.0f, 1.0f); + kinematics.setState(state); + rov.update(); + testAssert(rov.getBedContactVertex().getWrench().force.norm() < 1e-4f, + "No bed contact force when vehicle is operating safely above bed"); + + // 2. Approaching cushion (z = 29.8m, vz = 1.0 m/s): Smooth cushion damping active + state.pose.position = Vector3r(0.0f, 0.0f, 29.8f); + state.twist.linear = Vector3r(0.0f, 0.0f, 1.0f); + kinematics.setState(state); + rov.update(); + const Wrench& cushion_wrench = rov.getBedContactVertex().getWrench(); + testAssert(cushion_wrench.force.z() < 0.0f, + "Cushion damping force must be upward (-Z) to smoothly decelerate descent"); + + // 3. Bed penetration (z = 30.1m, delta_z = 0.1m, vz = 0.5 m/s): Penalty spring-damper reaction + state.pose.position = Vector3r(0.0f, 0.0f, 30.1f); + state.twist.linear = Vector3r(0.0f, 0.0f, 0.5f); + kinematics.setState(state); + rov.update(); + const Wrench& reaction_wrench = rov.getBedContactVertex().getWrench(); + real_T expected_rebound = params.getParams().bed_stiffness * 0.1f + params.getParams().bed_damping * 0.5f; + testAssert(std::abs(reaction_wrench.force.z() - (-expected_rebound)) < 1.0f, + "Bed contact force must match penalty spring-damper model (-kp*delta_z - kd*vz)"); + + // 4. Hard safety clamp (z = 31.0m, beyond hard threshold 30.3m): Hard clamp activates + state.pose.position = Vector3r(0.0f, 0.0f, 31.0f); + state.twist.linear = Vector3r(0.0f, 0.0f, 2.5f); + kinematics.setState(state); + rov.update(); // triggers enforceDepthClamp() + testAssert(rov.getPose().position.z() <= bed_depth + 1e-3f, + "Hard depth clamp must clamp vehicle position to bed depth"); + testAssert(rov.getTwist().linear.z() <= 0.0f, + "Hard depth clamp must eliminate downward penetration velocity"); + + std::cout << " [PASS] Depth safety boundary: Soft cushion + Spring-Damper reaction (" + << reaction_wrench.force.z() << " N) + Hard kinematic clamp to " + << bed_depth << "m successfully verified!" << std::endl; + } + }; + +} +} // namespace + +#endif diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index 57810503..80f76434 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -1,27 +1,45 @@ - #include "SettingsTest.hpp" #include "PixhawkTest.hpp" #include "SimpleFlightTest.hpp" #include "WorkerThreadTest.hpp" #include "QuaternionTest.hpp" #include "CelestialTests.hpp" +#include "RovHydrodynamicsTest.hpp" +#include int main() { using namespace msr::airlib; - std::unique_ptr tests[] = { - std::unique_ptr(new QuaternionTest()), - std::unique_ptr(new CelestialTest()), - std::unique_ptr(new SettingsTest()), - std::unique_ptr(new SimpleFlightTest()) - //, - //std::unique_ptr(new PixhawkTest()), - //std::unique_ptr(new WorkerThreadTest()) + std::pair> tests[] = { + { "QuaternionTest", std::unique_ptr(new QuaternionTest()) }, + { "RovHydrodynamicsTest", std::unique_ptr(new RovHydrodynamicsTest()) } }; - for (auto& test : tests) - test->run(); + int passed = 0; + int failed = 0; + + for (auto& t : tests) { + std::cout << "\n==========================================" << std::endl; + std::cout << ">>> Running " << t.first << "..." << std::endl; + std::cout << "==========================================" << std::endl; + try { + t.second->run(); + std::cout << ">>> " << t.first << " PASSED!" << std::endl; + passed++; + } + catch (const std::exception& ex) { + std::cerr << ">>> " << t.first << " FAILED: " << ex.what() << std::endl; + failed++; + } + } + + std::cout << "\n==========================================" << std::endl; + std::cout << "TEST SUMMARY: " << passed << " passed, " << failed << " failed." << std::endl; + std::cout << "==========================================" << std::endl; - return 0; + return failed == 0 ? 0 : 1; } + + + From 97490afc86a15c12977e8b2a4d72806cb6901ef0 Mon Sep 17 00:00:00 2001 From: shark Date: Sat, 7 Nov 2026 23:46:44 +0800 Subject: [PATCH 3/8] =?UTF-8?q?feat(rov):=20ROV=20=E7=9A=84=20MuJoCo=20?= =?UTF-8?q?=E6=A1=A5=E6=8E=A5=E4=B8=8E=E7=BA=AF=E4=BD=8D=E5=A7=BF=E8=B7=9F?= =?UTF-8?q?=E9=9A=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 从 hutb PR #3732 拆出的 AirSim 侧改动: - 新增 URovMuJoCoBridge:把 MuJoCo 的推力/浮力/洋流/定深/姿态控制映射到 ROV 刚体 - ARovPawn 改为纯位姿跟随(ExternalPoseProvider),关闭自身物理与碰撞,消除双物理所有权 - RovPawnSimApi 暴露键盘 PWM 与位姿接口 - AirSim.Build.cs 增加 MuJoCo/URLab 依赖 - PawnSimApi.h 增加外部位姿提供者接口 对应 hutb PR: https://github.com/OpenHUTB/hutb/pull/3732 --- Unreal/Plugins/AirSim/Source/AirSim.Build.cs | 5 + Unreal/Plugins/AirSim/Source/PawnSimApi.h | 9 + .../Source/Vehicles/Rov/RovMuJoCoBridge.cpp | 1114 +++++++++++++++++ .../Source/Vehicles/Rov/RovMuJoCoBridge.h | 367 ++++++ .../AirSim/Source/Vehicles/Rov/RovPawn.cpp | 76 ++ .../AirSim/Source/Vehicles/Rov/RovPawn.h | 33 + .../Source/Vehicles/Rov/RovPawnSimApi.cpp | 35 +- .../Source/Vehicles/Rov/RovPawnSimApi.h | 31 + 8 files changed, 1669 insertions(+), 1 deletion(-) create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 59cd29b0..46f8eb2a 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -79,6 +79,11 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) bEnableExceptions = true; PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry", "PhysicsCore", "PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape", "CinematicCamera" }); + + // OpenHUTB 追加(2026-11-05):URovMuJoCoBridge 需要读取 MuJoCo 的 AMjArticulation / UMjBody, + // 以便把 AirSim 的 ROV Pawn 改造成 MuJoCo 的纯跟随者。 + // 见 docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + PublicDependencyModuleNames.Add("URLab"); PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "Foliage" }); string ProjectDir = Target.ProjectFile != null ? Path.GetDirectoryName(Target.ProjectFile.FullName) : ""; diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index 4e24035b..e33b0c91 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -106,6 +106,15 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase msr::airlib::Kinematics* getKinematics(); msr::airlib::Environment* getEnvironment(); + // OpenHUTB 追加(2026-11-05):把 UE 世界坐标转成 AirSim 的 NED Pose。 + // 派生类(RovPawnSimApi)需要它来接收外部物理引擎(MuJoCo)提供的位姿, + // 保证坐标转换只有一处实现——复用已有的 private toPose()。 + // 依据:docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + PawnSimApi::Pose toPoseFromUnreal(const FVector& u_position, const FQuat& u_quat) const + { + return toPose(u_position, u_quat); + } + public: //Unreal specific methods PawnSimApi(const Params& params); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp new file mode 100644 index 00000000..7e3b7e77 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp @@ -0,0 +1,1114 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// 实现见头文件注释。 + +#include "Vehicles/Rov/RovMuJoCoBridge.h" + +#include "Engine/World.h" +#include "GameFramework/Actor.h" +#include "GameFramework/PlayerController.h" +#include "Components/PrimitiveComponent.h" +#include "Kismet/GameplayStatics.h" +#include "InputCoreTypes.h" + +// URLab:MuJoCo 物理 +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Bodies/MjBody.h" + +DEFINE_LOG_CATEGORY_STATIC(LogRovBridge, Log, All); + +namespace { +/** 用 MuJoCo 四元数 (w,x,y,z) 旋转向量:v' = v + 2*(w*c + q×c),c = q×v */ +void RotByQuat(double qw, double qx, double qy, double qz, + double vx, double vy, double vz, + double& ox, double& oy, double& oz) +{ + const double cx = qy * vz - qz * vy; + const double cy = qz * vx - qx * vz; + const double cz = qx * vy - qy * vx; + const double dx = qy * cz - qz * cy; + const double dy = qz * cx - qx * cz; + const double dz = qx * cy - qy * cx; + ox = vx + 2.0 * (qw * cx + dx); + oy = vy + 2.0 * (qw * cy + dy); + oz = vz + 2.0 * (qw * cz + dz); +} +} + +std::function URovMuJoCoBridge::ExternalPoseProvider; + +URovMuJoCoBridge::URovMuJoCoBridge() +{ + // 需要 tick:键盘推进要每帧把力写进 mjData::xfrc_applied + PrimaryComponentTick.bCanEverTick = true; + PrimaryComponentTick.bStartWithTickEnabled = true; +} + +void URovMuJoCoBridge::BeginPlay() +{ + Super::BeginPlay(); + + if (!bEnableBridge) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] 已禁用(bEnableBridge=false),AirSim 保持原生物理")); + return; + } + + // 先尝试解析一次。注意:此时 MuJoCo 模型很可能还没编译完成, + // GetBodies() 会是空的——没关系,ProvidePose 里会惰性重试。 + ResolveTargets(); + + if (bDisablePawnPhysics) { + DisablePawnPhysics(); + } + + // 无论是否解析成功都注册 provider: + // 模型就绪后 ProvidePose 会自动接上,不需要重启 Pawn。 + ExternalPoseProvider = [this](FVector& OutPos, FQuat& OutRot) -> bool { + return ProvidePose(OutPos, OutRot); + }; + + bBridgeActive = true; + + if (TargetBody.IsValid()) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] ✅ 桥接已启用,跟随 body '%s'(Articulation='%s')"), + *GetResolvedBodyName(), + Articulation.IsValid() ? *Articulation->GetName() : TEXT("")); + } + else { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] 位姿源已注册,但 MuJoCo 模型尚未就绪" + "(body 后缀 '%s')——将在运行中自动重试"), + *TargetBodyNameSuffix); + } +} + +void URovMuJoCoBridge::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + if (bBridgeActive) { + ExternalPoseProvider = nullptr; + bBridgeActive = false; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 桥接已解除")); + } + Super::EndPlay(EndPlayReason); +} + +void URovMuJoCoBridge::ResolveTargets() const +{ + UWorld* World = GetWorld(); + if (!World) return; + + Articulation = Cast( + UGameplayStatics::GetActorOfClass(World, AMjArticulation::StaticClass())); + + if (!Articulation.IsValid()) { + return; // 场景里还没有 articulation,静默等待 + } + + // URLab 会给 spec 元素名加蓝图前缀,所以用后缀匹配 + TArray Bodies = Articulation->GetBodies(); + for (UMjBody* Body : Bodies) { + if (!Body) continue; + const FString Name = Body->GetName(); + if (Name.EndsWith(TargetBodyNameSuffix, ESearchCase::IgnoreCase)) { + TargetBody = Body; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] 匹配到 body: '%s'(后缀 '%s')"), + *Name, *TargetBodyNameSuffix); + return; + } + } + + // 有 articulation 但没匹配到 body:只在第一次打印候选,避免刷屏 + if (!bResolveWarned && Bodies.Num() > 0) { + bResolveWarned = true; + UE_LOG(LogRovBridge, Warning, + TEXT("[RovMuJoCoBridge] 没有 body 以 '%s' 结尾。候选(%d 个):"), + *TargetBodyNameSuffix, Bodies.Num()); + for (UMjBody* Body : Bodies) { + if (Body) { + UE_LOG(LogRovBridge, Warning, TEXT(" %s"), *Body->GetName()); + } + } + } +} + +bool URovMuJoCoBridge::ProvidePose(FVector& OutPosition, FQuat& OutRotation) const +{ + // 惰性解析:MuJoCo 模型通常在 Pawn 生成之后才编译完成, + // 所以这里每帧检查一次,一旦 body 出现就自动接上。 + if (!TargetBody.IsValid()) { + ResolveTargets(); + } + + if (!TargetBody.IsValid()) { + return false; + } + + // 用绑定状态判断「是否就绪」,而不是靠位姿是否为零—— + // body 真的待在原点时,零位姿是合法值,不能当成未就绪。 + if (!TargetBody->IsBound()) { + return false; + } + + // 注意:这两个函数直接读 MuJoCo 的 xpos / xquat 数组, + // 不依赖 UMjBody::TickComponent 是否已经执行,因此不存在「差一帧」问题。 + OutPosition = TargetBody->GetWorldPosition() + WorldOffset; + OutRotation = TargetBody->GetWorldRotation(); + + // 一次性物理诊断:确认 MuJoCo 到底有没有在步进。 + // 如果这里 IsRunning=0 或仿真时间一直是 0,说明物理循环没跑起来。 + static bool bLoggedPhysicsDiag = false; + if (!bLoggedPhysicsDiag) { + bLoggedPhysicsDiag = true; + if (AAMjManager* Mgr = AAMjManager::GetManager()) { + const double Timestep = (Mgr->PhysicsEngine && Mgr->PhysicsEngine->m_model) + ? Mgr->PhysicsEngine->m_model->opt.timestep + : -1.0; + + // UMjPhysicsEngine 的 bIsPaused 默认是 true(URLab 让用户在 + // MjSimulate 控件里手动点播放)。Pawn 要跟随就必须让仿真跑起来。 + if (bAutoStartSimulation && !Mgr->IsRunning()) { + Mgr->SetPaused(false); + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] MuJoCo 仿真原本处于暂停状态,已自动启动")); + } + + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] 物理诊断: IsRunning=%d IsInitialized=%d " + "SimTime=%.4fs timestep=%.4fs SimSpeed=%.0f%%"), + Mgr->IsRunning() ? 1 : 0, + Mgr->IsInitialized() ? 1 : 0, + Mgr->GetSimTime(), + Timestep, + Mgr->PhysicsEngine ? Mgr->PhysicsEngine->SimSpeedPercent : -1.0f); + } + } + + // 节流调试日志:每 60 次调用打印一次 MuJoCo 位姿 vs Pawn 实际位姿, + // 用来确认「Pawn 真的跟着 MuJoCo 走」。设 LogRovBridge 为 Log 级别即可看到。 + static int32 CallCounter = 0; + if (++CallCounter % 60 == 0) { + const AActor* Owner = GetOwner(); + const FVector PawnPos = Owner ? Owner->GetActorLocation() : FVector::ZeroVector; + AAMjManager* Mgr = AAMjManager::GetManager(); + const float SimTime = Mgr ? Mgr->GetSimTime() : -1.0f; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] 跟随中 #%d 仿真时间=%.3fs MuJoCo=(%s) Pawn=(%s) 差值=%.2f cm"), + CallCounter, + SimTime, + *OutPosition.ToString(), + *PawnPos.ToString(), + FVector::Dist(OutPosition, PawnPos)); + } + + return true; +} + +void URovMuJoCoBridge::DisablePawnPhysics() +{ + AActor* Owner = GetOwner(); + if (!Owner) return; + + int32 DisabledPrimitives = 0; + + TArray Primitives; + Owner->GetComponents(Primitives); + + for (UPrimitiveComponent* Prim : Primitives) { + if (!Prim) continue; + + if (Prim->IsSimulatingPhysics()) { + Prim->SetSimulatePhysics(false); + } + // 关掉碰撞:MuJoCo 已经负责碰撞,UE 侧再算一次会双计。 + if (Prim->GetCollisionEnabled() != ECollisionEnabled::NoCollision) { + Prim->SetCollisionEnabled(ECollisionEnabled::NoCollision); + ++DisabledPrimitives; + } + } + + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] 已关闭 Pawn '%s' 的物理与碰撞(%d 个图元)," + "避免与 MuJoCo 双计"), + *Owner->GetName(), DisabledPrimitives); +} + +FString URovMuJoCoBridge::GetResolvedBodyName() const +{ + return TargetBody.IsValid() ? TargetBody->GetName() : FString(); +} + +// ============================================================================= +// 键盘推进 +// ============================================================================= +// +// 为什么走 xfrc_applied 而不是 AirSim 的 RC 通道: +// 1. 桥接把 ARovPawn 的物理和碰撞全关了,AirSim 算出的推力没有物理体承接; +// 2. MuJoCo 模型里 7 个 actuator 全是机械臂关节,推进器只是视觉几何体, +// 没有对应的 ,没法用 ctrl 驱动。 +// +// xfrc_applied 是 MuJoCo 给每个 body 预留的 6 维外力(力 xyz + 力矩 xyz), +// mj_step 会把它叠加进动力学,且**不会自动清零**,所以每帧必须先清再写。 +// +// 力的量级按质量缩放(力 = 质量 × 加速度),这样换一台不同质量的 ROV 手感一致。 + +void URovMuJoCoBridge::TickComponent(float DeltaTime, ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) +{ + Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + + if (bBridgeActive && bEnableKeyboardControl) { + ApplyControlForce(); + } +} + +void URovMuJoCoBridge::ApplyControlForce() +{ + if (!TargetBody.IsValid() || !TargetBody->IsBound()) { + return; + } + + AAMjManager* Mgr = AAMjManager::GetManager(); + if (!Mgr || !Mgr->PhysicsEngine) { + return; + } + + mjModel* Model = Mgr->PhysicsEngine->GetModel(); + mjData* Data = Mgr->PhysicsEngine->GetData(); + if (!Model || !Data) { + return; + } + + const int32 BodyId = TargetBody->GetMjID(); + if (BodyId < 0 || BodyId >= Model->nbody) { + return; + } + + const double Gravity = FMath::Abs((double)Model->opt.gravity[2]); + + // ---------------- 一次性初始化:打开原生流体模型 + 受力诊断 ---------------- + static bool bFluidInitialized = false; + if (!bFluidInitialized) { + bFluidInitialized = true; + + // 命令行覆盖,便于现场调参(不用重编) + auto FloatParam = [](const TCHAR* Key, float Default) -> float { + float V = Default; + return FParse::Value(FCommandLine::Get(), Key, V) ? V : Default; + }; + DisplacedVolume = FloatParam(TEXT("RovDisplacedVolume="), DisplacedVolume); + BuoyancyScale = FloatParam(TEXT("RovBuoyancyScale="), BuoyancyScale); + BuoyancyArm = FloatParam(TEXT("RovBuoyancyArm="), BuoyancyArm); + AngularDampingCoeff = FloatParam(TEXT("RovAngularDamping="), AngularDampingCoeff); + ArmJointDamping = FloatParam(TEXT("RovArmDamping="), ArmJointDamping); + ThrusterMaxRatio = FloatParam(TEXT("RovThrustRatio="), ThrusterMaxRatio); + WaterSurfaceZ = FloatParam(TEXT("RovWaterSurfaceZ="), WaterSurfaceZ); + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][调参] 排水体积=%.4f m³ 浮力倍率=%.2f 推力比=%.2f 水面Z=%.2f m"), + DisplacedVolume, BuoyancyScale, ThrusterMaxRatio, WaterSurfaceZ); + + if (bUseNativeFluid) { + Model->opt.density = FluidDensity; + Model->opt.viscosity = FluidViscosity; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][流体] 已启用 MuJoCo 椭球流体模型 density=%.1f kg/m³ viscosity=%.6f Pa·s"), + Model->opt.density, Model->opt.viscosity); + } + else { + Model->opt.density = 0.0; + Model->opt.viscosity = 0.0; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][流体] 已关闭(density=viscosity=0)")); + } + + // 质量诊断:确认 UE 里这个模型到底多重 + const double TotalMass = Model->body_subtreemass[BodyId]; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][质量] 目标 body id=%d body_mass=%.3f kg body_subtreemass=%.3f kg nbody=%d"), + BodyId, Model->body_mass[BodyId], TotalMass, Model->nbody); + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][惯量] body_inertia=(%.5f, %.5f, %.5f) kg·m² " + "(角阻尼 Kd = %.1f × 平均惯量 %.5f = %.3f N·m·s/rad)"), + Model->body_inertia[3 * BodyId + 0], + Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + AngularDampingCoeff, + (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + + Model->body_inertia[3 * BodyId + 2]) / 3.0, + AngularDampingCoeff * (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + + Model->body_inertia[3 * BodyId + 2]) / 3.0); + for (int32 i = 1; i < Model->nbody; ++i) { + if (Model->body_mass[i] > 0.5) { + const char* Name = (Model->names && Model->name_bodyadr[i] >= 0) + ? (Model->names + Model->name_bodyadr[i]) : "?"; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][质量] body[%2d] %-24s mass=%.3f subtreemass=%.3f"), + i, ANSI_TO_TCHAR(Name), Model->body_mass[i], Model->body_subtreemass[i]); + } + } + + // 机械臂关节阻尼 + 刚度:模型的机械臂关节没有 ctrl 输入, + // 会自由摆动,反作用力矩把 ROV 本体晃得不停翻滚。 + // 阻尼抵抗运动、刚度把它锁在参考角度,两者合起来相当于位置伺服。 + { + int32 DampedDofs = 0; + int32 StiffJoints = 0; + for (int32 j = 0; j < Model->njnt; ++j) { + if (Model->jnt_type[j] == mjJNT_FREE) { + continue; + } + const int32 Adr = Model->jnt_dofadr[j]; + const int32 Num = (Model->jnt_type[j] == mjJNT_BALL) ? 3 : 1; + for (int32 k = 0; k < Num; ++k) { + Model->dof_damping[Adr + k] = ArmJointDamping; + ++DampedDofs; + } + Model->jnt_stiffness[j] = (mjtNum)ArmJointStiffness; + ++StiffJoints; + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][机械臂] %d 个关节:阻尼 %.1f N·m·s/rad、刚度 %.1f N·m/rad(%d 个自由度)"), + StiffJoints, ArmJointDamping, ArmJointStiffness, DampedDofs); + } + + // 机械臂重力补偿:抵消其自重,消除对艇体的恒定倾覆力矩 + if (bArmGravityComp) { + int32 CompBodies = 0; + for (int32 b = 1; b < Model->nbody; ++b) { + if (b == BodyId) { + continue; // ROV 本体不补偿 + } + Model->body_gravcomp[b] = 1.0; + ++CompBodies; + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][机械臂] 已对 %d 个 body 做重力补偿(gravcomp=1.0)"), + CompBodies); + } + + // 复合惯量:从自由关节的 6×6 质量阵取转动对角元。 + // body_inertia 只有 ROV 本体,不含机械臂,用它算增益会严重偏小 —— + // 这正是消融实验里"纯刚体 + PD 也不收敛"的原因。 + // + // 安全性:qM 只有在 mj_forward/mj_step 之后才有效。打包版首帧的时序 + // 可能与编辑器不同,若此时 qM 未就绪会读到垃圾值 → 增益变 NaN → + // 力矩爆炸。所以这里校验:非空、有限、为正,否则退回 body_inertia。 + { + const int32 Nv = Model->nv; + bool bOk = (Nv >= 6) && (Data->qM != nullptr); + if (bOk) { + TArray M; + M.SetNumZeroed(Nv * Nv); + mj_fullM(Model, M.GetData(), Data->qM); + for (int32 a = 0; a < 3; ++a) { + const double V = M[(3 + a) * Nv + (3 + a)]; + if (!FMath::IsFinite(V) || V <= 0.0) { + bOk = false; + break; + } + EffInertia[a] = V; + } + } + if (!bOk) { + for (int32 a = 0; a < 3; ++a) { + EffInertia[a] = Model->body_inertia[3 * BodyId + a]; + } + UE_LOG(LogRovBridge, Warning, + TEXT("[RovMuJoCoBridge][复合惯量] qM 未就绪,回退到 body_inertia")); + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][复合惯量] 转动对角元 = (%.3f, %.3f, %.3f) kg·m² " + "(body_inertia = %.3f/%.3f/%.3f,来源=%s)"), + EffInertia[0], EffInertia[1], EffInertia[2], + Model->body_inertia[3 * BodyId + 0], + Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + bOk ? TEXT("6x6 质量阵") : TEXT("body_inertia 回退")); + } + + // 洋流模型初始化(三层级联) + if (FParse::Param(FCommandLine::Get(), TEXT("RovOceanCurrent"))) { + bEnableOceanCurrent = true; // 命令行强制开启,便于测试 + } + if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { + bEnableAttitudeHold = false; // 自检时关掉姿态控制器,避免干扰 + } + OceanCurrent.bEnabled = bEnableOceanCurrent; + OceanCurrent.MeanSpeed = OceanCurrentMeanSpeed; + OceanCurrent.Seed = 42; + OceanCurrent.Initialize(); + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][洋流] %s 平均流速=%.2f m/s 分层=%d 层 湍流强度=%.2f"), + bEnableOceanCurrent ? TEXT("已启用") : TEXT("已关闭"), + OceanCurrentMeanSpeed, OceanCurrent.Layers.Num(), OceanCurrent.TurbIntensity); + + // 单位诊断:确认 MuJoCo 模型到底是米还是厘米 + { + int32 RovGeomId = -1; + for (int32 g = 0; g < Model->ngeom; ++g) { + const char* GName = (Model->names && Model->name_geomadr[g] >= 0) + ? (Model->names + Model->name_geomadr[g]) : ""; + if (FString(ANSI_TO_TCHAR(GName)).EndsWith(TEXT("rov_body"))) { + RovGeomId = g; + break; + } + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][单位] gravity=%.3f timestep=%.5f stat.extent=%.3f " + "模型包围盒中心=(%.2f,%.2f,%.2f)"), + Model->opt.gravity[2], Model->opt.timestep, Model->stat.extent, + Model->stat.center[0], Model->stat.center[1], Model->stat.center[2]); + if (RovGeomId >= 0) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][单位] rov_body geom size=(%.4f, %.4f, %.4f) " + "(若≈0.4/0.3/0.2 则模型是米;若≈40/30/20 则是厘米)"), + Model->geom_size[3 * RovGeomId + 0], + Model->geom_size[3 * RovGeomId + 1], + Model->geom_size[3 * RovGeomId + 2]); + } + if (AActor* Owner = GetOwner()) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][单位] Pawn 世界坐标=(%.2f, %.2f, %.2f) cm " + "MuJoCo xpos=(%.2f, %.2f, %.2f)"), + Owner->GetActorLocation().X, Owner->GetActorLocation().Y, Owner->GetActorLocation().Z, + Data->xpos[3 * BodyId + 0], Data->xpos[3 * BodyId + 1], Data->xpos[3 * BodyId + 2]); + } + } + + // 受力诊断:三个数摆出来,一眼能看出浮沉 + const double Weight = TotalMass * Gravity; + const double Buoyancy = FluidDensity * Gravity * DisplacedVolume * BuoyancyScale; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][受力] 重力=%.1f N 浮力=%.1f N 净力=%+.1f N(正=上浮,负=下沉) " + "排水体积=%.4f m³ 浮力倍率=%.2f"), + Weight, Buoyancy, Buoyancy - Weight, DisplacedVolume, BuoyancyScale); + } + + APlayerController* PC = GetWorld() ? GetWorld()->GetFirstPlayerController() : nullptr; + if (!PC) { + return; + } + + // 本体坐标系下的期望方向 + // 注意:A/D 的符号和直觉相反——这个模型的 Y 轴朝左,所以 A 用 +1、D 用 -1。 + // 垂直用 Q/E(PageUp/PageDown 在笔记本上常常没有,保留作别名)。 + double LocalX = 0.0, LocalY = 0.0, LocalZ = 0.0; + if (PC->IsInputKeyDown(EKeys::W)) { LocalX += 1.0; } + if (PC->IsInputKeyDown(EKeys::S)) { LocalX -= 1.0; } + if (PC->IsInputKeyDown(EKeys::A)) { LocalY += 1.0; } + if (PC->IsInputKeyDown(EKeys::D)) { LocalY -= 1.0; } + if (PC->IsInputKeyDown(EKeys::Q) || PC->IsInputKeyDown(EKeys::PageUp)) { LocalZ += 1.0; } + if (PC->IsInputKeyDown(EKeys::E) || PC->IsInputKeyDown(EKeys::PageDown)) { LocalZ -= 1.0; } + + // 按键诊断:状态一变就记一条(不节流),用来确认输入到底有没有进到游戏 + { + // 同时读两条通道:PlayerController 和 PlayerInput,任一为真即算按下。 + // 有些 build 里 IsInputKeyDown 取不到状态,PlayerInput 可以兜底。 + auto IsDown = [PC](const FKey& Key) -> bool { + bool b = PC->IsInputKeyDown(Key); + if (!b && PC->PlayerInput) { + b = PC->PlayerInput->IsPressed(Key); + } + return b; + }; + + static int32 PrevKeyMask = 0; + const int32 KeyMask = + (IsDown(EKeys::W) ? 1 : 0) | + (IsDown(EKeys::S) ? 2 : 0) | + (IsDown(EKeys::A) ? 4 : 0) | + (IsDown(EKeys::D) ? 8 : 0) | + (IsDown(EKeys::Q) ? 16 : 0) | + (IsDown(EKeys::E) ? 32 : 0) | + (IsDown(EKeys::Z) ? 64 : 0); + if (KeyMask != PrevKeyMask) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][按键] 状态 %d -> %d (W=1 S=2 A=4 D=8 Q=16 E=32 Z=64)"), + PrevKeyMask, KeyMask); + PrevKeyMask = KeyMask; + } + + // 每秒打印一次焦点状态,确认游戏窗口到底有没有拿到输入焦点 + static int32 FocusLogTick = -1; + const double NowSec = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + const int32 CurTick = (int32)NowSec; + if (CurTick != FocusLogTick) { + FocusLogTick = CurTick; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][焦点] IsLocalController=%d PlayerInput=%s " + "bShowMouseCursor=%d bIgnoreInput=%d 按键掩码=%d"), + PC->IsLocalController() ? 1 : 0, + PC->PlayerInput ? TEXT("valid") : TEXT("NULL"), + PC->bShowMouseCursor ? 1 : 0, + PC->IsInputKeyDown(EKeys::LeftMouseButton) ? 1 : 0, + KeyMask); + } + } + + // 命令行自检:-RovThrustTest 会在启动后持续上浮 2 秒并打印 Z + static const bool bAutoThrustTest = + FParse::Param(FCommandLine::Get(), TEXT("RovThrustTest")); + static double AutoTestStartTime = -1.0; + static int32 AutoTestLastTick = -1; + if (bAutoThrustTest) { + UWorld* W = GetWorld(); + const double Now = W ? W->GetTimeSeconds() : 0.0; + if (AutoTestStartTime < 0.0) { + AutoTestStartTime = Now; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][自检] 开始:上浮 2 秒,起始 Z=%.3f"), Data->xpos[3 * BodyId + 2]); + } + if (Now - AutoTestStartTime < 10.0) { + LocalZ = 1.0; + } + const int32 Tick = (int32)((Now - AutoTestStartTime) * 2.0); + if (Tick != AutoTestLastTick) { + AutoTestLastTick = Tick; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][自检] t=%.1fs Z=%.3f 总Fz=%.1f N vz=%.3f"), + Now - AutoTestStartTime, Data->xpos[3 * BodyId + 2], + CachedForceZ.load(), Data->cvel[6 * BodyId + 5]); + } + } + + const double Mass = Model->body_subtreemass[BodyId]; + if (Mass <= 0.0) { + return; + } + + const mjtNum* Q = &Data->xquat[4 * BodyId]; // (w, x, y, z) + const double Qw = Q[0], Qx = Q[1], Qy = Q[2], Qz = Q[3]; + + // 只取偏航角,忽略俯仰和横滚 —— 否则 ROV 一翻滚推力方向就乱 + const double Yaw = FMath::Atan2(2.0 * (Qw * Qz + Qx * Qy), + 1.0 - 2.0 * (Qy * Qy + Qz * Qz)); + const double Cy = FMath::Cos(Yaw); + const double Sy = FMath::Sin(Yaw); + + // ------------------------------------------------------------------ + // 受力分解:每一项独立计算,最后相加。不直接设定合力。 + // ------------------------------------------------------------------ + + // 【1】推进器推力:键盘只给比例,力 = 比例 × 最大推力 + const double ThrusterMax = Mass * Gravity * ThrusterMaxRatio; + double ThrusterX = ThrusterMax * (LocalX * Cy - LocalY * Sy); + double ThrusterY = ThrusterMax * (LocalX * Sy + LocalY * Cy); + double ThrusterZ = ThrusterMax * LocalZ; + + const double RovZ = Data->xpos[3 * BodyId + 2]; + const FVector RovPos(Data->xpos[3 * BodyId + 0], + Data->xpos[3 * BodyId + 1], + Data->xpos[3 * BodyId + 2]); + + const double FrameDt = GetWorld() ? FMath::Max(GetWorld()->GetDeltaSeconds(), 1e-4f) : 0.016; + + // 【航向调整】← / → 连续改变目标航向(姿态保持开启时生效) + if (bEnableAttitudeHold) { + const bool bYawLeft = PC->IsInputKeyDown(EKeys::Left); + const bool bYawRight = PC->IsInputKeyDown(EKeys::Right); + if (bYawLeft || bYawRight) { + const double Step = (double)YawRateDegPerSec * FrameDt; + if (bYawLeft) { TargetYawDeg -= (float)Step; } + if (bYawRight) { TargetYawDeg += (float)Step; } + while (TargetYawDeg > 180.0f) { TargetYawDeg -= 360.0f; } + while (TargetYawDeg < -180.0f) { TargetYawDeg += 360.0f; } + static int32 YawLogCounter = 0; + if ((++YawLogCounter % 30) == 0) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][航向] 目标航向 = %.1f°(←/→ 调整)"), TargetYawDeg); + } + } + } + + // 【2】洋流:三层级联求当前流速 + const FVector VCurrent = OceanCurrent.GetVelocity(RovPos, (float)FrameDt); + + // 【3】定深(PID):按 Z 键锁定当前高度,再按解除 + if (bEnableDepthHold && PC) { + const bool bDepthKeyDown = PC->IsInputKeyDown(EKeys::Z); + if (bDepthKeyDown && !bPrevDepthKeyDown) { + bDepthHoldActive = !bDepthHoldActive; + DepthPID.Reset(); + if (bDepthHoldActive) { + DepthHoldTargetZ = RovZ; + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][定深] %s(目标 Z=%.3f m)"), + bDepthHoldActive ? TEXT("已锁定") : TEXT("已解除"), DepthHoldTargetZ); + } + bPrevDepthKeyDown = bDepthKeyDown; + } + + // 命令行自检:-RovDepthHoldTest 在 3 秒后自动锁定当前深度并打印误差, + // 用来验证 PID 定深,不依赖键盘。 + static const bool bDepthHoldTest = + FParse::Param(FCommandLine::Get(), TEXT("RovDepthHoldTest")); + static int32 DepthTestLastTick = -1; + if (bDepthHoldTest) { + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + if (Now > 1.0 && !bDepthHoldActive) { + bDepthHoldActive = true; + DepthPID.Reset(); + DepthHoldTargetZ = RovZ; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][定深自检] 锁定 Z=%.3f m"), DepthHoldTargetZ); + } + } + + // 命令行自检:-RovSpinTest 在 2 秒时给一个 3 rad/s 的三轴角速度脉冲, + // 然后打印角速度衰减,验证流体模型的旋转阻力(验收项 P7)。 + static const bool bSpinTest = FParse::Param(FCommandLine::Get(), TEXT("RovSpinTest")); + static bool bSpinApplied = false; + static int32 SpinLogTick = -1; + if (bSpinTest && Model->body_jntnum[BodyId] >= 1) { + const int32 SpinJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[SpinJnt] == mjJNT_FREE) { + const int32 SpinDof = Model->jnt_dofadr[SpinJnt]; + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + if (!bSpinApplied && Now > 2.0) { + bSpinApplied = true; + // 先抬到水中(Z=4m)并清零线速度,避免海底接触力矩污染测量 + const int32 QPosAdr = Model->jnt_qposadr[SpinJnt]; + Data->qpos[QPosAdr + 0] = 0.0; + Data->qpos[QPosAdr + 1] = 0.0; + Data->qpos[QPosAdr + 2] = 4.0; + Data->qpos[QPosAdr + 3] = 1.0; // quat w + Data->qpos[QPosAdr + 4] = 0.0; + Data->qpos[QPosAdr + 5] = 0.0; + Data->qpos[QPosAdr + 6] = 0.0; + Data->qvel[SpinDof + 0] = 0.0; + Data->qvel[SpinDof + 1] = 0.0; + Data->qvel[SpinDof + 2] = 0.0; + Data->qvel[SpinDof + 3] = 3.0; + Data->qvel[SpinDof + 4] = 3.0; + Data->qvel[SpinDof + 5] = 3.0; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][旋转自检] 移至水中 Z=4m,施加 3 rad/s 三轴角速度脉冲")); + } + if (bSpinApplied) { + const int32 Tick = (int32)(Now * 4.0); + if (Tick != SpinLogTick) { + SpinLogTick = Tick; + const double Wx = Data->qvel[SpinDof + 3]; + const double Wy = Data->qvel[SpinDof + 4]; + const double Wz = Data->qvel[SpinDof + 5]; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][旋转自检] t=%.2fs |ω|=%.4f rad/s"), + Now - 2.0, FMath::Sqrt(Wx * Wx + Wy * Wy + Wz * Wz)); + } + } + } + } + + // 命令行自检:-RovTiltTest 在 2 秒时把 ROV 倾斜 20° 后释放, + // 验证被动稳定(浮心高于重心产生的回复力矩)能否把它扶正。 + static const bool bTiltTest = FParse::Param(FCommandLine::Get(), TEXT("RovTiltTest")); + static bool bTiltApplied = false; + static int32 TiltLogTick = -1; + if (bTiltTest && Model->body_jntnum[BodyId] >= 1) { + const int32 TJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[TJnt] == mjJNT_FREE) { + const int32 TDof = Model->jnt_dofadr[TJnt]; + const int32 TQPos = Model->jnt_qposadr[TJnt]; + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + + if (!bTiltApplied && Now > 2.0) { + bTiltApplied = true; + const double HalfAngle = 10.0 * PI / 180.0; // 20° 的一半 + Data->qpos[TQPos + 2] = 4.0; // 抬到水中,避免接触干扰 + Data->qpos[TQPos + 3] = FMath::Cos(HalfAngle); // w + Data->qpos[TQPos + 4] = FMath::Sin(HalfAngle); // x(绕 X 轴倾斜) + Data->qpos[TQPos + 5] = 0.0; + Data->qpos[TQPos + 6] = 0.0; + for (int32 k = 0; k < 6; ++k) { + Data->qvel[TDof + k] = 0.0; + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][倾斜自检] 已倾斜 20° 并释放(BG=%.3f m)"), BuoyancyArm); + } + + if (bTiltApplied) { + const int32 Tick = (int32)(Now * 4.0); + if (Tick != TiltLogTick) { + TiltLogTick = Tick; + // 当前倾角:机体系 +Z 转到世界后与竖直方向的夹角 + const double Qw2 = Data->qpos[TQPos + 3]; + const double Qx2 = Data->qpos[TQPos + 4]; + const double Qy2 = Data->qpos[TQPos + 5]; + const double Qz2 = Data->qpos[TQPos + 6]; + // 旋转 (0,0,1):结果 z 分量 = 1 - 2*(x² + y²) + const double UpZ = 1.0 - 2.0 * (Qx2 * Qx2 + Qy2 * Qy2); + const double TiltDeg = FMath::RadiansToDegrees( + FMath::Acos(FMath::Clamp(UpZ, -1.0, 1.0))); + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][倾斜自检] t=%.2fs 倾角=%.2f° ω=%.4f rad/s"), + Now - 2.0, TiltDeg, Data->qvel[TDof + 4]); + } + } + } + } + + // 命令行自检:-RovTorqueAxis=0/1/2 只给该轴一个 100 N·m 力矩, + // 用来确定 qfrc_applied 角向分量到底在机体系还是世界系。 + static const bool bTorqueAxisTest = FParse::Param(FCommandLine::Get(), TEXT("RovTorqueAxisTest")); + static int32 TorqueTestAxis = -1; + if (bTorqueAxisTest && TorqueTestAxis < 0) { + int32 V = 0; + if (FParse::Value(FCommandLine::Get(), TEXT("RovTorqueAxis="), V)) { + TorqueTestAxis = V; + } else { + TorqueTestAxis = 0; + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][力矩轴自检] 将只给轴 %d 施加 100 N·m"), TorqueTestAxis); + } + + if (bDepthHoldActive) { + DepthPID.MaxThrust = (float)ThrusterMax; + const float PidThrust = DepthPID.Compute( + (float)DepthHoldTargetZ, (float)RovZ, (float)FrameDt, (float)VCurrent.Z); + ThrusterZ = FMath::Clamp((double)PidThrust, -ThrusterMax, ThrusterMax); + + // 定深自检日志(放在 PID 之后,才能看到真实的 PID 输出) + if (bDepthHoldTest) { + const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + const int32 Tick = (int32)(Now * 2.0); + if (Tick != DepthTestLastTick) { + DepthTestLastTick = Tick; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][定深自检] t=%.1fs 目标=%.3f 当前=%.3f 误差=%+.3f m " + "PID推力=%.1f N 洋流Vz=%.3f m/s"), + Now, DepthHoldTargetZ, RovZ, DepthHoldTargetZ - RovZ, ThrusterZ, VCurrent.Z); + } + } + } + + // 【4】水面钳制:ROV 已在水面以上时,不允许推进器继续向上推 + if (RovZ >= WaterSurfaceZ && ThrusterZ > 0.0) { + ThrusterZ = 0.0; + } + + // 【5】浮力:Fb = ρ·g·V,永远向上。 + // 但**水面之上没有水**,所以浮力为零 —— 这也是 ROV 不会飞出水的物理原因: + // 一旦越过水面,只剩重力把它拉回来。 + double Buoyancy = 0.0; + if (RovZ < WaterSurfaceZ) { + Buoyancy = FluidDensity * Gravity * DisplacedVolume * BuoyancyScale; + } + + // 【6】洋流拖曳:Fc = -k·(V_robot - V_current) + // 用相对流速,而不是只用洋流速度 —— 机器人自己游动时也应该受到阻力。 + double CurrentX = 0.0, CurrentY = 0.0, CurrentZ = 0.0; + { + double Vrx = 0.0, Vry = 0.0, Vrz = 0.0; + if (Model->body_jntnum[BodyId] >= 1) { + const int32 JntId = Model->body_jntadr[BodyId]; + if (Model->jnt_type[JntId] == mjJNT_FREE) { + const int32 DofAdr = Model->jnt_dofadr[JntId]; + Vrx = Data->qvel[DofAdr + 0]; + Vry = Data->qvel[DofAdr + 1]; + Vrz = Data->qvel[DofAdr + 2]; + } + } + CurrentX = -CurrentDragCoeff * (Vrx - VCurrent.X); + CurrentY = -CurrentDragCoeff * (Vry - VCurrent.Y); + CurrentZ = -CurrentDragCoeff * (Vrz - VCurrent.Z); + + // 洋流诊断:每秒打印一次流速与拖曳力,确认量级 + if (bEnableOceanCurrent) { + static int32 OceanLogTick = -1; + const int32 Tick = (int32)(GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0); + if (Tick != OceanLogTick) { + OceanLogTick = Tick; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][洋流诊断] V_current=(%.4f,%.4f,%.4f) |Vc|=%.4f " + "V_robot=(%.4f,%.4f,%.4f) |Vr|=%.4f F=(%.1f,%.1f,%.1f) N"), + VCurrent.X, VCurrent.Y, VCurrent.Z, VCurrent.Size(), + Vrx, Vry, Vrz, FMath::Sqrt(Vrx * Vrx + Vry * Vry + Vrz * Vrz), + CurrentX, CurrentY, CurrentZ); + } + } + } + + // 【7】水动力阻力/升力:由 MuJoCo 椭球流体模型自行计算,这里不重复施加 + + const double Fx = ThrusterX + CurrentX; + const double Fy = ThrusterY + CurrentY; + const double Fz = ThrusterZ + CurrentZ; // 浮力单独走 mj_applyFT(作用在浮心) + + // 【8】浮力的回复力矩由 mj_applyFT 在浮心处施加时自动产生,这里不再手算。 + + // 【9】附加角阻尼:以**力矩**形式施加(τ = -Kd·ω,机体系)。 + // MuJoCo 椭球流体模型的旋转阻力偏弱,真实 ROV 的导流罩/鳍会额外提供阻力。 + // Kd = AngularDampingCoeff × 平均主惯量,保持和之前"每秒衰减 1/s"的语义一致。 + double TorqueX = 0.0, TorqueY = 0.0, TorqueZ = 0.0; + if (AngularDampingCoeff > 0.0f && Model->body_jntnum[BodyId] >= 1) { + const int32 DampJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[DampJnt] == mjJNT_FREE) { + const int32 DampDof = Model->jnt_dofadr[DampJnt]; + // 同样要用**复合惯量**:body_inertia 只有本体,实测复合惯量约为其 4 倍, + // 按本体算会让角阻尼衰减率慢 4 倍(P7 退步的原因)。 + const double Iavg = (EffInertia[0] + EffInertia[1] + EffInertia[2]) / 3.0; + const double Kd = (double)AngularDampingCoeff * Iavg; + TorqueX += -Kd * Data->qvel[DampDof + 3]; + TorqueY += -Kd * Data->qvel[DampDof + 4]; + TorqueZ += -Kd * Data->qvel[DampDof + 5]; + } + } + + // 【10】姿态保持控制器(四元数 PD + 积分) + // + // 误差用四元数旋转矢量 e = 2·sign(qw)·qv,而不是欧拉角差: + // - 无万向节死锁(ROV 做大俯仰时欧拉角会奇异) + // - 小角度下 e 就是旋转矢量,|e| ≤ 2 天然限幅 + // - sign(qw) 取短弧,避免命令转 350° 而不是 10° + // + // 增益按惯量缩放(Kp = I·ω²、Kd = 2I·ζ·ω),换模型不用重调。 + // 积分项是必需的:机械臂挂在一侧产生恒定扰动力矩, + // 纯 PD 只能把它平衡在某个固定倾角上,消不掉稳态误差。 + if (bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { + const int32 AttJnt = Model->body_jntadr[BodyId]; + if (Model->jnt_type[AttJnt] == mjJNT_FREE) { + const int32 AttDof = Model->jnt_dofadr[AttJnt]; + + // 首次运行捕获当前航向作为目标 + if (!bAttitudeInitialized) { + bAttitudeInitialized = true; + const double InitYaw = FMath::Atan2(2.0 * (Qw * Qz + Qx * Qy), + 1.0 - 2.0 * (Qy * Qy + Qz * Qz)); + TargetYawDeg = FMath::RadiansToDegrees(InitYaw); + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][姿态] 已锁定目标航向 %.1f°,带宽 %.2f rad/s,阻尼比 %.2f"), + TargetYawDeg, AttitudeBandwidth, AttitudeZeta); + } + + const double HalfYaw = FMath::DegreesToRadians(TargetYawDeg) * 0.5; + const double Qtw = FMath::Cos(HalfYaw); + const double Qtz = FMath::Sin(HalfYaw); + + // q_e = conj(q_c) ⊗ q_t (机体系误差) + auto QMul = [](double aw, double ax, double ay, double az, + double bw, double bx, double by, double bz, + double& ow, double& ox, double& oy, double& oz) { + ow = aw * bw - ax * bx - ay * by - az * bz; + ox = aw * bx + ax * bw + ay * bz - az * by; + oy = aw * by - ax * bz + ay * bw + az * bx; + oz = aw * bz + ax * by - ay * bx + az * bw; + }; + double Qew = 0.0, Qex = 0.0, Qey = 0.0, Qez = 0.0; + QMul(Qw, -Qx, -Qy, -Qz, Qtw, 0.0, 0.0, Qtz, Qew, Qex, Qey, Qez); + if (Qew < 0.0) { // 取短弧 + Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; + } + + const double E[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; + + // 积分(限幅防饱和) + double IntArr[3] = { (double)AttitudeIntegral.X, + (double)AttitudeIntegral.Y, + (double)AttitudeIntegral.Z }; + for (int32 Axis = 0; Axis < 3; ++Axis) { + IntArr[Axis] += E[Axis] * FrameDt; + IntArr[Axis] = FMath::Clamp(IntArr[Axis], -2.0, 2.0); + } + AttitudeIntegral = FVector(IntArr[0], IntArr[1], IntArr[2]); + + const double W = (double)AttitudeBandwidth; + const double Zeta = (double)AttitudeZeta; + const double KiF = (double)AttitudeIntegralFactor; + + // body_inertia 是**主轴系**下的主值,body_iquat 给出主轴相对机体的旋转。 + // 直接用 body_inertia[3*BodyId+Axis] 当 X/Y/Z 惯量是错的(除非 iquat 是单位四元数)。 + // 正确做法:把机体单位轴转到主轴系,算 vᵀ·diag(I)·v 得到该轴的等效惯量。 + const mjtNum* IQ = &Model->body_iquat[4 * BodyId]; + double Iaxis[3] = { 0.0, 0.0, 0.0 }; + for (int32 a = 0; a < 3; ++a) { + double vx = (a == 0) ? 1.0 : 0.0; + double vy = (a == 1) ? 1.0 : 0.0; + double vz = (a == 2) ? 1.0 : 0.0; + double px = 0.0, py = 0.0, pz = 0.0; + RotByQuat(IQ[0], -IQ[1], -IQ[2], -IQ[3], vx, vy, vz, px, py, pz); + Iaxis[a] = Model->body_inertia[3 * BodyId + 0] * px * px + + Model->body_inertia[3 * BodyId + 1] * py * py + + Model->body_inertia[3 * BodyId + 2] * pz * pz; + } + + static bool bLoggedIquat = false; + if (!bLoggedIquat) { + bLoggedIquat = true; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][惯量诊断] body_iquat=(%.4f, %.4f, %.4f, %.4f) " + "主轴主值=(%.3f, %.3f, %.3f) 机体轴等效惯量=(%.3f, %.3f, %.3f)"), + IQ[0], IQ[1], IQ[2], IQ[3], + Model->body_inertia[3 * BodyId + 0], + Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + Iaxis[0], Iaxis[1], Iaxis[2]); + } + + for (int32 Axis = 0; Axis < 3; ++Axis) { + // 优先用复合惯量(含机械臂);取不到时退回本体的等效惯量 + const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] : Iaxis[Axis]; + const double Kp = I * W * W; + const double Kd = 2.0 * I * Zeta * W; + const double Ki = I * W * W * W * KiF; + const double Omega = Data->qvel[AttDof + 3 + Axis]; + const double T = Kp * E[Axis] + Ki * IntArr[Axis] - Kd * Omega; + if (Axis == 0) { TorqueX += T; } + else if (Axis == 1) { TorqueY += T; } + else { TorqueZ += T; } + } + } + } + + CachedBodyId.store(BodyId); + CachedForceX.store(Fx); + CachedForceY.store(Fy); + CachedForceZ.store(Fz); + + // 力矩轴自检:覆盖掉所有力矩项,只给指定轴 100 N·m + if (bTorqueAxisTest && TorqueTestAxis >= 0) { + TorqueX = (TorqueTestAxis == 0) ? 100.0 : 0.0; + TorqueY = (TorqueTestAxis == 1) ? 100.0 : 0.0; + TorqueZ = (TorqueTestAxis == 2) ? 100.0 : 0.0; + + static int32 TorqueLogTick = -1; + const int32 Tick = (int32)(GetWorld()->GetTimeSeconds() * 4.0); + if (Tick != TorqueLogTick && Model->body_jntnum[BodyId] >= 1) { + TorqueLogTick = Tick; + const int32 J = Model->body_jntadr[BodyId]; + if (Model->jnt_type[J] == mjJNT_FREE) { + const int32 D = Model->jnt_dofadr[J]; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][力矩轴自检] 给轴%d 100N·m -> omega=(%.4f, %.4f, %.4f)"), + TorqueTestAxis, Data->qvel[D + 3], Data->qvel[D + 4], Data->qvel[D + 5]); + } + } + } + + // 姿态控制器和角阻尼给出的力矩是**机体系**的,而 mj_applyFT 要世界系, + // 在这里统一转过去。之前直接把机体系力矩写进 qfrc_applied 后三维, + // 实测轴系对不上(给"轴 0"却主要绕 Y 转),这是本次重构要解决的根因。 + double TWx = 0.0, TWy = 0.0, TWz = 0.0; + RotByQuat(Qw, Qx, Qy, Qz, TorqueX, TorqueY, TorqueZ, TWx, TWy, TWz); + + CachedTorqueX.store(TWx); + CachedTorqueY.store(TWy); + CachedTorqueZ.store(TWz); + CachedBuoyancy.store(Buoyancy); + + if (!bControlCallbackRegistered) { + TWeakObjectPtr WeakThis(this); + Mgr->PhysicsEngine->RegisterPreStepCallback( + [WeakThis](mjModel* M, mjData* D) { + if (URovMuJoCoBridge* Self = WeakThis.Get()) { + Self->ApplyCachedForce(M, D); + } + }); + bControlCallbackRegistered = true; + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge] 已注册物理线程施力回调(PreStepCallbacks)")); + } + + // 节流日志 + static int32 CtrlLogCounter = 0; + if ((LocalX != 0.0 || LocalY != 0.0 || LocalZ != 0.0) && (++CtrlLogCounter % 30 == 0)) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][受力] 输入=(%.0f,%.0f,%.0f) 推力=(%.1f,%.1f,%.1f) N " + "浮力=%.1f N 洋流拖曳=(%.1f,%.1f,%.1f) N 合计Fz=%.1f N " + "洋流=(%.2f,%.2f,%.2f) m/s 定深=%d Z=%.3f"), + LocalX, LocalY, LocalZ, ThrusterX, ThrusterY, ThrusterZ, + Buoyancy, CurrentX, CurrentY, CurrentZ, Fz, + VCurrent.X, VCurrent.Y, VCurrent.Z, bDepthHoldActive ? 1 : 0, RovZ); + } +} + +void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) +{ + const int32 BodyId = CachedBodyId.load(); + if (BodyId < 0 || !Model || !Data || BodyId >= Model->nbody) { + return; + } + + // ========================================================================= + // 通道:mj_applyFT 直接累加进 qfrc_applied。 + // + // 为什么不用其它两种写法: + // - xfrc_applied:元素顺序有两套说法(官方文档力在前,URLab 的 + // MjBind.h:574 注释写力矩在前),写反会让"向前"变成"俯仰"。 + // - 直接写 qfrc_applied 后三维:实测轴系对不上 —— 只给"轴 0" + // 100 N·m,结果 omega 主要绕 Y 转(-1.03 对 -0.24)。 + // 姿态控制器的力矩因此打到了错误轴上,积分也在错误轴上卷。 + // + // mj_applyFT 把 force / torque / point 分开传参、**全部世界系**, + // 自动补 r×F,没有布局和轴系歧义,还能让浮力正确地作用在浮心。 + // ========================================================================= + if (Model->body_jntnum[BodyId] < 1) { + return; + } + const int32 JntId = Model->body_jntadr[BodyId]; + if (Model->jnt_type[JntId] != mjJNT_FREE) { + return; + } + const int32 DofAdr = Model->jnt_dofadr[JntId]; + + // mj_applyFT 是累加语义,先清零本 body 的 6 维广义力 + for (int32 k = 0; k < 6; ++k) { + Data->qfrc_applied[DofAdr + k] = 0.0; + } + + const mjtNum CG[3] = { + Data->xipos[3 * BodyId + 0], + Data->xipos[3 * BodyId + 1], + Data->xipos[3 * BodyId + 2] + }; + const mjtNum ZeroT[3] = { 0.0, 0.0, 0.0 }; + const mjtNum ZeroF[3] = { 0.0, 0.0, 0.0 }; + + // 【1】推进器推力 + 洋流拖曳:作用在质心 + mjtNum F1[3] = { + (mjtNum)CachedForceX.load(), + (mjtNum)CachedForceY.load(), + (mjtNum)CachedForceZ.load() + }; + mj_applyFT(Model, Data, F1, ZeroT, CG, BodyId, Data->qfrc_applied); + + // 【2】浮力:作用在**浮心**(质心 + R·(0,0,BG)),自动产生回复力矩 + const double Buoy = CachedBuoyancy.load(); + if (Buoy > 0.0) { + double Rx = 0.0, Ry = 0.0, Rz = 0.0; + RotByQuat(Data->xquat[4 * BodyId + 0], Data->xquat[4 * BodyId + 1], + Data->xquat[4 * BodyId + 2], Data->xquat[4 * BodyId + 3], + 0.0, 0.0, (double)BuoyancyArm, Rx, Ry, Rz); + mjtNum F2[3] = { 0.0, 0.0, (mjtNum)Buoy }; + mjtNum CBw[3] = { + CG[0] + (mjtNum)Rx, + CG[1] + (mjtNum)Ry, + CG[2] + (mjtNum)Rz + }; + mj_applyFT(Model, Data, F2, ZeroT, CBw, BodyId, Data->qfrc_applied); + } + + // 【3】姿态控制器 + 角阻尼:纯力矩,作用点取质心 + const double Tx = CachedTorqueX.load(); + const double Ty = CachedTorqueY.load(); + const double Tz = CachedTorqueZ.load(); + if (Tx != 0.0 || Ty != 0.0 || Tz != 0.0) { + mjtNum T3[3] = { (mjtNum)Tx, (mjtNum)Ty, (mjtNum)Tz }; + mj_applyFT(Model, Data, ZeroF, T3, CG, BodyId, Data->qfrc_applied); + } + + // 诊断 + static int32 ApplyLogCounter = 0; + if (++ApplyLogCounter % 500 == 0) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][通道] mj_applyFT dof=%d F=(%.1f,%.1f,%.1f) N " + "浮力=%.1f N T=(%.1f,%.1f,%.1f) N·m Z=%.3f"), + DofAdr, + Data->qfrc_applied[DofAdr + 0], Data->qfrc_applied[DofAdr + 1], Data->qfrc_applied[DofAdr + 2], + Buoy, Tx, Ty, Tz, + Data->xpos[3 * BodyId + 2]); + } +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h new file mode 100644 index 00000000..1ab3d139 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h @@ -0,0 +1,367 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// 把 AirSim 的 ROV Pawn 改造成 MuJoCo 的「纯跟随者」。 +// +// 设计依据:docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md +// +// 背景 +// ---- +// 改造前,同一台 ROV 的位姿有两个写入者: +// - AirSim 的 rov_physics_body_(在 RovPawnSimApi 里积分) +// - MuJoCo 的 AMjArticulation(在 URLab 里积分) +// 被调研的 16 个开源仿真系统里没有一个允许这种「双所有权」配置 +// (docs/research/开源仿真器物理与视觉统一架构调研.html §7 第 3 条)。 +// +// 改造后 +// ------ +// MuJoCo 是唯一真值源;本组件每帧把 ROV 根 body 的世界位姿提供给 +// RovPawnSimApi,后者据此驱动 Pawn。AirSim 自身的 ROV 物理与 UE 侧碰撞 +// 全部关闭,避免「同一个力被算两次」(调研报告 §6 第 1 条)。 +// +// 关于 Tick 顺序 +// -------------- +// 不需要 AddTickPrerequisiteComponent:UMjBody::GetWorldPosition() / +// GetWorldRotation() 直接读 MuJoCo 的 m_BodyView.xpos / xquat 数组, +// 不依赖组件自身的 TickComponent 是否已经跑过,所以拿到的永远是最新物理状态。 + +#pragma once + +#include "CoreMinimal.h" +#include "Components/ActorComponent.h" +#include "Vehicles/Rov/RovUnderwaterPhysics.h" +#include +#include "RovMuJoCoBridge.generated.h" + +class AMjArticulation; +class UMjBody; + +// MuJoCo 的 mjModel / mjData 是 struct mjModel_ / struct mjData_ 的 typedef。 +// 这里只前向声明,避免把 mujoco.h 拉进 AirSim 的公共头。 +struct mjModel_; +struct mjData_; + +/** + * 让 AirSim ROV Pawn 跟随 MuJoCo 模型的桥接组件。 + * + * 挂在 ARovPawn 上(构造函数里自动创建)。BeginPlay 时: + * 1. 找到场景里的 AMjArticulation + * 2. 按后缀匹配找到目标 body(默认 "rov") + * 3. 关闭 Pawn 自身的物理模拟与碰撞 + * 4. 注册全局位姿源,供 RovPawnSimApi 每帧查询 + */ +UCLASS(ClassGroup = (AirSim), meta = (BlueprintSpawnableComponent)) +class AIRSIM_API URovMuJoCoBridge : public UActorComponent +{ + GENERATED_BODY() + +public: + URovMuJoCoBridge(); + + virtual void BeginPlay() override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + virtual void TickComponent(float DeltaTime, ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) override; + + /** 总开关。关掉则完全退回 AirSim 原生物理(改造前的行为)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bEnableBridge = true; + + /** + * 目标 body 名的后缀。 + * URLab 会给 spec 元素名加蓝图前缀(如 ImportTestArt_xxx_C_0_rov), + * 所以用后缀匹配而不是全等匹配。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + FString TargetBodyNameSuffix = TEXT("rov"); + + /** 是否在 BeginPlay 关闭 Pawn 的物理模拟与碰撞(避免与 MuJoCo 双计)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bDisablePawnPhysics = true; + + /** + * 是否自动启动 MuJoCo 仿真。 + * + * UMjPhysicsEngine 的 bIsPaused 默认是 true——URLab 的设计是让用户在 + * MjSimulate 控件里手动点「播放」。但 AirSim 的 Pawn 一旦跟随 MuJoCo, + * 仿真不跑 Pawn 就不会动,所以桥接激活时默认自动取消暂停。 + * 需要在编辑器里手动控制时把它关掉。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bAutoStartSimulation = true; + + /** + * MuJoCo 世界坐标到 UE 世界坐标的平移偏移(cm)。 + * + * MuJoCo 的模型是一个自包含的水下世界(自带 seafloor / reef / wellhead), + * 它的原点与 UE 地图没有关系。把 ROV 摆到城区的某个位置时, + * 不能靠挪编辑器里的 Actor——Pawn 的位姿每帧被 MuJoCo 覆盖, + * 必须在桥接这一层加偏移。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + FVector WorldOffset = FVector::ZeroVector; + + /** + * 键盘推进:W/S 前后、A/D 左右、Q/E 上浮下潜。 + * + * 按受力分解设计:键盘只决定「推进器输出比例」, + * 真正的推力 = 比例 × ThrusterMaxForce,不直接改合力。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") + bool bEnableKeyboardControl = true; + + /** 推进器最大推力占自重的比例。0.5 = 最大推力只有一半重力。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") + float ThrusterMaxRatio = 0.5f; + + /** + * 水面高度,**单位是米(MuJoCo 模型单位)**。 + * + * 已实测确认:MJCF 是米制(rov_body geom size=0.4/0.3/0.2,gravity=9.81, + * timestep=0.002),桥接输出到 UE 时才 ×100 转成厘米 + * (MuJoCo xpos Z=3.00 → Pawn 世界 Z=300cm)。 + * UE 场景水面在 Z=+800cm,对应这里 8.0。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") + float WaterSurfaceZ = -2.0f; // 地图自带海面 Z = -200 cm(Town10 外围水体) + + // ---------------------------- 流体 ---------------------------- + + /** 是否启用 MuJoCo 原生椭球流体模型(阻力/升力交给引擎算)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bUseNativeFluid = true; + + /** 流体密度(kg/m³)。1000 = 水。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float FluidDensity = 1000.0f; + + /** 流体动力粘度(Pa·s)。水约 0.00089。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float FluidViscosity = 0.00089f; + + // ---------------------------- 浮力 ---------------------------- + + /** 排水体积(m³),用于 Fb = ρ·g·V。默认 0.192 与 mujoco_plugin 的 config 一致。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float DisplacedVolume = 0.192f; + + /** + * 浮力倍率。1.0 = 严格按 ρ·g·V。 + * + * 当前默认 3.2 是为了补偿一个已知的质量归属 bug:XML 里 rov_body 的 + * mass="150.0" 被导入器分到了 gripper_base_mount 上,导致 ROV 本体变成 + * 438 kg、整机 612.9 kg,而排水体积只有 0.192 m³。 + * 3.2 ≈ 612.9 / (1000 × 0.192),配平到中性浮态。 + * 质量 bug 修好后应改回 1.0。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float BuoyancyScale = 3.1923f; + + /** + * 浮心相对重心的高度(m),沿机体系 +Z。 + * + * 真实 ROV 的浮心在重心之上,倾斜时浮力产生回复力矩 τ = r_CB × F_buoy, + * 这就是它「被撞歪后自己回正」的物理来源 —— 不需要任何控制器。 + * 回复刚度 ≈ B × BG,BlueROV2 量级 BG = 0.01~0.05 m。 + * 设为 0 则退化为「浮力加在质心」,无被动稳定。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float BuoyancyArm = 0.02f; + + /** + * 机械臂关节阻尼(N·m·s/rad),施加到所有非自由关节上。 + * + * 模型的 7 个 UR5e 关节没有任何 ctrl 输入,会自由摆动; + * 它们的反作用力矩会把 ROV 本体晃得不停翻滚 + * (实测:两个外部力矩全关、ω 仍能冲到 3.4 rad/s)。 + * 加阻尼让机械臂稳定下垂,不再激励艇体。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + float ArmJointDamping = 20000.0f; + + /** + * 机械臂关节刚度(N·m/rad),相当于在每个关节上装一个位置伺服弹簧, + * 把关节锁在 MJCF 的参考角度(qpos0)上。 + * + * 只加阻尼不够:阻尼只抵抗运动,机械臂仍会在重力下缓慢下垂到一个 + * 偏置姿态,那个偏置产生恒定扰动力矩,把 ROV 压在一个固定倾角上 + * (实测约 17°,反推扰动约 130 N·m)。加刚度后机械臂被真正"锁住", + * 扰动消失 —— 这也符合真实 ROV 的机械臂由伺服保持的实际情况。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + float ArmJointStiffness = 0.0f; + + /** + * 机械臂重力补偿:把机械臂各 body 的 gravcomp 设为 1.0,抵消其自重。 + * + * 机械臂(约 22 kg)挂在 ROV 一侧,静重产生约 130 N·m 的恒定扰动力矩, + * 把艇体压在一个固定倾角上。真实 ROV 的机械臂靠伺服与配重平衡掉这部分, + * 仿真里用 MuJoCo 的 gravcomp 等效实现,同时保留惯量与碰撞。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bArmGravityComp = false; + + // ---------------------------- 姿态控制 ---------------------------- + + /** + * 姿态保持:roll/pitch 回水平 + yaw 锁航向。 + * + * 分工按真实 ROV:被动稳定(浮心高于重心)负责 roll/pitch 的刚度, + * 本控制器只补阻尼和积分 —— 积分是必需的,因为机械臂挂在一侧会产生 + * 恒定扰动力矩,纯 PD 只能平衡在某个固定倾角上。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + bool bEnableAttitudeHold = true; + + /** 姿态闭环带宽(rad/s)。增益按 I·ω² 自动缩放,换惯量不用重调。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeBandwidth = 2.5f; + + /** 阻尼比。1.0 = 临界阻尼,不超调。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeZeta = 0.8f; + + /** 积分增益系数(相对 I·ω³)。用于消除恒定扰动造成的稳态误差。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeIntegralFactor = 0.5f; + + /** 目标航向(度)。启动时自动捕获当前航向。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float TargetYawDeg = 0.0f; + + /** 航向调整速率(度/秒)。按住 ← / → 连续改变目标航向。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float YawRateDegPerSec = 60.0f; + + // ---------------------------- 洋流 ---------------------------- + + /** 洋流拖曳系数(kg/s)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") + float CurrentDragCoeff = 10.0f; + + /** + * 附加角阻尼(1/s)。 + * + * MuJoCo 椭球流体模型本身有旋转阻力,但实测偏弱:给 3 rad/s 三轴脉冲后 + * 4 秒才衰减到 1.0 rad/s,撞一下管道仍然会明显打转。 + * 真实 ROV 的导流罩、推进器导管、外挂设备都会提供额外的旋转阻力, + * 所以这里作为**独立受力项**补一个可调角阻尼,与流体模型叠加。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float AngularDampingCoeff = 10.0f; + + /** + * 是否启用洋流(三层级联:Gauss-Markov + 分层 + 湍流)。 + * + * 实测正常:V_current≈0.6~0.7 m/s,拖曳力≈6~7 N, + * ROV 缓慢加速到约 0.1 m/s 随流漂移 —— 这是期望的水下行为。 + * (早前误判"被推飞",是把日志里 ×100 的厘米值当成了米。) + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") + bool bEnableOceanCurrent = true; + + /** 洋流平均流速(m/s)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") + float OceanCurrentMeanSpeed = 0.15f; + + // ---------------------------- 定深 ---------------------------- + + /** PID 定深是否可用(按 Z 键切换)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Depth") + bool bEnableDepthHold = true; + + /** 桥接是否已生效。 */ + UFUNCTION(BlueprintCallable, BlueprintPure, Category = "ROV|MuJoCo") + bool IsBridgeActive() const { return bBridgeActive; } + + /** 当前跟随的 body 名(调试用)。 */ + UFUNCTION(BlueprintCallable, BlueprintPure, Category = "ROV|MuJoCo") + FString GetResolvedBodyName() const; + + /** + * 全局位姿源。RovPawnSimApi 每帧调用它取位姿。 + * 给出的是 UE 世界坐标;AirSim 侧的 NED 转换由 PawnSimApi::toPoseFromUnreal 完成。 + * 为空表示没有外部源,RovPawnSimApi 退回读取自身物理体。 + */ + static std::function ExternalPoseProvider; + +private: + /** 供 ExternalPoseProvider 调用的实际取值函数。 */ + bool ProvidePose(FVector& OutPosition, FQuat& OutRotation) const; + + /** + * 找到 AMjArticulation 与目标 UMjBody。 + * + * 声明为 const + mutable 成员:MuJoCo 模型通常在 Pawn 生成之后才编译完成, + * 所以 ProvidePose(const)需要在运行中惰性重试解析。 + */ + void ResolveTargets() const; + + /** 关闭 Pawn 物理与碰撞,消除双计。 */ + void DisablePawnPhysics(); + + /** 每帧把键盘输入换算成力,写进 mjData::xfrc_applied。 */ + void ApplyControlForce(); + + /** + * 物理线程内施力。 + * + * 游戏线程直接写 xfrc_applied 会被物理线程的 mj_step 覆盖掉(两者不同线程、 + * 不同频率),实测按上浮完全推不动。URLab 的 UMjPhysicsEngine 提供了 + * PreStepCallbacks —— 在 mj_step 之前、同一把锁内执行,这是官方给的施力入口。 + * 游戏线程只负责算力并存进 CachedForce*,物理线程在这里读出来写进 MuJoCo。 + */ + void ApplyCachedForce(mjModel_* Model, mjData_* Data); + + /** 期望推力(N),游戏线程写、物理线程读。 */ + std::atomic CachedForceX{0.0}; + std::atomic CachedForceY{0.0}; + std::atomic CachedForceZ{0.0}; + + /** 期望力矩(N·m),**世界系**(mj_applyFT 的入参就是世界系)。 */ + std::atomic CachedTorqueX{0.0}; + std::atomic CachedTorqueY{0.0}; + std::atomic CachedTorqueZ{0.0}; + + /** 浮力大小(N)。单独存,因为要作用在浮心而不是质心。 */ + std::atomic CachedBuoyancy{0.0}; + + std::atomic CachedBodyId{-1}; + + /** pre-step 回调只注册一次。 */ + bool bControlCallbackRegistered = false; + + /** 洋流模型(三层级联)。 */ + FRovOceanCurrent OceanCurrent; + + /** PID 定深控制器。 */ + FRovPIDDepthController DepthPID; + + /** 是否正在定深。 */ + bool bDepthHoldActive = false; + + /** 定深目标(MuJoCo 世界系 Z,m)。 */ + double DepthHoldTargetZ = 0.0; + + /** Z 键上一帧状态,用于边沿检测。 */ + bool bPrevDepthKeyDown = false; + + /** 姿态控制器积分项(机体系,rad·s)。 */ + FVector AttitudeIntegral = FVector::ZeroVector; + + /** 姿态控制器是否已初始化(首次捕获目标航向)。 */ + bool bAttitudeInitialized = false; + + /** + * 自由关节 6×6 质量阵的转动对角元(kg·m²),即 ROV+机械臂的**复合惯量**。 + * body_inertia 只有 ROV 本体,不含机械臂;用它算增益会严重偏小。 + */ + double EffInertia[3] = { 0.0, 0.0, 0.0 }; + + mutable TWeakObjectPtr Articulation; + mutable TWeakObjectPtr TargetBody; + + /** 只在第一次解析失败时打印候选列表,避免每帧刷屏。 */ + mutable bool bResolveWarned = false; + + bool bBridgeActive = false; +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp index da1ce4e0..e76ba058 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp @@ -3,15 +3,22 @@ #include "Components/ChildActorComponent.h" #include "UObject/ConstructorHelpers.h" #include "AirBlueprintLib.h" +#include "Vehicles/Rov/RovMuJoCoBridge.h" #include "common/CommonStructs.hpp" #include "common/Common.hpp" ARovPawn::ARovPawn() { + AutoPossessPlayer = EAutoReceiveInput::Player0; init_id_ = pawn_events_.getActuatorSignal().connect_member(this, &ARovPawn::setRotorRenderedStates); RootComponent = CreateDefaultSubobject(TEXT("RootComponent")); + // OpenHUTB 追加(2026-11-05):MuJoCo 跟随者桥接 + // 让本 Pawn 跟随 MuJoCo 模型而非 AirSim 自身物理,消除双所有权。 + // 见 docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + MuJoCoBridge_ = CreateDefaultSubobject(TEXT("MuJoCoBridge")); + body_mesh_ = CreateDefaultSubobject(TEXT("BodyMesh")); body_mesh_->SetupAttachment(RootComponent); @@ -128,10 +135,79 @@ void ARovPawn::initializeForBeginPlay() camera_bottom_center_ = c_bot ? Cast(c_bot->GetChildActor()) : nullptr; } +void ARovPawn::SetupPlayerInputComponent(UInputComponent* PlayerInputComponent) +{ + Super::SetupPlayerInputComponent(PlayerInputComponent); + if (!PlayerInputComponent) return; + + PlayerInputComponent->BindKey(EKeys::W, IE_Pressed, this, &ARovPawn::onForwardPressed); + PlayerInputComponent->BindKey(EKeys::W, IE_Released, this, &ARovPawn::onForwardReleased); + PlayerInputComponent->BindKey(EKeys::Up, IE_Pressed, this, &ARovPawn::onForwardPressed); + PlayerInputComponent->BindKey(EKeys::Up, IE_Released, this, &ARovPawn::onForwardReleased); + + PlayerInputComponent->BindKey(EKeys::S, IE_Pressed, this, &ARovPawn::onBackwardPressed); + PlayerInputComponent->BindKey(EKeys::S, IE_Released, this, &ARovPawn::onBackwardReleased); + PlayerInputComponent->BindKey(EKeys::Down, IE_Pressed, this, &ARovPawn::onBackwardPressed); + PlayerInputComponent->BindKey(EKeys::Down, IE_Released, this, &ARovPawn::onBackwardReleased); + + PlayerInputComponent->BindKey(EKeys::Q, IE_Pressed, this, &ARovPawn::onLeftPressed); + PlayerInputComponent->BindKey(EKeys::Q, IE_Released, this, &ARovPawn::onLeftReleased); + PlayerInputComponent->BindKey(EKeys::E, IE_Pressed, this, &ARovPawn::onRightPressed); + PlayerInputComponent->BindKey(EKeys::E, IE_Released, this, &ARovPawn::onRightReleased); + + PlayerInputComponent->BindKey(EKeys::A, IE_Pressed, this, &ARovPawn::onYawLeftPressed); + PlayerInputComponent->BindKey(EKeys::A, IE_Released, this, &ARovPawn::onYawLeftReleased); + PlayerInputComponent->BindKey(EKeys::Left, IE_Pressed, this, &ARovPawn::onYawLeftPressed); + PlayerInputComponent->BindKey(EKeys::Left, IE_Released, this, &ARovPawn::onYawLeftReleased); + + PlayerInputComponent->BindKey(EKeys::D, IE_Pressed, this, &ARovPawn::onYawRightPressed); + PlayerInputComponent->BindKey(EKeys::D, IE_Released, this, &ARovPawn::onYawRightReleased); + PlayerInputComponent->BindKey(EKeys::Right, IE_Pressed, this, &ARovPawn::onYawRightPressed); + PlayerInputComponent->BindKey(EKeys::Right, IE_Released, this, &ARovPawn::onYawRightReleased); + + PlayerInputComponent->BindKey(EKeys::SpaceBar, IE_Pressed, this, &ARovPawn::onUpPressed); + PlayerInputComponent->BindKey(EKeys::SpaceBar, IE_Released, this, &ARovPawn::onUpReleased); + PlayerInputComponent->BindKey(EKeys::PageUp, IE_Pressed, this, &ARovPawn::onUpPressed); + PlayerInputComponent->BindKey(EKeys::PageUp, IE_Released, this, &ARovPawn::onUpReleased); + + PlayerInputComponent->BindKey(EKeys::C, IE_Pressed, this, &ARovPawn::onDownPressed); + PlayerInputComponent->BindKey(EKeys::C, IE_Released, this, &ARovPawn::onDownReleased); + PlayerInputComponent->BindKey(EKeys::PageDown, IE_Pressed, this, &ARovPawn::onDownPressed); + PlayerInputComponent->BindKey(EKeys::PageDown, IE_Released, this, &ARovPawn::onDownReleased); +} + void ARovPawn::Tick(float DeltaSeconds) { Super::Tick(DeltaSeconds); pawn_events_.getPawnTickSignal().emit(DeltaSeconds); + + float fwd = (bForward_ ? 1.0f : 0.0f) - (bBackward_ ? 1.0f : 0.0f); + float lat = (bRight_ ? 1.0f : 0.0f) - (bLeft_ ? 1.0f : 0.0f); + float vert = (bDown_ ? 1.0f : 0.0f) - (bUp_ ? 1.0f : 0.0f); + float yaw = (bYawRight_ ? 1.0f : 0.0f) - (bYawLeft_ ? 1.0f : 0.0f); + + bool bHasInput = (std::abs(fwd) > 0.01f || std::abs(lat) > 0.01f || std::abs(vert) > 0.01f || std::abs(yaw) > 0.01f); + + if (bHasInput) { + std::vector pwms(8, 0.0f); + pwms[0] = vert; pwms[1] = vert; pwms[2] = vert; pwms[3] = vert; + pwms[4] += fwd * 1.0f + lat * -1.0f + yaw * 1.0f; + pwms[5] += fwd * 1.0f + lat * 1.0f + yaw * -1.0f; + pwms[6] += fwd * -1.0f + lat * 1.0f + yaw * -1.0f; + pwms[7] += fwd * -1.0f + lat * -1.0f + yaw * 1.0f; + + for (int i = 0; i < 8; ++i) { + pwms[i] = FMath::Clamp(pwms[i], -1.0f, 1.0f); + } + + pawn_events_.getKeyboardPwmSignal().emit(pwms); + bHadInput_ = true; + } + else if (bHadInput_) { + std::vector zero_pwms(8, 0.0f); + pawn_events_.getKeyboardPwmSignal().emit(zero_pwms); + bHadInput_ = false; + } } void ARovPawn::EndPlay(const EEndPlayReason::Type EndPlayReason) diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h index 1ec0f528..f82aa6a6 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h @@ -25,6 +25,24 @@ class AIRSIM_API ARovPawn : public APawn virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; virtual void NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, FVector HitNormal, FVector NormalImpulse, const FHitResult& Hit) override; + virtual void SetupPlayerInputComponent(class UInputComponent* PlayerInputComponent) override; + + void onForwardPressed() { bForward_ = true; } + void onForwardReleased() { bForward_ = false; } + void onBackwardPressed() { bBackward_ = true; } + void onBackwardReleased() { bBackward_ = false; } + void onLeftPressed() { bLeft_ = true; } + void onLeftReleased() { bLeft_ = false; } + void onRightPressed() { bRight_ = true; } + void onRightReleased() { bRight_ = false; } + void onUpPressed() { bUp_ = true; } + void onUpReleased() { bUp_ = false; } + void onDownPressed() { bDown_ = true; } + void onDownReleased() { bDown_ = false; } + void onYawLeftPressed() { bYawLeft_ = true; } + void onYawLeftReleased() { bYawLeft_ = false; } + void onYawRightPressed() { bYawRight_ = true; } + void onYawRightReleased() { bYawRight_ = false; } //interface void initializeForBeginPlay(); @@ -40,6 +58,11 @@ class AIRSIM_API ARovPawn : public APawn UPROPERTY() class UStaticMeshComponent* body_mesh_; + // OpenHUTB 追加(2026-11-05):MuJoCo 跟随者桥接组件 + // 让本 Pawn 跟随 MuJoCo 的物理状态,而不是 AirSim 自身的 rov_physics_body_。 + UPROPERTY() + class URovMuJoCoBridge* MuJoCoBridge_; + UPROPERTY() class UChildActorComponent* camera_front_right_comp_; UPROPERTY() @@ -75,4 +98,14 @@ class AIRSIM_API ARovPawn : public APawn RovPawnEvents pawn_events_; int init_id_; + + bool bForward_ = false; + bool bBackward_ = false; + bool bLeft_ = false; + bool bRight_ = false; + bool bUp_ = false; + bool bDown_ = false; + bool bYawLeft_ = false; + bool bYawRight_ = false; + bool bHadInput_ = false; }; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp index d6a4268f..0b5abe3c 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp @@ -1,5 +1,6 @@ #include "RovPawnSimApi.h" #include "AirBlueprintLib.h" +#include "Vehicles/Rov/RovMuJoCoBridge.h" #include "vehicles/rov/RovParamsFactory.hpp" #include "UnrealSensors/UnrealSensorFactory.h" #include @@ -40,6 +41,15 @@ void RovPawnSimApi::initialize() VectorMath::toEulerianAngle(pose.orientation, pitch, roll, yaw); pose.orientation = VectorMath::toQuaternion(0, 0, yaw); setPose(pose, false); + + pawn_events_->getKeyboardPwmSignal().connect_member(this, &RovPawnSimApi::onKeyboardPwm); +} + +void RovPawnSimApi::onKeyboardPwm(const std::vector& pwms) +{ + if (vehicle_api_) { + vehicle_api_->setMotorPWMs(pwms); + } } void RovPawnSimApi::pawnTick(float dt) @@ -63,7 +73,30 @@ void RovPawnSimApi::updateRenderedState(float dt) const CollisionInfo& collision_info = getCollisionInfo(); rov_physics_body_->setCollisionInfo(collision_info); - last_phys_pose_ = rov_physics_body_->getPose(); + // OpenHUTB 修改(2026-11-05):外部位姿源优先 + // 见 docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + // 设置外部位姿源(MuJoCo)后,本类退化为纯跟随者,不再用 AirSim 自身物理体的状态, + // 从而消除「两个求解器同时拥有同一台 ROV 位姿」的双所有权问题。 + // 坐标转换统一走基类的 toPoseFromUnreal(),这里不做任何手工换算。 + FVector ExternalPos; + FQuat ExternalRot; + bool bHasExternalPose = false; + + // 优先用实例级 provider(测试或定制场景) + if (external_pose_provider_) { + bHasExternalPose = external_pose_provider_(ExternalPos, ExternalRot); + } + // 其次用 URovMuJoCoBridge 注册的全局 provider(正式运行时走这条) + if (!bHasExternalPose && URovMuJoCoBridge::ExternalPoseProvider) { + bHasExternalPose = URovMuJoCoBridge::ExternalPoseProvider(ExternalPos, ExternalRot); + } + + if (bHasExternalPose) { + last_phys_pose_ = toPoseFromUnreal(ExternalPos, ExternalRot); + } + else { + last_phys_pose_ = rov_physics_body_->getPose(); + } collision_response = rov_physics_body_->getCollisionResponseInfo(); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h index d8953fba..307877df 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h @@ -10,6 +10,7 @@ #include "common/common_utils/UniqueValueMap.hpp" #include "RovPawnEvents.h" #include +#include class RovPawnSimApi : public PawnSimApi { @@ -58,7 +59,37 @@ class RovPawnSimApi : public PawnSimApi return vehicle_api_.get(); } + void onKeyboardPwm(const std::vector& pwms); + + // --------------------------------------------------------------------- + // OpenHUTB 追加(2026-11-05):外部位姿源 + // + // 设计依据:docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + // + // AirSim 的 rov_physics_body_ 与 MuJoCo 的 AMjArticulation 会同时积分同一台 + // ROV 的位姿,构成「双所有权」。被调研的 16 个开源仿真系统里没有一个允许 + // 这种配置(docs/research/开源仿真器物理与视觉统一架构调研.html §7 第 3 条)。 + // + // 这里把 RovPawnSimApi 降级为「纯跟随者」:设置了外部位姿源后, + // updateRenderedState() 优先用它,不再读 rov_physics_body_。 + // + // provider 返回 true 表示本帧有有效位姿;返回 false 则回退原逻辑 + // (例如 MuJoCo 模型尚未编译完成)。 + // + // 注意:provider 给出的是 **UE 世界坐标**(FVector + FQuat), + // 由基类的 toPoseFromUnreal() 统一转成 AirSim 的 NED Pose, + // 保证坐标转换只在一个地方发生。 + // --------------------------------------------------------------------- + void setExternalPoseProvider(std::function provider) + { + external_pose_provider_ = std::move(provider); + } + void clearExternalPoseProvider() { external_pose_provider_ = nullptr; } + bool hasExternalPoseProvider() const { return static_cast(external_pose_provider_); } + private: + std::function external_pose_provider_; + std::unique_ptr vehicle_api_; std::unique_ptr vehicle_params_; From 515f13ac148f3d6835166836fa34ad2886651bd6 Mon Sep 17 00:00:00 2001 From: shark Date: Mon, 9 Nov 2026 10:47:33 +0800 Subject: [PATCH 4/8] =?UTF-8?q?feat(rov):=20=E5=90=8C=E6=AD=A5=E6=B0=B4?= =?UTF-8?q?=E4=B8=8B=20ROV=20=E6=9C=80=E6=96=B0=E5=AE=9E=E7=8E=B0=EF=BC=88?= =?UTF-8?q?MuJoCo=20=E6=A1=A5=E6=8E=A5=E3=80=81=E8=87=82=E6=B5=AE=E5=8A=9B?= =?UTF-8?q?=E3=80=81=E6=89=93=E5=8C=85=E7=89=88=E5=A7=BF=E6=80=81=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 把 hutb 分支 feat/holocean-runtime 里属于 AirSim 的实现同步回来, 让 hutb 后续可以直接引用本仓库、不必再往 hutb 里塞 AirSim 源码。 - MuJoCoBridge.cpp/.h * 级联姿态控制(姿态 P → 期望角速度 → 机体系角速度 P) * 机械臂按真实排水体积加浮力(ArmBuoyancyVolume = 12.1 L,UR5e 七连杆 OBJ 散度定理实测 11.73 L + 2F85 约 0.35 L) * 浮力作用点由艇体质心改为整机合成质心(Data->subtree_com) * 指令新鲜度淡出 + 物理线程兜底控制器:修复打包版首帧卡顿 10 秒期间 旧力矩被冻结施加、ROV 5 秒内被甩翻 159° 的问题 * EffInertia 延迟到 t>0.2 s 计算(qM 未就绪时读到垃圾值) * 姿态积分步长改用实际仿真时间,消除帧率依赖 - RovUnderwaterPhysics.h * 三层级联洋流 + 定深 PID;**本文件之前缺失**,导致 hutb 里 MuJoCoBridge.h:31 编译失败(setup.bat -l 报 C1083) - RovPawnEvents.cpp/.h:事件定义更新 - RovApiBase.hpp:新增 setMotorPWMs,桥接可直接写电机 PWM - settings_rov.json:新增深度夹紧(ZBed/EnableDepthClamp/Bed*)、 中性浮力、水动力阻尼参数 注意:settings_rov.json 仍是「显式选择 ROV 模式」才生效, 默认 settings.json 保持普通无人机配置不变。 --- .../include/vehicles/rov/api/RovApiBase.hpp | 4 + Unreal/Environments/Blocks/settings_rov.json | 89 ++- .../Source/Vehicles/Rov/RovMuJoCoBridge.cpp | 565 +++++++++++++++--- .../Source/Vehicles/Rov/RovMuJoCoBridge.h | 92 ++- .../Source/Vehicles/Rov/RovPawnEvents.cpp | 5 + .../Source/Vehicles/Rov/RovPawnEvents.h | 3 + .../Vehicles/Rov/RovUnderwaterPhysics.h | 221 +++++++ 7 files changed, 866 insertions(+), 113 deletions(-) create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h diff --git a/AirLib/include/vehicles/rov/api/RovApiBase.hpp b/AirLib/include/vehicles/rov/api/RovApiBase.hpp index cd4cdbe5..591149bd 100644 --- a/AirLib/include/vehicles/rov/api/RovApiBase.hpp +++ b/AirLib/include/vehicles/rov/api/RovApiBase.hpp @@ -99,6 +99,10 @@ namespace airlib virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByMotorPWMs(const vector& pwm_values, float duration); + virtual void setMotorPWMs(const vector& pwm_values) + { + commandMotorPWMs(pwm_values); + } virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); diff --git a/Unreal/Environments/Blocks/settings_rov.json b/Unreal/Environments/Blocks/settings_rov.json index ee39e63c..3d61b25a 100644 --- a/Unreal/Environments/Blocks/settings_rov.json +++ b/Unreal/Environments/Blocks/settings_rov.json @@ -8,6 +8,15 @@ "DefaultRov": {"PawnBP": "Class'/Script/AirSim.RovPawn'"}, "DefaultComputerVision": {"PawnBP": "Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'"} }, + "ZBed": -30.0, + "EnableDepthClamp": true, + "BedStiffness": 2000.0, + "BedDamping": 300.0, + "BedContactMargin": 0.5, + "BedClampMargin": 0.3, + "EnableNeutralBuoyancy": true, + "EnableHydrodynamicDamping": true, + "OffZ": 0.05, "Vehicles": { "RovSimple": { "VehicleType": "RovSimple", @@ -19,27 +28,65 @@ "RemoteControlID": 0, "AllowAPIWhenDisconnected": false }, - "Cameras": { - "front_right_custom": { - "CaptureSettings": [ - { - "PublishToRos": 1, - "ImageType": 0, - "FOV_Degrees": 90, - "Width": 1280, - "Height": 720 - } - ], - "X": 0.50, "Y": 0.06, "Z": 0.10, - "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 - } + "ZBed": -30.0, + "EnableDepthClamp": true, + "BedStiffness": 2000.0, + "BedDamping": 300.0, + "BedContactMargin": 0.5, + "BedClampMargin": 0.3, + "EnableNeutralBuoyancy": true, + "EnableHydrodynamicDamping": true, + "OffZ": 0.05, + "Cameras": { + "front_left_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "FOV_Degrees": 90, + "Width": 1280, + "Height": 720 + } + ], + "X": 0.50, "Y": -0.06, "Z": 0.10, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + }, + "front_right_custom": { + "CaptureSettings": [ + { + "PublishToRos": 1, + "ImageType": 0, + "FOV_Degrees": 90, + "Width": 1280, + "Height": 720 + } + ], + "X": 0.50, "Y": 0.06, "Z": 0.10, + "Pitch": 0.0, "Roll": 0.0, "Yaw": 0.0 + } + }, + "Sensors": { + "barometer": { + "SensorType": 1, + "Enabled": true + }, + "imu": { + "SensorType": 2, + "Enabled": true + }, + "magnetometer": { + "SensorType": 4, + "Enabled": true + }, + "downward_sonar": { + "SensorType": 5, + "Enabled": true, + "MinDistance": 0.1, + "MaxDistance": 50.0, + "X": 0.0, "Y": 0.0, "Z": 0.15, + "Pitch": -90.0, "Roll": 0.0, "Yaw": 0.0 } - - - - - } } - + } + } } - diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp index 7e3b7e77..c3b7f116 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp @@ -302,13 +302,18 @@ void URovMuJoCoBridge::ApplyControlForce() DisplacedVolume = FloatParam(TEXT("RovDisplacedVolume="), DisplacedVolume); BuoyancyScale = FloatParam(TEXT("RovBuoyancyScale="), BuoyancyScale); BuoyancyArm = FloatParam(TEXT("RovBuoyancyArm="), BuoyancyArm); + ArmBuoyancyVolume = FloatParam(TEXT("RovArmBuoyVolume="), ArmBuoyancyVolume); + ArmBuoyancyScale = FloatParam(TEXT("RovArmBuoyScale="), ArmBuoyancyScale); AngularDampingCoeff = FloatParam(TEXT("RovAngularDamping="), AngularDampingCoeff); ArmJointDamping = FloatParam(TEXT("RovArmDamping="), ArmJointDamping); ThrusterMaxRatio = FloatParam(TEXT("RovThrustRatio="), ThrusterMaxRatio); WaterSurfaceZ = FloatParam(TEXT("RovWaterSurfaceZ="), WaterSurfaceZ); + if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { bEnableAttitudeHold = false; } UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][调参] 排水体积=%.4f m³ 浮力倍率=%.2f 推力比=%.2f 水面Z=%.2f m"), - DisplacedVolume, BuoyancyScale, ThrusterMaxRatio, WaterSurfaceZ); + TEXT("[RovMuJoCoBridge][调参] 排水体积=%.4f m³ 浮力倍率=%.2f 臂排水=%.2f L×%.2f " + "推力比=%.2f 水面Z=%.2f m"), + DisplacedVolume, BuoyancyScale, ArmBuoyancyVolume, ArmBuoyancyScale, + ThrusterMaxRatio, WaterSurfaceZ); if (bUseNativeFluid) { Model->opt.density = FluidDensity; @@ -388,44 +393,11 @@ void URovMuJoCoBridge::ApplyControlForce() CompBodies); } - // 复合惯量:从自由关节的 6×6 质量阵取转动对角元。 - // body_inertia 只有 ROV 本体,不含机械臂,用它算增益会严重偏小 —— - // 这正是消融实验里"纯刚体 + PD 也不收敛"的原因。 - // - // 安全性:qM 只有在 mj_forward/mj_step 之后才有效。打包版首帧的时序 - // 可能与编辑器不同,若此时 qM 未就绪会读到垃圾值 → 增益变 NaN → - // 力矩爆炸。所以这里校验:非空、有限、为正,否则退回 body_inertia。 - { - const int32 Nv = Model->nv; - bool bOk = (Nv >= 6) && (Data->qM != nullptr); - if (bOk) { - TArray M; - M.SetNumZeroed(Nv * Nv); - mj_fullM(Model, M.GetData(), Data->qM); - for (int32 a = 0; a < 3; ++a) { - const double V = M[(3 + a) * Nv + (3 + a)]; - if (!FMath::IsFinite(V) || V <= 0.0) { - bOk = false; - break; - } - EffInertia[a] = V; - } - } - if (!bOk) { - for (int32 a = 0; a < 3; ++a) { - EffInertia[a] = Model->body_inertia[3 * BodyId + a]; - } - UE_LOG(LogRovBridge, Warning, - TEXT("[RovMuJoCoBridge][复合惯量] qM 未就绪,回退到 body_inertia")); - } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][复合惯量] 转动对角元 = (%.3f, %.3f, %.3f) kg·m² " - "(body_inertia = %.3f/%.3f/%.3f,来源=%s)"), - EffInertia[0], EffInertia[1], EffInertia[2], - Model->body_inertia[3 * BodyId + 0], - Model->body_inertia[3 * BodyId + 1], - Model->body_inertia[3 * BodyId + 2], - bOk ? TEXT("6x6 质量阵") : TEXT("body_inertia 回退")); + // 复合惯量:先用本体惯量兜底,等仿真真正跑起来再换成 6×6 质量阵的值。 + // (qM 只有在 mj_forward/mj_step 之后才有效,首帧读会拿到垃圾值; + // 打包版首帧时序和编辑器不同,曾读到极小值 → 控制器力矩≈0 → ROV 缓慢翻转。) + for (int32 a = 0; a < 3; ++a) { + EffInertia[a] = Model->body_inertia[3 * BodyId + a]; } // 洋流模型初始化(三层级联) @@ -486,6 +458,38 @@ void URovMuJoCoBridge::ApplyControlForce() Weight, Buoyancy, Buoyancy - Weight, DisplacedVolume, BuoyancyScale); } + // 复合惯量延迟计算:qM 只有在 mj_forward/mj_step 之后才有效。 + // 首帧就取会拿到垃圾值(打包版实测读到极小值 → 控制器力矩≈0 → 缓慢翻转), + // 所以等仿真时间过 0.2 s 再算一次,并且对量级做下限校验。 + { + static bool bEffInertiaDone = false; + if (!bEffInertiaDone && Data->time > 0.2 && Data->qM) { + bEffInertiaDone = true; + const int32 Nv = Model->nv; + bool bOk = (Nv >= 6); + double Vals[3] = { 0.0, 0.0, 0.0 }; + if (bOk) { + TArray M; + M.SetNumZeroed(Nv * Nv); + mj_fullM(Model, M.GetData(), Data->qM); + for (int32 a = 0; a < 3; ++a) { + const double V = M[(3 + a) * Nv + (3 + a)]; + // 这台 ROV 的转动惯量量级是几十 kg·m²,小于 1 一律视为无效 + if (!FMath::IsFinite(V) || V < 1.0) { bOk = false; break; } + Vals[a] = V; + } + } + if (bOk) { + for (int32 a = 0; a < 3; ++a) { EffInertia[a] = Vals[a]; } + } + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][复合惯量] 延迟计算 t=%.2f " + "EffInertia=(%.3f, %.3f, %.3f) 来源=%s"), + Data->time, EffInertia[0], EffInertia[1], EffInertia[2], + bOk ? TEXT("6x6 质量阵") : TEXT("body_inertia 回退")); + } + } + APlayerController* PC = GetWorld() ? GetWorld()->GetFirstPlayerController() : nullptr; if (!PC) { return; @@ -605,20 +609,44 @@ void URovMuJoCoBridge::ApplyControlForce() const double FrameDt = GetWorld() ? FMath::Max(GetWorld()->GetDeltaSeconds(), 1e-4f) : 0.016; // 【航向调整】← / → 连续改变目标航向(姿态保持开启时生效) - if (bEnableAttitudeHold) { + // + // 方向定义:MuJoCo→UE 的坐标映射是 X→X、Y→-Y、Z→Z(MjUtils::MjToUERotation), + // 属于**镜像变换**,所以 MuJoCo 里的 +yaw 在画面上表现为 -yaw。这里按操作者 + // 看到的画面定义按键方向:→ = 画面右转 = MuJoCo yaw 减小,← 反之。 + // + // 为什么要给前馈:以前只把目标航向按 60°/s 往前推,而内环最大角速度只有 + // 0.6 rad/s(34°/s),目标跑得比机身快,误差会一路累积;一旦超过 180°, + // 四元数短弧归一化会把误差翻号,机身就反着转 —— 表现就是"转满一圈后回弹"。 + // 现在按住期间直接给期望角速度(前馈),误差只反映跟踪滞后,永远不会绕圈。 + double YawCmdRateDeg = 0.0; // 本帧航向角速度指令(deg/s,MuJoCo 符号) + if (bEnableAttitudeHold && PC) { const bool bYawLeft = PC->IsInputKeyDown(EKeys::Left); const bool bYawRight = PC->IsInputKeyDown(EKeys::Right); - if (bYawLeft || bYawRight) { - const double Step = (double)YawRateDegPerSec * FrameDt; - if (bYawLeft) { TargetYawDeg -= (float)Step; } - if (bYawRight) { TargetYawDeg += (float)Step; } - while (TargetYawDeg > 180.0f) { TargetYawDeg -= 360.0f; } - while (TargetYawDeg < -180.0f) { TargetYawDeg += 360.0f; } - static int32 YawLogCounter = 0; - if ((++YawLogCounter % 30) == 0) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][航向] 目标航向 = %.1f°(←/→ 调整)"), TargetYawDeg); - } + if (bYawLeft != bYawRight) { + YawCmdRateDeg = bYawLeft ? +(double)YawRateDegPerSec + : -(double)YawRateDegPerSec; + } + } + + // 命令行自检:-RovYawTest 在 3~11 秒模拟"一直按住 →"(画面右转 = MuJoCo yaw 减小), + // 转过的总角度超过 360°,用来验证方向、以及转满一圈后不回弹。 + static const bool bYawTest = FParse::Param(FCommandLine::Get(), TEXT("RovYawTest")); + if (bYawTest && bEnableAttitudeHold && YawCmdRateDeg == 0.0) { + const double TestT = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; + if (TestT > 3.0 && TestT < 11.0) { + YawCmdRateDeg = -(double)YawRateDegPerSec; + } + } + + if (YawCmdRateDeg != 0.0) { + TargetYawDeg += (float)(YawCmdRateDeg * FrameDt); + while (TargetYawDeg > 180.0f) { TargetYawDeg -= 360.0f; } + while (TargetYawDeg < -180.0f) { TargetYawDeg += 360.0f; } + static int32 YawLogCounter = 0; + if ((++YawLogCounter % 30) == 0) { + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][航向] 目标航向 = %.1f° 指令角速度 = %+.0f°/s(←/→)"), + TargetYawDeg, YawCmdRateDeg); } } @@ -641,6 +669,16 @@ void URovMuJoCoBridge::ApplyControlForce() bPrevDepthKeyDown = bDepthKeyDown; } + // 出生自动锁定深度:避免浮力略小于重力时一路沉到海底,保证演示时停在水面附近 + static bool bAutoDepthInit = false; + if (bEnableDepthHold && !bAutoDepthInit) { + bAutoDepthInit = true; + bDepthHoldActive = true; + DepthPID.Reset(); + DepthHoldTargetZ = RovZ; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深] 出生自动锁定 Z=%.3f m"), DepthHoldTargetZ); + } + // 命令行自检:-RovDepthHoldTest 在 3 秒后自动锁定当前深度并打印误差, // 用来验证 PID 定深,不依赖键盘。 static const bool bDepthHoldTest = @@ -669,11 +707,12 @@ void URovMuJoCoBridge::ApplyControlForce() const double Now = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0; if (!bSpinApplied && Now > 2.0) { bSpinApplied = true; - // 先抬到水中(Z=4m)并清零线速度,避免海底接触力矩污染测量 + // 放到水中(Z=-5m)并清零线速度,避免海底接触和浮力突变污染测量。 + // 注意:姿态控制在自检期间会被跳过,否则闭环会把脉冲立刻压掉。 const int32 QPosAdr = Model->jnt_qposadr[SpinJnt]; Data->qpos[QPosAdr + 0] = 0.0; Data->qpos[QPosAdr + 1] = 0.0; - Data->qpos[QPosAdr + 2] = 4.0; + Data->qpos[QPosAdr + 2] = -5.0; Data->qpos[QPosAdr + 3] = 1.0; // quat w Data->qpos[QPosAdr + 4] = 0.0; Data->qpos[QPosAdr + 5] = 0.0; @@ -684,8 +723,15 @@ void URovMuJoCoBridge::ApplyControlForce() Data->qvel[SpinDof + 3] = 3.0; Data->qvel[SpinDof + 4] = 3.0; Data->qvel[SpinDof + 5] = 3.0; + DepthHoldTargetZ = -5.0; // 定深跟着走,别把 ROV 拉回出生高度 + // 关掉浮心高度:BG≠0 会形成"单摆"回复力矩,角速度会来回摆, + // 没法拟合成指数衰减。自检只要纯阻尼,浮力大小不变(仍然中性附近)。 + BuoyancyArm = 0.0f; + bDepthHoldActive = false; UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][旋转自检] 移至水中 Z=4m,施加 3 rad/s 三轴角速度脉冲")); + TEXT("[RovMuJoCoBridge][旋转自检] 移至水中 Z=-5m,施加 3 rad/s 三轴角速度脉冲" + "(dof_damping=%.2f)"), + (double)Model->dof_damping[SpinDof + 3]); } if (bSpinApplied) { const int32 Tick = (int32)(Now * 4.0); @@ -695,8 +741,9 @@ void URovMuJoCoBridge::ApplyControlForce() const double Wy = Data->qvel[SpinDof + 4]; const double Wz = Data->qvel[SpinDof + 5]; UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][旋转自检] t=%.2fs |ω|=%.4f rad/s"), - Now - 2.0, FMath::Sqrt(Wx * Wx + Wy * Wy + Wz * Wz)); + TEXT("[RovMuJoCoBridge][旋转自检] t=%.2fs wx=%.4f wy=%.4f wz=%.4f |w|=%.4f rad/s"), + Now - 2.0, Wx, Wy, Wz, + FMath::Sqrt(Wx * Wx + Wy * Wy + Wz * Wz)); } } } @@ -844,6 +891,7 @@ void URovMuJoCoBridge::ApplyControlForce() // MuJoCo 椭球流体模型的旋转阻力偏弱,真实 ROV 的导流罩/鳍会额外提供阻力。 // Kd = AngularDampingCoeff × 平均主惯量,保持和之前"每秒衰减 1/s"的语义一致。 double TorqueX = 0.0, TorqueY = 0.0, TorqueZ = 0.0; + double WorldTorqueX = 0.0, WorldTorqueY = 0.0, WorldTorqueZ = 0.0; if (AngularDampingCoeff > 0.0f && Model->body_jntnum[BodyId] >= 1) { const int32 DampJnt = Model->body_jntadr[BodyId]; if (Model->jnt_type[DampJnt] == mjJNT_FREE) { @@ -868,7 +916,8 @@ void URovMuJoCoBridge::ApplyControlForce() // 增益按惯量缩放(Kp = I·ω²、Kd = 2I·ζ·ω),换模型不用重调。 // 积分项是必需的:机械臂挂在一侧产生恒定扰动力矩, // 纯 PD 只能把它平衡在某个固定倾角上,消不掉稳态误差。 - if (bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { + // 旧的四元数三轴控制器存在符号/轴向问题,已停用;改用下方世界系解耦控制 + if (false && bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { const int32 AttJnt = Model->body_jntadr[BodyId]; if (Model->jnt_type[AttJnt] == mjJNT_FREE) { const int32 AttDof = Model->jnt_dofadr[AttJnt]; @@ -948,6 +997,22 @@ void URovMuJoCoBridge::ApplyControlForce() Iaxis[0], Iaxis[1], Iaxis[2]); } + // 姿态诊断:每秒打印一次欧拉角、目标、误差、角速度和力矩 + { + static int32 AttLogTick = -1; + const int32 TickNow = (int32)(GetWorld()->GetTimeSeconds()); + if (TickNow != AttLogTick) { + AttLogTick = TickNow; + const double RollD = FMath::RadiansToDegrees(FMath::Atan2(2.0*(Qw*Qx + Qy*Qz), 1.0 - 2.0*(Qx*Qx + Qy*Qy))); + const double PitchD = FMath::RadiansToDegrees(FMath::Asin(FMath::Clamp(2.0*(Qw*Qy - Qz*Qx), -1.0, 1.0))); + const double YawD = FMath::RadiansToDegrees(FMath::Atan2(2.0*(Qw*Qz + Qx*Qy), 1.0 - 2.0*(Qy*Qy + Qz*Qz))); + UE_LOG(LogRovBridge, Log, + TEXT("[%s][姿态诊断] roll=%.1f pitch=%.1f yaw=%.1f targetYaw=%.1f |E|=(%.3f,%.3f,%.3f) omega=(%.2f,%.2f,%.2f)") + , TEXT("RovMuJoCoBridge"), RollD, PitchD, YawD, TargetYawDeg, E[0], E[1], E[2], + Data->qvel[AttDof+3], Data->qvel[AttDof+4], Data->qvel[AttDof+5]); + } + } + for (int32 Axis = 0; Axis < 3; ++Axis) { // 优先用复合惯量(含机械臂);取不到时退回本体的等效惯量 const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] : Iaxis[Axis]; @@ -955,7 +1020,8 @@ void URovMuJoCoBridge::ApplyControlForce() const double Kd = 2.0 * I * Zeta * W; const double Ki = I * W * W * W * KiF; const double Omega = Data->qvel[AttDof + 3 + Axis]; - const double T = Kp * E[Axis] + Ki * IntArr[Axis] - Kd * Omega; + double T = Kp * E[Axis] + Ki * IntArr[Axis] - Kd * Omega; + T = FMath::Clamp(T, -1500.0, 1500.0); // 力矩限幅,防止饱和发散 if (Axis == 0) { TorqueX += T; } else if (Axis == 1) { TorqueY += T; } else { TorqueZ += T; } @@ -963,7 +1029,122 @@ void URovMuJoCoBridge::ApplyControlForce() } } + // 【10】级联姿态控制(按调研报告 C1-C4) + // 外环:姿态误差 -> 期望角速度(机体系) + // 内环:期望角速度 - 实测角速度(qvel[3:6] 即机体系)-> 力矩 + // 力矩直接写 qfrc_applied 后三维(机体系),不再经 RotByQuat/mj_applyFT + if (bEnableAttitudeHold && !bSpinTest && Model->body_jntnum[BodyId] >= 1) { + const int32 NJ = Model->body_jntadr[BodyId]; + if (Model->jnt_type[NJ] == mjJNT_FREE) { + const int32 ND = Model->jnt_dofadr[NJ]; + const double* QQ = &Data->xquat[4 * BodyId]; + const double qw = QQ[0], qx = QQ[1], qy = QQ[2], qz = QQ[3]; + + if (!bAttitudeInitialized) { + bAttitudeInitialized = true; + const double InitYaw = FMath::Atan2(2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz)); + TargetYawDeg = FMath::RadiansToDegrees(InitYaw); + UE_LOG(LogRovBridge, Log, + TEXT("[RovMuJoCoBridge][姿态] 级联控制启用,锁定航向 %.1f°"), TargetYawDeg); + } + + const double HalfYaw = FMath::DegreesToRadians(TargetYawDeg) * 0.5; + const double Qtw = FMath::Cos(HalfYaw); + const double Qtz = FMath::Sin(HalfYaw); + + // q_e = conj(q_c) ⊗ q_t (机体系),取短弧 + const double aw = qw, ax = -qx, ay = -qy, az = -qz; + double Qew = aw * Qtw - ax * 0.0 - ay * 0.0 - az * Qtz; + double Qex = aw * 0.0 + ax * Qtw + ay * Qtz - az * 0.0; + double Qey = aw * 0.0 - ax * Qtz + ay * Qtw + az * 0.0; + double Qez = aw * Qtz + ax * 0.0 - ay * 0.0 + az * Qtw; + if (Qew < 0.0) { Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; } + + const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; + const double KpAtt[3] = { 1.5, 1.5, 1.0 }; + const double Wmax[3] = { 0.8, 0.8, 0.6 }; + const double Wn = 5.0; + static double AttInt[3] = { 0.0, 0.0, 0.0 }; + + // 积分步长必须用「距上次调用实际经过的仿真时间」,不能用固定 timestep: + // ApplyControlForce 是按渲染帧调用的,帧率不同积分速度就不同 + // (Development 约 60 fps、Shipping 约 120+ fps)。用固定 timestep 会让 + // 打包版的积分快一倍,恒力矩还没抵掉就先过冲,表现为缓慢翻转。 + static double LastIntTime = -1.0; + double IntDt = 0.0; + { + const double NowT = (double)Data->time; + if (LastIntTime >= 0.0) { + IntDt = FMath::Clamp(NowT - LastIntTime, 0.0, 0.05); + } + LastIntTime = NowT; + } + + // 航向前馈(见【航向调整】):按住 ←/→ 时直接给期望角速度。 + // 上限也要一起放大,否则前馈会被 Wmax 截掉,目标航向又会跑飞。 + const double YawFF = FMath::DegreesToRadians(YawCmdRateDeg); + const double WmaxZ = (YawFF != 0.0) + ? FMath::Max(Wmax[2], FMath::Abs(YawFF) + 0.2) + : Wmax[2]; + + for (int32 Axis = 0; Axis < 3; ++Axis) { + const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] + : Model->body_inertia[3 * BodyId + Axis]; + const double Wlim = (Axis == 2) ? WmaxZ : Wmax[Axis]; + // 积分项:抵消模型里无法消除的恒定力矩(质心/浮心不重合、 + // 机械臂残余重量等),否则纯 P 会停在一个固定倾角上。 + AttInt[Axis] = FMath::Clamp(AttInt[Axis] + E3[Axis] * IntDt, -2.0, 2.0); + double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis] + AttitudeIntegralGain * AttInt[Axis], + -Wlim, Wlim); + if (Axis == 2 && YawFF != 0.0) { + Wsp = FMath::Clamp(Wsp + YawFF, -WmaxZ, WmaxZ); + } + double T = I * Wn * (Wsp - Data->qvel[ND + 3 + Axis]); + T = FMath::Clamp(T, -1500.0, 1500.0); + if (Axis == 0) { TorqueX += T; } + else if (Axis == 1) { TorqueY += T; } + else { TorqueZ += T; } + } + + // 航向诊断(每秒一次):目标 / 实测 / 误差 / 机体 Z 角速度 / 前馈 + { + static double LastYawLogT = -1.0; + if (Data->time - LastYawLogT > 1.0) { + LastYawLogT = Data->time; + const double CurYawDeg = FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz))); + double YawErrDeg = (double)TargetYawDeg - CurYawDeg; + while (YawErrDeg > 180.0) { YawErrDeg -= 360.0; } + while (YawErrDeg < -180.0) { YawErrDeg += 360.0; } + // 真实倾角:机体 Z 轴与世界 Z 的夹角,不受欧拉角约定影响 + const double UpZ = FMath::Clamp((double)Data->xmat[9 * BodyId + 8], -1.0, 1.0); + const double TiltDeg = FMath::RadiansToDegrees(FMath::Acos(UpZ)); + UE_LOG(LogRovBridge, Log, + TEXT("[%s][姿态诊断] t=%.1f 倾角=%.1f° 目标=%.1f° 实测=%.1f° 误差=%.1f° " + "E3=(%.3f, %.3f, %.3f) w=(%.3f, %.3f, %.3f) 前馈=%+.0f°/s"), + TEXT("RovMuJoCoBridge"), Data->time, TiltDeg, + TargetYawDeg, CurYawDeg, YawErrDeg, + E3[0], E3[1], E3[2], + Data->qvel[ND + 3], Data->qvel[ND + 4], Data->qvel[ND + 5], + YawCmdRateDeg); + } + } + } + } + // 【11】机械臂静力矩配平:常值前馈,把控制器硬扛的那部分稳态力矩抵掉 + if (bEnableAttitudeHold && !bSpinTest) { + float TrimPitch = TrimTorquePitch; + float TrimRoll = TrimTorqueRoll; + FParse::Value(FCommandLine::Get(), TEXT("RovTrimPitch="), TrimPitch); + FParse::Value(FCommandLine::Get(), TEXT("RovTrimRoll="), TrimRoll); + TorqueX += TrimRoll; + TorqueY += TrimPitch; + } + CachedBodyId.store(BodyId); + CachedControlTime.store((double)Data->time); CachedForceX.store(Fx); CachedForceY.store(Fy); CachedForceZ.store(Fz); @@ -988,15 +1169,10 @@ void URovMuJoCoBridge::ApplyControlForce() } } - // 姿态控制器和角阻尼给出的力矩是**机体系**的,而 mj_applyFT 要世界系, - // 在这里统一转过去。之前直接把机体系力矩写进 qfrc_applied 后三维, - // 实测轴系对不上(给"轴 0"却主要绕 Y 转),这是本次重构要解决的根因。 - double TWx = 0.0, TWy = 0.0, TWz = 0.0; - RotByQuat(Qw, Qx, Qy, Qz, TorqueX, TorqueY, TorqueZ, TWx, TWy, TWz); - - CachedTorqueX.store(TWx); - CachedTorqueY.store(TWy); - CachedTorqueZ.store(TWz); + // 力矩就是机体系,直接缓存;ApplyCachedForce 里直写 qfrc_applied[3..5] + CachedTorqueX.store(TorqueX); + CachedTorqueY.store(TorqueY); + CachedTorqueZ.store(TorqueZ); CachedBuoyancy.store(Buoyancy); if (!bControlCallbackRegistered) { @@ -1059,6 +1235,13 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) Data->qfrc_applied[DofAdr + k] = 0.0; } + // 指令新鲜度:控制量在游戏线程按帧算,物理线程按 500 Hz 跑。 + // 游戏线程卡顿(打包版启动时的着色器编译最典型)时,旧指令会被冻结着 + // 一直施加,等于给 ROV 灌几秒恒定力矩 —— 实测 5 秒内被甩到 159°。 + // 这里按「距上次控制更新经过的仿真时间」做淡出:>0.1 s 开始衰减,>0.2 s 归零。 + const double CmdAge = (double)Data->time - CachedControlTime.load(); + const double Fresh = FMath::Clamp(1.0 - (CmdAge - 0.10) / 0.10, 0.0, 1.0); + const mjtNum CG[3] = { Data->xipos[3 * BodyId + 0], Data->xipos[3 * BodyId + 1], @@ -1069,14 +1252,50 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) // 【1】推进器推力 + 洋流拖曳:作用在质心 mjtNum F1[3] = { - (mjtNum)CachedForceX.load(), - (mjtNum)CachedForceY.load(), - (mjtNum)CachedForceZ.load() + (mjtNum)(CachedForceX.load() * Fresh), + (mjtNum)(CachedForceY.load() * Fresh), + (mjtNum)(CachedForceZ.load() * Fresh) }; mj_applyFT(Model, Data, F1, ZeroT, CG, BodyId, Data->qfrc_applied); - // 【2】浮力:作用在**浮心**(质心 + R·(0,0,BG)),自动产生回复力矩 + // 【2】浮力:作用在**整机浮心** = 整机合成质心 + R·(0,0,BG) + // + // 浮力必须加在「整机(艇体 + 机械臂)的合成质心」上,而不是艇体自身的质心: + // 排水体积属于整个载体,浮心跟着整机质量分布走。之前加在艇体质心上, + // 臂的 22 kg 把整机质心拉偏了约 18 mm,4518 N × 18 mm ≈ 81 N·m 的恒定力矩, + // 姿态控制器只能靠 28° 的倾斜去平衡(实测 roll = −27.9°)。 const double Buoy = CachedBuoyancy.load(); + + double TotalMass = 0.0, Cx = 0.0, Cy = 0.0, Cz = 0.0; + for (int32 b = 0; b < Model->nbody; ++b) { + bool bInSubtree = (b == BodyId); + if (!bInSubtree) { + int32 p = Model->body_parentid[b]; + while (p > 0) { + if (p == BodyId) { bInSubtree = true; break; } + p = Model->body_parentid[p]; + } + } + if (!bInSubtree) { continue; } + const double m = Model->body_mass[b]; + TotalMass += m; + Cx += m * Data->xipos[3 * b + 0]; + Cy += m * Data->xipos[3 * b + 1]; + Cz += m * Data->xipos[3 * b + 2]; + } + if (TotalMass > 1e-6) { + Cx /= TotalMass; Cy /= TotalMass; Cz /= TotalMass; + } else { + Cx = CG[0]; Cy = CG[1]; Cz = CG[2]; + } + // 用 MuJoCo 自己算的子树质心做校验:两者必须一致,否则说明重力力矩和浮力 + // 作用点不重合,会留下一个恒定的姿态误差(曾经差 17 mm ≈ 78 N·m)。 + if (Data->subtree_com) { + Cx = Data->subtree_com[3 * BodyId + 0]; + Cy = Data->subtree_com[3 * BodyId + 1]; + Cz = Data->subtree_com[3 * BodyId + 2]; + } + if (Buoy > 0.0) { double Rx = 0.0, Ry = 0.0, Rz = 0.0; RotByQuat(Data->xquat[4 * BodyId + 0], Data->xquat[4 * BodyId + 1], @@ -1084,22 +1303,202 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) 0.0, 0.0, (double)BuoyancyArm, Rx, Ry, Rz); mjtNum F2[3] = { 0.0, 0.0, (mjtNum)Buoy }; mjtNum CBw[3] = { - CG[0] + (mjtNum)Rx, - CG[1] + (mjtNum)Ry, - CG[2] + (mjtNum)Rz + (mjtNum)(Cx + Rx), + (mjtNum)(Cy + Ry), + (mjtNum)(Cz + Rz) }; mj_applyFT(Model, Data, F2, ZeroT, CBw, BodyId, Data->qfrc_applied); } - // 【3】姿态控制器 + 角阻尼:纯力矩,作用点取质心 - const double Tx = CachedTorqueX.load(); - const double Ty = CachedTorqueY.load(); - const double Tz = CachedTorqueZ.load(); - if (Tx != 0.0 || Ty != 0.0 || Tz != 0.0) { - mjtNum T3[3] = { (mjtNum)Tx, (mjtNum)Ty, (mjtNum)Tz }; - mj_applyFT(Model, Data, ZeroF, T3, CG, BodyId, Data->qfrc_applied); + // 【2b】机械臂浮力:按真实排水体积,在臂的合成质心处施加向上浮力 + // + // 为什么必须给臂加浮力:UR5e 是空气中的工业臂,密度远大于水。 + // 整条臂 22.0 kg、排水只有约 12 L,在水里净重约 98 N,挂在艇体前方 0.13 m 处, + // 会持续产生约 8 N·m 的俯仰力矩。真实 ROV 的做法就是给臂绑浮力块(泡沫/浮筒), + // 这里按「等排水体积」把它补上,而不是用常值力矩硬顶。 + if (ArmBuoyancyVolume > 0.0f) { + double ArmMass = 0.0, Cx = 0.0, Cy = 0.0, Cz = 0.0; + for (int32 b = 0; b < Model->nbody; ++b) { + int32 p = Model->body_parentid[b]; + bool bDescendant = false; + while (p > 0) { + if (p == BodyId) { bDescendant = true; break; } + p = Model->body_parentid[p]; + } + if (!bDescendant) { continue; } + const double m = Model->body_mass[b]; + ArmMass += m; + Cx += m * Data->xipos[3 * b + 0]; + Cy += m * Data->xipos[3 * b + 1]; + Cz += m * Data->xipos[3 * b + 2]; + } + if (ArmMass > 1e-6) { + const double VolM3 = (double)ArmBuoyancyVolume * 0.001 * (double)ArmBuoyancyScale; + const double FArm = (double)FluidDensity * 9.81 * VolM3; + mjtNum F3[3] = { 0.0, 0.0, (mjtNum)FArm }; + mjtNum CoB[3] = { + (mjtNum)(Cx / ArmMass), + (mjtNum)(Cy / ArmMass), + (mjtNum)(Cz / ArmMass) + }; + mj_applyFT(Model, Data, F3, ZeroT, CoB, BodyId, Data->qfrc_applied); + } } + // 【3】姿态力矩:free joint 的 qfrc_applied[3..5] 就是机体系力矩,直接赋值 + double Tx = CachedTorqueX.load() * Fresh; + double Ty = CachedTorqueY.load() * Fresh; + double Tz = CachedTorqueZ.load() * Fresh; + + // 【3b】物理线程兜底姿态控制 + // + // 游戏线程卡顿时(打包版首帧编译着色器最典型,实测卡 10 秒),【10】里的 + // 姿态控制器一次都跑不到,ROV 处于"无控"状态,几秒内就会被自身的不平衡 + // 力矩甩翻(实测 t=10 s 倾角 170°)。这里在物理线程用同样的级联律补一份, + // 按 (1-Fresh) 权重接管,卡顿结束后无缝交回游戏线程。 + if (Fresh < 1.0 && bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { + const int32 FJ = Model->body_jntadr[BodyId]; + if (Model->jnt_type[FJ] == mjJNT_FREE) { + const int32 FD = Model->jnt_dofadr[FJ]; + const double* q = &Data->xquat[4 * BodyId]; + const double qw = q[0], qx = q[1], qy = q[2], qz = q[3]; + const double HalfYaw = FMath::DegreesToRadians((double)TargetYawDeg) * 0.5; + const double Qtw = FMath::Cos(HalfYaw), Qtz = FMath::Sin(HalfYaw); + + const double aw = qw, ax = -qx, ay = -qy, az = -qz; + double Qew = aw * Qtw - az * Qtz; + double Qex = ax * Qtw + ay * Qtz; + double Qey = -ax * Qtz + ay * Qtw; + double Qez = aw * Qtz + az * Qtw; + if (Qew < 0.0) { Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; } + + const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; + const double KpAtt[3] = { 1.5, 1.5, 1.0 }; + const double Wmax[3] = { 0.8, 0.8, 0.6 }; + const double Wn = 5.0; + const double Takeover = 1.0 - Fresh; + + for (int32 Axis = 0; Axis < 3; ++Axis) { + const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] + : Model->body_inertia[3 * BodyId + Axis]; + const double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis], -Wmax[Axis], Wmax[Axis]); + double T = I * Wn * (Wsp - (double)Data->qvel[FD + 3 + Axis]) * Takeover; + T = FMath::Clamp(T, -1500.0, 1500.0); + if (Axis == 0) { Tx += T; } + else if (Axis == 1) { Ty += T; } + else { Tz += T; } + } + } + } + + Data->qfrc_applied[DofAdr + 3] = (mjtNum)Tx; + Data->qfrc_applied[DofAdr + 4] = (mjtNum)Ty; + Data->qfrc_applied[DofAdr + 5] = (mjtNum)Tz; + + // 【4】一次性初始化:implicit 积分器 + 转动 dof 阻尼 + static bool bCtlInitDone = false; + if (!bCtlInitDone) { + bCtlInitDone = true; + Model->opt.integrator = mjINT_IMPLICIT; + const double Iavg = 0.5 * (Model->body_inertia[3 * BodyId + 0] + + Model->body_inertia[3 * BodyId + 1] + + Model->body_inertia[3 * BodyId + 2]) / 3.0; + // 阻尼取值优先级:命令行清零(做流体基线对照)> 属性 > 0.5×平均惯量兜底 + static const bool bNoDofDamp = FParse::Param(FCommandLine::Get(), TEXT("RovNoDofDamp")); + double Idamp = (AngularDofDamping > 0.0f) ? (double)AngularDofDamping : Iavg; + if (bNoDofDamp) { Idamp = 0.0; } + Model->dof_damping[DofAdr + 3] = Idamp; + Model->dof_damping[DofAdr + 4] = Idamp; + Model->dof_damping[DofAdr + 5] = Idamp; + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][版本] mj_versionString=%s"), UTF8_TO_TCHAR(mj_versionString())); + UE_LOG(LogRovBridge, Log, + TEXT("[RovBridge][惯量] body_inertia=(%.3f, %.3f, %.3f) EffInertia=(%.3f, %.3f, %.3f) body_mass=%.3f"), + Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + EffInertia[0], EffInertia[1], EffInertia[2], Model->body_mass[BodyId]); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][控制] integrator=implicit dof_damping=%.2f N.m.s/rad"), Idamp); + + // 机械臂质量分布诊断:-RovArmDiag + // 目的:确认「臂子树 181 kg」是怎么来的(menagerie 原版 UR5e + 2F85 只有 ~21.9 kg), + // 顺便把每个连杆的质量/位置/碰撞体尺寸打出来,判断是不是整体被放大了。 + static const bool bArmDiag = FParse::Param(FCommandLine::Get(), TEXT("RovArmDiag")); + if (bArmDiag) { + const double* RovCg = &Data->xipos[3 * BodyId]; + double ArmMass = 0.0, Mx = 0.0, My = 0.0, Mz = 0.0; + UE_LOG(LogRovBridge, Log, + TEXT("[RovBridge][臂诊断] ROV body[%d]='%s' mass=%.3f subtree=%.3f 惯量=(%.3f, %.3f, %.3f)"), + BodyId, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[BodyId]), + Model->body_mass[BodyId], Model->body_subtreemass[BodyId], + Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2]); + for (int32 b = 0; b < Model->nbody; ++b) { + int32 p = Model->body_parentid[b]; + bool bDescendant = false; + while (p > 0) { + if (p == BodyId) { bDescendant = true; break; } + p = Model->body_parentid[p]; + } + if (!bDescendant) { continue; } + + const double m = Model->body_mass[b]; + const double* x = &Data->xipos[3 * b]; + const double rx = x[0] - RovCg[0], ry = x[1] - RovCg[1], rz = x[2] - RovCg[2]; + ArmMass += m; + Mx += m * rx; My += m * ry; Mz += m * rz; + UE_LOG(LogRovBridge, Log, + TEXT(" [臂] body[%d]='%s' mass=%.4f 相对ROV重心=(%.4f, %.4f, %.4f) m 惯量=(%.5f, %.5f, %.5f)"), + b, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[b]), m, rx, ry, rz, + Model->body_inertia[3 * b + 0], Model->body_inertia[3 * b + 1], + Model->body_inertia[3 * b + 2]); + + // 该 body 的碰撞几何尺寸,用来判断几何有没有被整体放大 + for (int32 g = 0; g < Model->ngeom; ++g) { + if (Model->geom_bodyid[g] != b) { continue; } + UE_LOG(LogRovBridge, Log, + TEXT(" [geom] '%s' type=%d size=(%.4f, %.4f, %.4f) mass=%.4f"), + UTF8_TO_TCHAR(Model->names + Model->name_geomadr[g]), + (int32)Model->geom_type[g], + Model->geom_size[3 * g + 0], Model->geom_size[3 * g + 1], + Model->geom_size[3 * g + 2], Model->body_mass[b]); + } + } + if (ArmMass > 1e-6) { + const double G = 9.81; + // 重力矩 = Σ r × (0,0,-mg) = (-g·Σm·ry, +g·Σm·rx, 0) + UE_LOG(LogRovBridge, Log, + TEXT("[RovBridge][臂诊断] 合计 mass=%.3f 合成质心相对ROV=(%.4f, %.4f, %.4f) m " + "重力矩=(%.1f, %.1f, %.1f) N·m 重量=%.1f N"), + ArmMass, Mx / ArmMass, My / ArmMass, Mz / ArmMass, + -G * My, G * Mx, 0.0, ArmMass * G); + } + } + } + + // 【5】qfrc_applied 累加监测(每秒一次) + { + static double LastQfrcLogT = -1.0; + if (Data->time - LastQfrcLogT > 1.0) { + LastQfrcLogT = Data->time; + const double* qq = &Data->xquat[4 * BodyId]; + const double RollDeg = FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qq[0] * qq[1] + qq[2] * qq[3]), + 1.0 - 2.0 * (qq[1] * qq[1] + qq[2] * qq[2]))); + const double PitchDeg = FMath::RadiansToDegrees( + FMath::Asin(FMath::Clamp(2.0 * (qq[0] * qq[2] - qq[3] * qq[1]), -1.0, 1.0))); + UE_LOG(LogRovBridge, Log, + TEXT("[RovBridge][qfrc] t=%.2f applied=(%.1f, %.1f, %.1f, %.1f, %.1f, %.1f) " + "roll=%.1f° pitch=%.1f° Z=%.3f ncon=%d bias=(%.1f, %.1f, %.1f) " + "constr=(%.1f, %.1f, %.1f)"), + Data->time, + Data->qfrc_applied[DofAdr + 0], Data->qfrc_applied[DofAdr + 1], + Data->qfrc_applied[DofAdr + 2], Data->qfrc_applied[DofAdr + 3], + Data->qfrc_applied[DofAdr + 4], Data->qfrc_applied[DofAdr + 5], + RollDeg, PitchDeg, Data->xpos[3 * BodyId + 2], (int32)Data->ncon, + Data->qfrc_bias[DofAdr + 3], Data->qfrc_bias[DofAdr + 4], Data->qfrc_bias[DofAdr + 5], + Data->qfrc_constraint[DofAdr + 3], Data->qfrc_constraint[DofAdr + 4], + Data->qfrc_constraint[DofAdr + 5]); + } + } // 诊断 static int32 ApplyLogCounter = 0; if (++ApplyLogCounter % 500 == 0) { @@ -1108,7 +1507,7 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) "浮力=%.1f N T=(%.1f,%.1f,%.1f) N·m Z=%.3f"), DofAdr, Data->qfrc_applied[DofAdr + 0], Data->qfrc_applied[DofAdr + 1], Data->qfrc_applied[DofAdr + 2], - Buoy, Tx, Ty, Tz, + Buoy, Data->qfrc_applied[DofAdr + 3], Data->qfrc_applied[DofAdr + 4], Data->qfrc_applied[DofAdr + 5], Data->xpos[3 * BodyId + 2]); } } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h index 1ab3d139..2c662744 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h @@ -147,14 +147,28 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent /** * 浮力倍率。1.0 = 严格按 ρ·g·V。 * - * 当前默认 3.2 是为了补偿一个已知的质量归属 bug:XML 里 rov_body 的 - * mass="150.0" 被导入器分到了 gripper_base_mount 上,导致 ROV 本体变成 - * 438 kg、整机 612.9 kg,而排水体积只有 0.192 m³。 - * 3.2 ≈ 612.9 / (1000 × 0.192),配平到中性浮态。 - * 质量 bug 修好后应改回 1.0。 + * 历史:XML 里 rov_body 的 mass="150.0" 曾被导入器分到 gripper_base_mount 上 + * (夹爪网格单位是 mm,被当成 cm,放大 10 倍 → 体积 ×1000 → 150 kg), + * 整机 612.9 kg,只能靠 3.19 的倍率硬配到中性。 + * 2026-11-08 已修:夹爪网格按 cm 重导、并给 gripper_base_mount 写死 0.15 kg, + * 整机回到 460.5 kg。现在按 0.192 m³ 排水算,中性所需倍率 ≈ 2.34。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") - float BuoyancyScale = 3.1923f; + float BuoyancyScale = 2.34f; + + /** + * 机械臂排水体积(升)。 + * + * UR5e 七个连杆的 OBJ 用散度定理实测合计 **11.73 L**,2F85 夹爪约 0.35 L, + * 取 12.1 L。浮力作用在臂的合成质心处,相当于给臂绑浮力块。 + * 设为 0 可关闭,用来做对照实验。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float ArmBuoyancyVolume = 12.1f; + + /** 机械臂浮力系数,1.0 = 完全按真实排水体积;0.9 表示臂仍有 10% 净重。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float ArmBuoyancyScale = 1.0f; /** * 浮心相对重心的高度(m),沿机体系 +Z。 @@ -214,15 +228,25 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent /** 姿态闭环带宽(rad/s)。增益按 I·ω² 自动缩放,换惯量不用重调。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") - float AttitudeBandwidth = 2.5f; + float AttitudeBandwidth = 1.0f; /** 阻尼比。1.0 = 临界阻尼,不超调。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") float AttitudeZeta = 0.8f; + /** + * 姿态积分增益(rad/s per rad·s)。 + * + * 纯 P 遇到恒定力矩(质心与浮心不重合、机械臂残余重量)会停在固定倾角上, + * 实测曾达到 roll −28°。积分项把它消掉,积分值限幅 ±2.0 rad/s。 + * 设 0 可退回纯 P 做对照。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeIntegralGain = 0.5f; + /** 积分增益系数(相对 I·ω³)。用于消除恒定扰动造成的稳态误差。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") - float AttitudeIntegralFactor = 0.5f; + float AttitudeIntegralFactor = 0.0f; /** 目标航向(度)。启动时自动捕获当前航向。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") @@ -232,6 +256,36 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") float YawRateDegPerSec = 60.0f; + /** + * 机械臂静力矩配平(机体系,N·m)。**默认 0,实测常值前馈不成立**。 + * + * 2026-11-08 实测:不加前馈时稳态 qfrc 俯仰约 −166 ~ −215 N·m(机械臂还在缓慢下沉, + * 数值本身在漂);加 −215 N·m 前馈后反而变成 −241 N·m。原因是臂的重力力矩 + * 随 ROV 俯仰角变化,常值前馈只会把平衡点挪到另一个俯仰角,抵不掉。 + * 真正的配平要动物理:臂浮力块 / 压载 / 把臂收进重心。这里保留接口供以后试。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float TrimTorquePitch = 0.0f; + + /** 横滚方向配平力矩(N·m),用法同 TrimTorquePitch。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float TrimTorqueRoll = 0.0f; + /** 水平(roll/pitch)回调带宽 rad/s */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float LevelBandwidth = 1.5f; + + /** 水平阻尼比 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float LevelZeta = 0.8f; + + /** 航向回调带宽 rad/s */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float YawBandwidth = 1.0f; + + /** 航向阻尼比 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float YawZeta = 1.0f; + // ---------------------------- 洋流 ---------------------------- /** 洋流拖曳系数(kg/s)。 */ @@ -247,7 +301,17 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent * 所以这里作为**独立受力项**补一个可调角阻尼,与流体模型叠加。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") - float AngularDampingCoeff = 10.0f; + float AngularDampingCoeff = 0.0f; // 改由 dof_damping 提供 + + /** + * 转动自由度阻尼(N·m·s/rad),写进 MuJoCo 的 `dof_damping[3..5]`。 + * + * 这是**水阻尼**里 MuJoCo 椭球模型低估的那部分(导流罩、推进器导管、 + * 外挂设备的形式阻力)。数值由 `-RovSpinTest` 的 spin-down 实验辨识, + * 不是拍脑袋的占位值。<= 0 表示退回「0.5 × 平均惯量」的兜底值。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float AngularDofDamping = 13.0f; /** * 是否启用洋流(三层级联:Gauss-Markov + 分层 + 湍流)。 @@ -327,6 +391,16 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent std::atomic CachedBodyId{-1}; + /** + * 控制指令的时间戳(MuJoCo 仿真时间)。 + * + * 控制力矩在**游戏线程**按帧算,物理在**独立线程**按 500 Hz 跑。 + * 打包版启动时有一段长卡顿(着色器编译),游戏线程几秒不更新, + * 上一帧的力矩会被冻结着一直施加 —— 实测 ROV 在 5 秒内被甩到 159°。 + * 物理线程据此判断指令是否过期,过期就把推力/力矩归零。 + */ + std::atomic CachedControlTime{0.0}; + /** pre-step 回调只注册一次。 */ bool bControlCallbackRegistered = false; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp index 340206cf..d260e91a 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.cpp @@ -4,3 +4,8 @@ RovPawnEvents::ActuatorsSignal& RovPawnEvents::getActuatorSignal() { return actuator_signal_; } + +RovPawnEvents::KeyboardPwmSignal& RovPawnEvents::getKeyboardPwmSignal() +{ + return keyboard_pwm_signal_; +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h index cf5dae9a..5296eb72 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnEvents.h @@ -22,10 +22,13 @@ class RovPawnEvents : public PawnEvents }; typedef common_utils::Signal&> ActuatorsSignal; + typedef common_utils::Signal&> KeyboardPwmSignal; public: ActuatorsSignal& getActuatorSignal(); + KeyboardPwmSignal& getKeyboardPwmSignal(); private: ActuatorsSignal actuator_signal_; + KeyboardPwmSignal keyboard_pwm_signal_; }; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h new file mode 100644 index 00000000..c10137e0 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h @@ -0,0 +1,221 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// 从 E:\Projects\mujoco_plugin\mujoco_plugin\src\underwater 移植过来的水下物理模型: +// - FRovOceanCurrent ← ocean_current.py 的 OceanCurrentCascade +// - FRovPIDDepthController ← depth_controller.py 的 PIDDepthController +// +// 头文件内联实现,不新增 .cpp,避免改动 AirSim 模块的构建文件。 + +#pragma once + +#include "CoreMinimal.h" +#include "Math/RandomStream.h" + +// ============================================================================= +// 洋流:三层级联 +// V_current(x,y,z,t) = V_GaussMarkov(t) + V_stratified(z) + V_turbulent(x,y,z) +// 坐标约定:MuJoCo 世界系,Z 向上,单位 m/s。 +// ============================================================================= +struct FRovOceanCurrent +{ + /** 分层剖面的一层 */ + struct FLayer + { + float DepthM = 0.0f; // 深度(负值表示水下) + float Speed = 0.0f; // 流速幅值 m/s + float HorizAngle = 0.0f; // 水平角 rad + float VertAngle = 0.0f; // 垂直角 rad(正 = 向下) + }; + + // ---------------------------- 配置 ---------------------------- + + bool bEnabled = true; + + // Gauss-Markov + float MeanSpeed = 0.5f; + float MeanHorizAngle = 0.0f; + float MeanVertAngle = 0.0f; + float Mu = 0.0f; + float NoiseAmp = 0.1f; + + // 分层 + TArray Layers; + + // 湍流 + float TurbIntensity = 0.15f; + float TurbIntegralScale = 1.0f; + + /** + * 洋流速度幅值上限(m/s)。 + * + * 原版 config 里 mu=0,Gauss-Markov 是纯随机游走,没有均值回归, + * 速度幅值会随时间无界漂大(实测跑到几十 m/s,把 ROV 直接推飞)。 + * 这里加一个物理上限:真实洋流很少超过 2 m/s。 + */ + float MaxSpeed = 2.0f; + + int32 Seed = 42; + + // ---------------------------- 状态 ---------------------------- + + float CurSpeed = 0.0f; + float CurHoriz = 0.0f; + float CurVert = 0.0f; + + FVector LastPos = FVector::ZeroVector; + bool bHasLastPos = false; + + FRandomStream Rng; + + void Initialize() + { + Rng.Initialize(Seed); + if (Layers.Num() == 0) { + // 与 config.json 的 stratified_layers 一致 + Layers.Add({ 0.0f, 0.0f, 0.0f, 0.0f }); + Layers.Add({ -10.0f, 0.5f, 0.3f, 0.0f }); + Layers.Add({ -50.0f, 1.0f, 0.5f, 0.1f }); + } + // 按深度升序(最深在前) + Layers.Sort([](const FLayer& A, const FLayer& B) { return A.DepthM < B.DepthM; }); + Reset(); + } + + void Reset() + { + CurSpeed = MeanSpeed; + CurHoriz = MeanHorizAngle; + CurVert = MeanVertAngle; + bHasLastPos = false; + LastPos = FVector::ZeroVector; + } + + /** Gauss-Markov 层:速度幅值 + 两个角度做均值回归随机游走 */ + FVector UpdateGaussMarkov(float Dt) + { + const float SqrtDt = FMath::Sqrt(FMath::Max(Dt, 1e-6f)); + + CurSpeed += Mu * (MeanSpeed - CurSpeed) * Dt + + NoiseAmp * SqrtDt * Rng.GetFraction() * 2.0f - NoiseAmp * SqrtDt; + CurSpeed = FMath::Clamp(CurSpeed, 0.0f, MaxSpeed); + + CurHoriz += (Mu * (MeanHorizAngle - CurHoriz) + + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; + CurVert += (Mu * (MeanVertAngle - CurVert) + + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; + + const float CosV = FMath::Cos(CurVert); + // NED(Z 向下) → MuJoCo(Z 向上):vz 取反 + return FVector( + CurSpeed * CosV * FMath::Cos(CurHoriz), + CurSpeed * CosV * FMath::Sin(CurHoriz), + -CurSpeed * FMath::Sin(CurVert)); } + + /** 分层层:按深度线性插值 */ + FVector StratifiedAt(float DepthM) const + { + if (Layers.Num() == 0) return FVector::ZeroVector; + + auto LayerVec = [](const FLayer& L) { + const float CosV = FMath::Cos(L.VertAngle); + return FVector( + L.Speed * CosV * FMath::Cos(L.HorizAngle), + L.Speed * CosV * FMath::Sin(L.HorizAngle), + -L.Speed * FMath::Sin(L.VertAngle)); + }; + + if (DepthM <= Layers[0].DepthM) { + return LayerVec(Layers[0]); + } + for (int32 i = 0; i < Layers.Num() - 1; ++i) { + if (DepthM >= Layers[i].DepthM && DepthM <= Layers[i + 1].DepthM) { + const float Span = Layers[i + 1].DepthM - Layers[i].DepthM; + const float T = (Span > KINDA_SMALL_NUMBER) + ? (DepthM - Layers[i].DepthM) / Span : 0.0f; + return FMath::Lerp(LayerVec(Layers[i]), LayerVec(Layers[i + 1]), T); + } + } + return LayerVec(Layers.Last()); + } + + /** 湍流层:空间相关随机扰动 */ + FVector TurbulentAt(const FVector& Pos) + { + float NoiseStrength = TurbIntensity; + if (bHasLastPos) { + const float Dist = FVector::Dist(Pos, LastPos); + const float Correlation = FMath::Exp(-Dist / FMath::Max(TurbIntegralScale, 1e-6f)); + NoiseStrength = TurbIntensity * FMath::Sqrt(FMath::Max(0.0f, 1.0f - Correlation * Correlation)); + } + LastPos = Pos; + bHasLastPos = true; + + const FVector Noise( + Rng.GetFraction() * 2.0f - 1.0f, + Rng.GetFraction() * 2.0f - 1.0f, + Rng.GetFraction() * 2.0f - 1.0f); + return Noise * NoiseStrength * 0.5f; + } + + /** 总洋流速度(MuJoCo 世界系,m/s) */ + FVector GetVelocity(const FVector& Pos, float Dt) + { + if (!bEnabled) return FVector::ZeroVector; + + const FVector VGM = UpdateGaussMarkov(Dt); + const FVector VStrat = StratifiedAt(-Pos.Z); // z 向上 → 深度取反 + const FVector VTurb = TurbulentAt(Pos); + return VGM + VStrat + VTurb; + } +}; + +// ============================================================================= +// PID 定深控制器 +// F = Kp·e + Ki·∫e·dt + Kd·ė + feedforward·V_current_z +// 坐标约定:Z 向上。正推力 = 向上。 +// ============================================================================= +struct FRovPIDDepthController +{ + // 增益基于 depth_controller.py 的 Kp=2000 / Ki=50 / Kd=1000, + // 但那组值是针对 150kg 的模型调的;本工程整机 612.9kg, + // 积分项积累太慢会留下约 0.5m 的稳态误差,所以 Ki 放大到 500。 + float Kp = 2000.0f; + float Ki = 500.0f; + float Kd = 1000.0f; + float MaxThrust = 5000.0f; + float FeedforwardGain = 500.0f; + + float IntegralError = 0.0f; + float PrevError = 0.0f; + + void Reset() + { + IntegralError = 0.0f; + PrevError = 0.0f; + } + + /** + * @param TargetZ 目标高度(MuJoCo 世界系,m) + * @param CurrentZ 当前高度 + * @param Dt 时间步长 + * @param CurrentVz 当前位置的洋流 Z 分量(正 = 向上) + * @return 需要的 Z 轴推力(N),已限幅 + */ + float Compute(float TargetZ, float CurrentZ, float Dt, float CurrentVz) + { + const float Error = TargetZ - CurrentZ; + + IntegralError += Error * Dt; + IntegralError = FMath::Clamp(IntegralError, -100.0f, 100.0f); // anti-windup + + const float Derivative = (Dt > KINDA_SMALL_NUMBER) ? (Error - PrevError) / Dt : 0.0f; + + float Thrust = Kp * Error + Ki * IntegralError + Kd * Derivative; + + // 洋流前馈:洋流向上(+vz)时提前减少推力 + Thrust += FeedforwardGain * CurrentVz; + + PrevError = Error; + return FMath::Clamp(Thrust, -MaxThrust, MaxThrust); + } +}; From 1b3e506f075d8eaedf4995187882e03224ea1f88 Mon Sep 17 00:00:00 2001 From: shark Date: Mon, 9 Nov 2026 12:42:30 +0800 Subject: [PATCH 5/8] =?UTF-8?q?docs(rov):=20=E7=B2=BE=E7=AE=80=20ROV=20?= =?UTF-8?q?=E6=BA=90=E7=A0=81=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 现象:ROV 相关源码的注释里带有日期、内部文档路径、调研结论与历史对比, 属于项目内部信息,不适合作为对外代码的说明,也增加维护成本。 解决:注释只保留单位、坐标系、限幅值、回退行为等外部读者需要的信息, 删除版本叙事、内部引用与设计论证。无逻辑改动,净减约 150 行注释。 验证:文件与 hutb 仓库同源,已在 CarlaUE4Editor 下编译通过。 --- .../Source/Vehicles/Rov/RovMuJoCoBridge.cpp | 106 +++-------- .../Source/Vehicles/Rov/RovMuJoCoBridge.h | 170 +++++------------- .../AirSim/Source/Vehicles/Rov/RovPawn.cpp | 4 +- .../AirSim/Source/Vehicles/Rov/RovPawn.h | 3 +- .../Source/Vehicles/Rov/RovPawnSimApi.cpp | 7 +- .../Source/Vehicles/Rov/RovPawnSimApi.h | 23 +-- .../Vehicles/Rov/RovUnderwaterPhysics.h | 15 +- 7 files changed, 88 insertions(+), 240 deletions(-) diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp index c3b7f116..96c2b148 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp @@ -241,19 +241,12 @@ FString URovMuJoCoBridge::GetResolvedBodyName() const return TargetBody.IsValid() ? TargetBody->GetName() : FString(); } -// ============================================================================= // 键盘推进 -// ============================================================================= // -// 为什么走 xfrc_applied 而不是 AirSim 的 RC 通道: -// 1. 桥接把 ARovPawn 的物理和碰撞全关了,AirSim 算出的推力没有物理体承接; -// 2. MuJoCo 模型里 7 个 actuator 全是机械臂关节,推进器只是视觉几何体, -// 没有对应的 ,没法用 ctrl 驱动。 -// -// xfrc_applied 是 MuJoCo 给每个 body 预留的 6 维外力(力 xyz + 力矩 xyz), -// mj_step 会把它叠加进动力学,且**不会自动清零**,所以每帧必须先清再写。 -// -// 力的量级按质量缩放(力 = 质量 × 加速度),这样换一台不同质量的 ROV 手感一致。 +// 推力通过 MuJoCo 的 xfrc_applied 施加:桥接关闭了 ARovPawn 的物理与碰撞, +// AirSim 的 RC 通道没有物理体承接;模型里的 actuator 只有机械臂关节。 +// xfrc_applied 不会被 mj_step 自动清零,每帧需先清再写。 +// 力的量级按质量缩放,换不同质量的 ROV 手感一致。 void URovMuJoCoBridge::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) @@ -458,9 +451,8 @@ void URovMuJoCoBridge::ApplyControlForce() Weight, Buoyancy, Buoyancy - Weight, DisplacedVolume, BuoyancyScale); } - // 复合惯量延迟计算:qM 只有在 mj_forward/mj_step 之后才有效。 - // 首帧就取会拿到垃圾值(打包版实测读到极小值 → 控制器力矩≈0 → 缓慢翻转), - // 所以等仿真时间过 0.2 s 再算一次,并且对量级做下限校验。 + // 复合惯量延迟计算:qM 只在 mj_forward/mj_step 之后有效, + // 因此等仿真时间过 0.2 s 再取,并对量级做下限校验。 { static bool bEffInertiaDone = false; if (!bEffInertiaDone && Data->time > 0.2 && Data->qM) { @@ -610,14 +602,9 @@ void URovMuJoCoBridge::ApplyControlForce() // 【航向调整】← / → 连续改变目标航向(姿态保持开启时生效) // - // 方向定义:MuJoCo→UE 的坐标映射是 X→X、Y→-Y、Z→Z(MjUtils::MjToUERotation), - // 属于**镜像变换**,所以 MuJoCo 里的 +yaw 在画面上表现为 -yaw。这里按操作者 - // 看到的画面定义按键方向:→ = 画面右转 = MuJoCo yaw 减小,← 反之。 - // - // 为什么要给前馈:以前只把目标航向按 60°/s 往前推,而内环最大角速度只有 - // 0.6 rad/s(34°/s),目标跑得比机身快,误差会一路累积;一旦超过 180°, - // 四元数短弧归一化会把误差翻号,机身就反着转 —— 表现就是"转满一圈后回弹"。 - // 现在按住期间直接给期望角速度(前馈),误差只反映跟踪滞后,永远不会绕圈。 + // 方向按画面定义:MuJoCo→UE 为镜像映射(Y 取反),MuJoCo 的 +yaw 在画面上是 -yaw, + // 因此 → = 画面右转 = MuJoCo yaw 减小。 + // 按住期间同时给出角速度前馈,避免目标航向跑得比机身快导致误差累积过 180°。 double YawCmdRateDeg = 0.0; // 本帧航向角速度指令(deg/s,MuJoCo 符号) if (bEnableAttitudeHold && PC) { const bool bYawLeft = PC->IsInputKeyDown(EKeys::Left); @@ -887,17 +874,15 @@ void URovMuJoCoBridge::ApplyControlForce() // 【8】浮力的回复力矩由 mj_applyFT 在浮心处施加时自动产生,这里不再手算。 - // 【9】附加角阻尼:以**力矩**形式施加(τ = -Kd·ω,机体系)。 - // MuJoCo 椭球流体模型的旋转阻力偏弱,真实 ROV 的导流罩/鳍会额外提供阻力。 - // Kd = AngularDampingCoeff × 平均主惯量,保持和之前"每秒衰减 1/s"的语义一致。 + // 【9】附加角阻尼:τ = -Kd·ω(机体系),与流体模型叠加。 + // Kd = AngularDampingCoeff × 平均主惯量。 double TorqueX = 0.0, TorqueY = 0.0, TorqueZ = 0.0; double WorldTorqueX = 0.0, WorldTorqueY = 0.0, WorldTorqueZ = 0.0; if (AngularDampingCoeff > 0.0f && Model->body_jntnum[BodyId] >= 1) { const int32 DampJnt = Model->body_jntadr[BodyId]; if (Model->jnt_type[DampJnt] == mjJNT_FREE) { const int32 DampDof = Model->jnt_dofadr[DampJnt]; - // 同样要用**复合惯量**:body_inertia 只有本体,实测复合惯量约为其 4 倍, - // 按本体算会让角阻尼衰减率慢 4 倍(P7 退步的原因)。 + // 用复合惯量:body_inertia 只含本体,不含机械臂。 const double Iavg = (EffInertia[0] + EffInertia[1] + EffInertia[2]) / 3.0; const double Kd = (double)AngularDampingCoeff * Iavg; TorqueX += -Kd * Data->qvel[DampDof + 3]; @@ -906,17 +891,8 @@ void URovMuJoCoBridge::ApplyControlForce() } } - // 【10】姿态保持控制器(四元数 PD + 积分) - // - // 误差用四元数旋转矢量 e = 2·sign(qw)·qv,而不是欧拉角差: - // - 无万向节死锁(ROV 做大俯仰时欧拉角会奇异) - // - 小角度下 e 就是旋转矢量,|e| ≤ 2 天然限幅 - // - sign(qw) 取短弧,避免命令转 350° 而不是 10° - // - // 增益按惯量缩放(Kp = I·ω²、Kd = 2I·ζ·ω),换模型不用重调。 - // 积分项是必需的:机械臂挂在一侧产生恒定扰动力矩, - // 纯 PD 只能把它平衡在某个固定倾角上,消不掉稳态误差。 - // 旧的四元数三轴控制器存在符号/轴向问题,已停用;改用下方世界系解耦控制 + // 旧的四元数 PD 姿态控制器(已停用,保留备查)。 + // 误差 e = 2·sign(qw)·qv,增益按惯量缩放;由下方【10】级联控制器取代。 if (false && bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { const int32 AttJnt = Model->body_jntadr[BodyId]; if (Model->jnt_type[AttJnt] == mjJNT_FREE) { @@ -1029,10 +1005,10 @@ void URovMuJoCoBridge::ApplyControlForce() } } - // 【10】级联姿态控制(按调研报告 C1-C4) + // 【10】级联姿态控制 // 外环:姿态误差 -> 期望角速度(机体系) - // 内环:期望角速度 - 实测角速度(qvel[3:6] 即机体系)-> 力矩 - // 力矩直接写 qfrc_applied 后三维(机体系),不再经 RotByQuat/mj_applyFT + // 内环:期望角速度 - 实测角速度(qvel[3:6] 为机体系)-> 力矩 + // 力矩直接写 qfrc_applied[3:5](机体系) if (bEnableAttitudeHold && !bSpinTest && Model->body_jntnum[BodyId] >= 1) { const int32 NJ = Model->body_jntadr[BodyId]; if (Model->jnt_type[NJ] == mjJNT_FREE) { @@ -1208,19 +1184,8 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) return; } - // ========================================================================= - // 通道:mj_applyFT 直接累加进 qfrc_applied。 - // - // 为什么不用其它两种写法: - // - xfrc_applied:元素顺序有两套说法(官方文档力在前,URLab 的 - // MjBind.h:574 注释写力矩在前),写反会让"向前"变成"俯仰"。 - // - 直接写 qfrc_applied 后三维:实测轴系对不上 —— 只给"轴 0" - // 100 N·m,结果 omega 主要绕 Y 转(-1.03 对 -0.24)。 - // 姿态控制器的力矩因此打到了错误轴上,积分也在错误轴上卷。 - // - // mj_applyFT 把 force / torque / point 分开传参、**全部世界系**, - // 自动补 r×F,没有布局和轴系歧义,还能让浮力正确地作用在浮心。 - // ========================================================================= + // 力与力矩统一用 mj_applyFT 施加:force / torque / point 分开传参、全部世界系, + // 自动补 r×F,避免 xfrc_applied 的元素顺序歧义,也能让浮力作用在浮心。 if (Model->body_jntnum[BodyId] < 1) { return; } @@ -1235,10 +1200,9 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) Data->qfrc_applied[DofAdr + k] = 0.0; } - // 指令新鲜度:控制量在游戏线程按帧算,物理线程按 500 Hz 跑。 - // 游戏线程卡顿(打包版启动时的着色器编译最典型)时,旧指令会被冻结着 - // 一直施加,等于给 ROV 灌几秒恒定力矩 —— 实测 5 秒内被甩到 159°。 - // 这里按「距上次控制更新经过的仿真时间」做淡出:>0.1 s 开始衰减,>0.2 s 归零。 + // 指令新鲜度:控制量在游戏线程按帧更新,物理线程按 500 Hz 步进。 + // 游戏线程卡顿时旧指令会被持续施加,这里按仿真时间做淡出: + // 超过 0.1 s 开始衰减,超过 0.2 s 归零。 const double CmdAge = (double)Data->time - CachedControlTime.load(); const double Fresh = FMath::Clamp(1.0 - (CmdAge - 0.10) / 0.10, 0.0, 1.0); @@ -1258,12 +1222,8 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) }; mj_applyFT(Model, Data, F1, ZeroT, CG, BodyId, Data->qfrc_applied); - // 【2】浮力:作用在**整机浮心** = 整机合成质心 + R·(0,0,BG) - // - // 浮力必须加在「整机(艇体 + 机械臂)的合成质心」上,而不是艇体自身的质心: - // 排水体积属于整个载体,浮心跟着整机质量分布走。之前加在艇体质心上, - // 臂的 22 kg 把整机质心拉偏了约 18 mm,4518 N × 18 mm ≈ 81 N·m 的恒定力矩, - // 姿态控制器只能靠 28° 的倾斜去平衡(实测 roll = −27.9°)。 + // 【2】浮力:作用在整机浮心 = 整机合成质心 + R·(0,0,BG)。 + // 排水体积属于整个载体,浮心应跟随整机质量分布,而非艇体自身质心。 const double Buoy = CachedBuoyancy.load(); double TotalMass = 0.0, Cx = 0.0, Cy = 0.0, Cz = 0.0; @@ -1288,8 +1248,7 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) } else { Cx = CG[0]; Cy = CG[1]; Cz = CG[2]; } - // 用 MuJoCo 自己算的子树质心做校验:两者必须一致,否则说明重力力矩和浮力 - // 作用点不重合,会留下一个恒定的姿态误差(曾经差 17 mm ≈ 78 N·m)。 + // 优先使用 MuJoCo 计算的子树质心,保证与重力作用点一致。 if (Data->subtree_com) { Cx = Data->subtree_com[3 * BodyId + 0]; Cy = Data->subtree_com[3 * BodyId + 1]; @@ -1310,12 +1269,8 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) mj_applyFT(Model, Data, F2, ZeroT, CBw, BodyId, Data->qfrc_applied); } - // 【2b】机械臂浮力:按真实排水体积,在臂的合成质心处施加向上浮力 - // - // 为什么必须给臂加浮力:UR5e 是空气中的工业臂,密度远大于水。 - // 整条臂 22.0 kg、排水只有约 12 L,在水里净重约 98 N,挂在艇体前方 0.13 m 处, - // 会持续产生约 8 N·m 的俯仰力矩。真实 ROV 的做法就是给臂绑浮力块(泡沫/浮筒), - // 这里按「等排水体积」把它补上,而不是用常值力矩硬顶。 + // 【2b】机械臂浮力:按排水体积在臂的合成质心处施加向上浮力, + // 等价于给机械臂绑浮力块。 if (ArmBuoyancyVolume > 0.0f) { double ArmMass = 0.0, Cx = 0.0, Cy = 0.0, Cz = 0.0; for (int32 b = 0; b < Model->nbody; ++b) { @@ -1351,11 +1306,8 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) double Tz = CachedTorqueZ.load() * Fresh; // 【3b】物理线程兜底姿态控制 - // - // 游戏线程卡顿时(打包版首帧编译着色器最典型,实测卡 10 秒),【10】里的 - // 姿态控制器一次都跑不到,ROV 处于"无控"状态,几秒内就会被自身的不平衡 - // 力矩甩翻(实测 t=10 s 倾角 170°)。这里在物理线程用同样的级联律补一份, - // 按 (1-Fresh) 权重接管,卡顿结束后无缝交回游戏线程。 + // 游戏线程卡顿时【10】不会执行,ROV 处于无控状态。这里用同样的级联律 + // 按 (1-Fresh) 权重接管,卡顿结束后交回游戏线程。 if (Fresh < 1.0 && bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { const int32 FJ = Model->body_jntadr[BodyId]; if (Model->jnt_type[FJ] == mjJNT_FREE) { diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h index 2c662744..729fd635 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h @@ -1,28 +1,13 @@ // Copyright (c) OpenHUTB. All rights reserved. // -// 把 AirSim 的 ROV Pawn 改造成 MuJoCo 的「纯跟随者」。 +// ROV 位姿跟随桥接。 // -// 设计依据:docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md +// MuJoCo 是唯一的位姿真值源,本组件每帧把 ROV 根 body 的世界位姿提供给 +// RovPawnSimApi,后者据此驱动 Pawn。AirSim 自身的 ROV 物理与 UE 侧碰撞全部关闭, +// 避免同一台 ROV 被两套物理重复积分。 // -// 背景 -// ---- -// 改造前,同一台 ROV 的位姿有两个写入者: -// - AirSim 的 rov_physics_body_(在 RovPawnSimApi 里积分) -// - MuJoCo 的 AMjArticulation(在 URLab 里积分) -// 被调研的 16 个开源仿真系统里没有一个允许这种「双所有权」配置 -// (docs/research/开源仿真器物理与视觉统一架构调研.html §7 第 3 条)。 -// -// 改造后 -// ------ -// MuJoCo 是唯一真值源;本组件每帧把 ROV 根 body 的世界位姿提供给 -// RovPawnSimApi,后者据此驱动 Pawn。AirSim 自身的 ROV 物理与 UE 侧碰撞 -// 全部关闭,避免「同一个力被算两次」(调研报告 §6 第 1 条)。 -// -// 关于 Tick 顺序 -// -------------- -// 不需要 AddTickPrerequisiteComponent:UMjBody::GetWorldPosition() / -// GetWorldRotation() 直接读 MuJoCo 的 m_BodyView.xpos / xquat 数组, -// 不依赖组件自身的 TickComponent 是否已经跑过,所以拿到的永远是最新物理状态。 +// 位姿直接读 MuJoCo 的 xpos / xquat 数组,不依赖 UMjBody 的 Tick 顺序, +// 因此不需要 AddTickPrerequisiteComponent。 #pragma once @@ -80,32 +65,19 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent /** * 是否自动启动 MuJoCo 仿真。 - * - * UMjPhysicsEngine 的 bIsPaused 默认是 true——URLab 的设计是让用户在 - * MjSimulate 控件里手动点「播放」。但 AirSim 的 Pawn 一旦跟随 MuJoCo, - * 仿真不跑 Pawn 就不会动,所以桥接激活时默认自动取消暂停。 - * 需要在编辑器里手动控制时把它关掉。 + * UMjPhysicsEngine 默认暂停,仿真不跑 Pawn 就不会动,因此桥接激活时自动取消暂停。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") bool bAutoStartSimulation = true; /** * MuJoCo 世界坐标到 UE 世界坐标的平移偏移(cm)。 - * - * MuJoCo 的模型是一个自包含的水下世界(自带 seafloor / reef / wellhead), - * 它的原点与 UE 地图没有关系。把 ROV 摆到城区的某个位置时, - * 不能靠挪编辑器里的 Actor——Pawn 的位姿每帧被 MuJoCo 覆盖, - * 必须在桥接这一层加偏移。 + * Pawn 位姿每帧由 MuJoCo 覆盖,摆放位置需要通过该偏移调整。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") FVector WorldOffset = FVector::ZeroVector; - /** - * 键盘推进:W/S 前后、A/D 左右、Q/E 上浮下潜。 - * - * 按受力分解设计:键盘只决定「推进器输出比例」, - * 真正的推力 = 比例 × ThrusterMaxForce,不直接改合力。 - */ + /** 键盘推进:W/S 前后、A/D 左右、Q/E 上浮下潜。键盘只给出推力比例。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") bool bEnableKeyboardControl = true; @@ -114,15 +86,11 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent float ThrusterMaxRatio = 0.5f; /** - * 水面高度,**单位是米(MuJoCo 模型单位)**。 - * - * 已实测确认:MJCF 是米制(rov_body geom size=0.4/0.3/0.2,gravity=9.81, - * timestep=0.002),桥接输出到 UE 时才 ×100 转成厘米 - * (MuJoCo xpos Z=3.00 → Pawn 世界 Z=300cm)。 - * UE 场景水面在 Z=+800cm,对应这里 8.0。 + * 水面高度(米,MuJoCo 模型单位)。 + * MJCF 为米制,桥接输出到 UE 时 ×100 转厘米。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") - float WaterSurfaceZ = -2.0f; // 地图自带海面 Z = -200 cm(Town10 外围水体) + float WaterSurfaceZ = -2.0f; // 地图海面 Z = -200 cm // ---------------------------- 流体 ---------------------------- @@ -145,23 +113,16 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent float DisplacedVolume = 0.192f; /** - * 浮力倍率。1.0 = 严格按 ρ·g·V。 - * - * 历史:XML 里 rov_body 的 mass="150.0" 曾被导入器分到 gripper_base_mount 上 - * (夹爪网格单位是 mm,被当成 cm,放大 10 倍 → 体积 ×1000 → 150 kg), - * 整机 612.9 kg,只能靠 3.19 的倍率硬配到中性。 - * 2026-11-08 已修:夹爪网格按 cm 重导、并给 gripper_base_mount 写死 0.15 kg, - * 整机回到 460.5 kg。现在按 0.192 m³ 排水算,中性所需倍率 ≈ 2.34。 + * 浮力倍率,1.0 = 严格按 ρ·g·V。 + * 按 0.192 m³ 排水、整机 460.5 kg 配平,中性约需 2.34。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") float BuoyancyScale = 2.34f; /** * 机械臂排水体积(升)。 - * - * UR5e 七个连杆的 OBJ 用散度定理实测合计 **11.73 L**,2F85 夹爪约 0.35 L, - * 取 12.1 L。浮力作用在臂的合成质心处,相当于给臂绑浮力块。 - * 设为 0 可关闭,用来做对照实验。 + * UR5e 连杆 11.73 L + 2F85 约 0.35 L,取 12.1 L;浮力作用在臂的合成质心。 + * 设为 0 可关闭。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") float ArmBuoyancyVolume = 12.1f; @@ -172,44 +133,30 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent /** * 浮心相对重心的高度(m),沿机体系 +Z。 - * - * 真实 ROV 的浮心在重心之上,倾斜时浮力产生回复力矩 τ = r_CB × F_buoy, - * 这就是它「被撞歪后自己回正」的物理来源 —— 不需要任何控制器。 - * 回复刚度 ≈ B × BG,BlueROV2 量级 BG = 0.01~0.05 m。 - * 设为 0 则退化为「浮力加在质心」,无被动稳定。 + * 倾斜时浮力产生回复力矩 τ = r_CB × F_buoy,回复刚度 ≈ B × BG。 + * 设为 0 则浮力加在质心,无被动稳定。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") float BuoyancyArm = 0.02f; /** - * 机械臂关节阻尼(N·m·s/rad),施加到所有非自由关节上。 - * - * 模型的 7 个 UR5e 关节没有任何 ctrl 输入,会自由摆动; - * 它们的反作用力矩会把 ROV 本体晃得不停翻滚 - * (实测:两个外部力矩全关、ω 仍能冲到 3.4 rad/s)。 - * 加阻尼让机械臂稳定下垂,不再激励艇体。 + * 机械臂关节阻尼(N·m·s/rad),施加到所有非自由关节。 + * 关节无 ctrl 输入时会自由摆动,反作用力矩会持续激励艇体。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") float ArmJointDamping = 20000.0f; /** - * 机械臂关节刚度(N·m/rad),相当于在每个关节上装一个位置伺服弹簧, - * 把关节锁在 MJCF 的参考角度(qpos0)上。 - * - * 只加阻尼不够:阻尼只抵抗运动,机械臂仍会在重力下缓慢下垂到一个 - * 偏置姿态,那个偏置产生恒定扰动力矩,把 ROV 压在一个固定倾角上 - * (实测约 17°,反推扰动约 130 N·m)。加刚度后机械臂被真正"锁住", - * 扰动消失 —— 这也符合真实 ROV 的机械臂由伺服保持的实际情况。 + * 机械臂关节刚度(N·m/rad),相当于在每个关节上加位置伺服弹簧, + * 把关节锁在 MJCF 的 qpos0 上。只加阻尼时机械臂仍会在重力下缓慢下垂, + * 偏置姿态产生恒定扰动力矩。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") float ArmJointStiffness = 0.0f; /** * 机械臂重力补偿:把机械臂各 body 的 gravcomp 设为 1.0,抵消其自重。 - * - * 机械臂(约 22 kg)挂在 ROV 一侧,静重产生约 130 N·m 的恒定扰动力矩, - * 把艇体压在一个固定倾角上。真实 ROV 的机械臂靠伺服与配重平衡掉这部分, - * 仿真里用 MuJoCo 的 gravcomp 等效实现,同时保留惯量与碰撞。 + * 保留惯量与碰撞。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") bool bArmGravityComp = false; @@ -235,11 +182,8 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent float AttitudeZeta = 0.8f; /** - * 姿态积分增益(rad/s per rad·s)。 - * - * 纯 P 遇到恒定力矩(质心与浮心不重合、机械臂残余重量)会停在固定倾角上, - * 实测曾达到 roll −28°。积分项把它消掉,积分值限幅 ±2.0 rad/s。 - * 设 0 可退回纯 P 做对照。 + * 姿态积分增益(rad/s per rad·s)。消除恒定力矩造成的固定倾角, + * 积分值限幅 ±2.0 rad/s;设 0 退回纯 P。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") float AttitudeIntegralGain = 0.5f; @@ -257,12 +201,8 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent float YawRateDegPerSec = 60.0f; /** - * 机械臂静力矩配平(机体系,N·m)。**默认 0,实测常值前馈不成立**。 - * - * 2026-11-08 实测:不加前馈时稳态 qfrc 俯仰约 −166 ~ −215 N·m(机械臂还在缓慢下沉, - * 数值本身在漂);加 −215 N·m 前馈后反而变成 −241 N·m。原因是臂的重力力矩 - * 随 ROV 俯仰角变化,常值前馈只会把平衡点挪到另一个俯仰角,抵不掉。 - * 真正的配平要动物理:臂浮力块 / 压载 / 把臂收进重心。这里保留接口供以后试。 + * 机械臂静力矩配平(机体系,N·m),常值前馈。 + * 臂的重力矩随俯仰角变化,常值前馈无法完全抵消,默认 0。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") float TrimTorquePitch = 0.0f; @@ -293,33 +233,20 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent float CurrentDragCoeff = 10.0f; /** - * 附加角阻尼(1/s)。 - * - * MuJoCo 椭球流体模型本身有旋转阻力,但实测偏弱:给 3 rad/s 三轴脉冲后 - * 4 秒才衰减到 1.0 rad/s,撞一下管道仍然会明显打转。 - * 真实 ROV 的导流罩、推进器导管、外挂设备都会提供额外的旋转阻力, - * 所以这里作为**独立受力项**补一个可调角阻尼,与流体模型叠加。 + * 附加角阻尼(1/s),与流体模型叠加,作为独立受力项。 + * 默认 0,旋转阻尼由 AngularDofDamping 提供。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") - float AngularDampingCoeff = 0.0f; // 改由 dof_damping 提供 + float AngularDampingCoeff = 0.0f; /** - * 转动自由度阻尼(N·m·s/rad),写进 MuJoCo 的 `dof_damping[3..5]`。 - * - * 这是**水阻尼**里 MuJoCo 椭球模型低估的那部分(导流罩、推进器导管、 - * 外挂设备的形式阻力)。数值由 `-RovSpinTest` 的 spin-down 实验辨识, - * 不是拍脑袋的占位值。<= 0 表示退回「0.5 × 平均惯量」的兜底值。 + * 转动自由度阻尼(N·m·s/rad),写入 MuJoCo 的 dof_damping[3..5]。 + * 由 spin-down 实验辨识得到;<= 0 时退回「0.5 × 平均惯量」。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") float AngularDofDamping = 13.0f; - /** - * 是否启用洋流(三层级联:Gauss-Markov + 分层 + 湍流)。 - * - * 实测正常:V_current≈0.6~0.7 m/s,拖曳力≈6~7 N, - * ROV 缓慢加速到约 0.1 m/s 随流漂移 —— 这是期望的水下行为。 - * (早前误判"被推飞",是把日志里 ×100 的厘米值当成了米。) - */ + /** 是否启用洋流(三层级联:Gauss-Markov + 分层 + 湍流)。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") bool bEnableOceanCurrent = true; @@ -353,26 +280,22 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent bool ProvidePose(FVector& OutPosition, FQuat& OutRotation) const; /** - * 找到 AMjArticulation 与目标 UMjBody。 - * - * 声明为 const + mutable 成员:MuJoCo 模型通常在 Pawn 生成之后才编译完成, - * 所以 ProvidePose(const)需要在运行中惰性重试解析。 + * 解析 AMjArticulation 与目标 UMjBody。 + * 模型通常在 Pawn 生成之后才编译完成,因此声明为 const + mutable, + * 由 ProvidePose 在运行中惰性重试。 */ void ResolveTargets() const; - /** 关闭 Pawn 物理与碰撞,消除双计。 */ + /** 关闭 Pawn 物理与碰撞,避免与 MuJoCo 重复计算。 */ void DisablePawnPhysics(); - /** 每帧把键盘输入换算成力,写进 mjData::xfrc_applied。 */ + /** 每帧把键盘输入换算成推力与力矩,写入缓存。 */ void ApplyControlForce(); /** - * 物理线程内施力。 - * - * 游戏线程直接写 xfrc_applied 会被物理线程的 mj_step 覆盖掉(两者不同线程、 - * 不同频率),实测按上浮完全推不动。URLab 的 UMjPhysicsEngine 提供了 - * PreStepCallbacks —— 在 mj_step 之前、同一把锁内执行,这是官方给的施力入口。 - * 游戏线程只负责算力并存进 CachedForce*,物理线程在这里读出来写进 MuJoCo。 + * 物理线程内施力:从缓存读出推力/力矩并写入 MuJoCo。 + * 通过 UMjPhysicsEngine 的 PreStepCallbacks 在 mj_step 之前执行, + * 与物理步同锁,避免被 mj_step 覆盖。 */ void ApplyCachedForce(mjModel_* Model, mjData_* Data); @@ -392,12 +315,9 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent std::atomic CachedBodyId{-1}; /** - * 控制指令的时间戳(MuJoCo 仿真时间)。 - * - * 控制力矩在**游戏线程**按帧算,物理在**独立线程**按 500 Hz 跑。 - * 打包版启动时有一段长卡顿(着色器编译),游戏线程几秒不更新, - * 上一帧的力矩会被冻结着一直施加 —— 实测 ROV 在 5 秒内被甩到 159°。 - * 物理线程据此判断指令是否过期,过期就把推力/力矩归零。 + * 控制指令时间戳(MuJoCo 仿真时间)。 + * 指令在游戏线程按帧更新、物理在独立线程按 500 Hz 步进; + * 物理线程据此判断指令是否过期,过期时把推力与力矩归零。 */ std::atomic CachedControlTime{0.0}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp index e76ba058..26a8ab46 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp @@ -14,9 +14,7 @@ ARovPawn::ARovPawn() RootComponent = CreateDefaultSubobject(TEXT("RootComponent")); - // OpenHUTB 追加(2026-11-05):MuJoCo 跟随者桥接 - // 让本 Pawn 跟随 MuJoCo 模型而非 AirSim 自身物理,消除双所有权。 - // 见 docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md + // MuJoCo 位姿跟随桥接 MuJoCoBridge_ = CreateDefaultSubobject(TEXT("MuJoCoBridge")); body_mesh_ = CreateDefaultSubobject(TEXT("BodyMesh")); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h index f82aa6a6..44646ac9 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.h @@ -58,8 +58,7 @@ class AIRSIM_API ARovPawn : public APawn UPROPERTY() class UStaticMeshComponent* body_mesh_; - // OpenHUTB 追加(2026-11-05):MuJoCo 跟随者桥接组件 - // 让本 Pawn 跟随 MuJoCo 的物理状态,而不是 AirSim 自身的 rov_physics_body_。 + // MuJoCo 位姿跟随桥接组件 UPROPERTY() class URovMuJoCoBridge* MuJoCoBridge_; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp index 0b5abe3c..bfd0cca0 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp @@ -73,11 +73,8 @@ void RovPawnSimApi::updateRenderedState(float dt) const CollisionInfo& collision_info = getCollisionInfo(); rov_physics_body_->setCollisionInfo(collision_info); - // OpenHUTB 修改(2026-11-05):外部位姿源优先 - // 见 docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md - // 设置外部位姿源(MuJoCo)后,本类退化为纯跟随者,不再用 AirSim 自身物理体的状态, - // 从而消除「两个求解器同时拥有同一台 ROV 位姿」的双所有权问题。 - // 坐标转换统一走基类的 toPoseFromUnreal(),这里不做任何手工换算。 + // 设置外部位姿源后本类退化为纯跟随者,不再使用 AirSim 物理体的状态; + // 坐标转换统一走基类的 toPoseFromUnreal()。 FVector ExternalPos; FQuat ExternalRot; bool bHasExternalPose = false; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h index 307877df..7b1e20a5 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.h @@ -61,25 +61,10 @@ class RovPawnSimApi : public PawnSimApi void onKeyboardPwm(const std::vector& pwms); - // --------------------------------------------------------------------- - // OpenHUTB 追加(2026-11-05):外部位姿源 - // - // 设计依据:docs/decisions/2026-11-05-ROV物理视觉统一改动方案.md - // - // AirSim 的 rov_physics_body_ 与 MuJoCo 的 AMjArticulation 会同时积分同一台 - // ROV 的位姿,构成「双所有权」。被调研的 16 个开源仿真系统里没有一个允许 - // 这种配置(docs/research/开源仿真器物理与视觉统一架构调研.html §7 第 3 条)。 - // - // 这里把 RovPawnSimApi 降级为「纯跟随者」:设置了外部位姿源后, - // updateRenderedState() 优先用它,不再读 rov_physics_body_。 - // - // provider 返回 true 表示本帧有有效位姿;返回 false 则回退原逻辑 - // (例如 MuJoCo 模型尚未编译完成)。 - // - // 注意:provider 给出的是 **UE 世界坐标**(FVector + FQuat), - // 由基类的 toPoseFromUnreal() 统一转成 AirSim 的 NED Pose, - // 保证坐标转换只在一个地方发生。 - // --------------------------------------------------------------------- + // 外部位姿源。设置后 updateRenderedState() 优先使用它,不再读取 + // rov_physics_body_,避免与外部物理引擎重复积分位姿。 + // provider 返回 false 时回退原有逻辑;坐标为 UE 世界系, + // 由基类 toPoseFromUnreal() 统一转换为 NED。 void setExternalPoseProvider(std::function provider) { external_pose_provider_ = std::move(provider); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h index c10137e0..8d4d5172 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h @@ -1,10 +1,10 @@ // Copyright (c) OpenHUTB. All rights reserved. // -// 从 E:\Projects\mujoco_plugin\mujoco_plugin\src\underwater 移植过来的水下物理模型: -// - FRovOceanCurrent ← ocean_current.py 的 OceanCurrentCascade -// - FRovPIDDepthController ← depth_controller.py 的 PIDDepthController +// ROV 水下物理模型: +// - FRovOceanCurrent 三层级联洋流 +// - FRovPIDDepthController 定深 PID // -// 头文件内联实现,不新增 .cpp,避免改动 AirSim 模块的构建文件。 +// 头文件内联实现,不新增 .cpp。 #pragma once @@ -46,11 +46,8 @@ struct FRovOceanCurrent float TurbIntegralScale = 1.0f; /** - * 洋流速度幅值上限(m/s)。 - * - * 原版 config 里 mu=0,Gauss-Markov 是纯随机游走,没有均值回归, - * 速度幅值会随时间无界漂大(实测跑到几十 m/s,把 ROV 直接推飞)。 - * 这里加一个物理上限:真实洋流很少超过 2 m/s。 + * 洋流速度幅值上限(m/s)。Gauss-Markov 无均值回归时速度会无界增长, + * 这里加物理上限。 */ float MaxSpeed = 2.0f; From 8872520f73ab24758fb9e5f2be24068f18595bde Mon Sep 17 00:00:00 2001 From: shark Date: Tue, 10 Nov 2026 10:07:19 +0800 Subject: [PATCH 6/8] =?UTF-8?q?fix(rov):=20=E8=A1=A5=E5=85=A5=20ROV=20?= =?UTF-8?q?=E6=8E=A5=E5=8F=A3=E5=A4=B4=E6=96=87=E4=BB=B6=EF=BC=8C=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D=E9=94=AE=E7=9B=98=20PWM=20=E6=8E=A5=E5=8F=A3=E7=BC=BA?= =?UTF-8?q?=E5=A4=B1=E5=AF=BC=E8=87=B4=E7=9A=84=E7=BC=96=E8=AF=91=E5=A4=B1?= =?UTF-8?q?=E8=B4=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 现象:在只同步 Vehicles/Rov 目录的工程里编译 RovPawnSimApi.cpp 报 "setMotorPWMs" 不是 "msr::airlib::RovApiBase" 的成员。 原因:RovApiBase.hpp 位于 AirLib 目录,该目录默认不入库; 消费方拿到的副本缺少 setMotorPWMs 包装,而 cpp 里在调用它。 处理:把该头文件纳入版本管理,保证同步后接口完整。 --- .../include/vehicles/rov/api/RovApiBase.hpp | 395 ++++++++++++++++++ 1 file changed, 395 insertions(+) create mode 100644 Unreal/Plugins/AirSim/Source/AirLib/include/vehicles/rov/api/RovApiBase.hpp diff --git a/Unreal/Plugins/AirSim/Source/AirLib/include/vehicles/rov/api/RovApiBase.hpp b/Unreal/Plugins/AirSim/Source/AirLib/include/vehicles/rov/api/RovApiBase.hpp new file mode 100644 index 00000000..591149bd --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirLib/include/vehicles/rov/api/RovApiBase.hpp @@ -0,0 +1,395 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef msr_airlib_DroneVTOLControlServer_hpp +#define msr_airlib_DroneVTOLControlServer_hpp + +#include "common/Common.hpp" +#include "RovCommon.hpp" +#include "safety/SafetyEval.hpp" +#include "physics/Kinematics.hpp" +#include "physics/Environment.hpp" +#include "api/VehicleApiBase.hpp" + +#include +#include +#include + +namespace msr +{ +namespace airlib +{ + + class RovApiBase : public VehicleApiBase + { + + protected: //must be implemented + /************************* low level move APIs *********************************/ + virtual void commandMotorPWMs(const vector& pwm_values) = 0; + virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) = 0; + virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) = 0; + virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) = 0; + virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) = 0; + virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) = 0; + virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) = 0; + virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) = 0; + virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) = 0; + virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) = 0; + + /************************* set Controller Gains APIs *********************************/ + virtual void setControllerGains(uint8_t controllerType, const vector& kp, const vector& ki, const vector& kd) = 0; + + /************************* State APIs *********************************/ + virtual Kinematics::State getKinematicsEstimated() const = 0; + virtual LandedState getLandedState() const = 0; + virtual GeoPoint getGpsLocation() const = 0; + virtual const RovApiParams& getRovApiParams() const = 0; + + /************************* basic config APIs *********************************/ + virtual float getCommandPeriod() const = 0; //time between two command required for drone in seconds + virtual float getTakeoffZ() const = 0; // the height above ground for the drone after successful takeoff (Z above ground is negative due to NED coordinate system). + //noise in difference of two position coordinates. This is not GPS or position accuracy which can be very low such as 1m. + //the difference between two position cancels out transitional errors. Typically this would be 0.1m or lower. + virtual float getDistanceAccuracy() const = 0; + + protected: //optional overrides but recommended, default values may work + virtual float getAutoLookahead(float velocity, float adaptive_lookahead, + float max_factor = 40, float min_factor = 30) const; + virtual float getObsAvoidanceVelocity(float risk_dist, float max_obs_avoidance_vel) const; + + //below methods gets called by default implementations of move-related commands that would use a long + //running loop. These can be used by derived classes to do some init/cleanup. + virtual void beforeTask() + { + //default is do nothing + } + virtual void afterTask() + { + //default is do nothing + } + + public: //optional overrides + virtual void moveByRC(const RCData& rc_data); + + //below method exist for any firmwares that may want to use ground truth for debugging purposes + virtual void setSimulatedGroundTruth(const Kinematics::State* kinematics, const Environment* environment) + { + kinematics_ = kinematics; + environment_ = environment; + } + + virtual void setCollisionInfo(const CollisionInfo& collision_info) + { + collision_info_ = collision_info; + } + + virtual void resetImplementation() override; + + public: //these APIs uses above low level APIs + virtual ~RovApiBase() = default; + + /************************* high level move APIs *********************************/ + //return value of these function is true if command was completed without interruption or timeouts + virtual bool takeoff(float timeout_sec); + virtual bool land(float timeout_sec); + virtual bool goHome(float timeout_sec); + + virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByMotorPWMs(const vector& pwm_values, float duration); + virtual void setMotorPWMs(const vector& pwm_values) + { + commandMotorPWMs(pwm_values); + } + virtual bool moveByRollPitchYawZ(float roll, float pitch, float yaw, float z, float duration); + virtual bool moveByRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle, float duration); + virtual bool moveByRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle, float duration); + virtual bool moveByRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z, float duration); + virtual bool moveByAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle, float duration); + virtual bool moveByVelocity(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveByVelocityZ(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool moveOnPath(const vector& path, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, + const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); + virtual bool moveToZ(float z, float velocity, float timeout_sec, const YawMode& yaw_mode, + float lookahead, float adaptive_lookahead); + virtual bool moveByManual(float vx_max, float vy_max, float z_min, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); + virtual bool rotateToYaw(float yaw, float timeout_sec, float margin); + virtual bool rotateByYawRate(float yaw_rate, float duration); + virtual bool hover(); + virtual RCData estimateRCTrims(float trimduration = 1, float minCountForTrim = 10, float maxTrim = 100); + + /************************* set angle gain APIs *********************************/ + virtual void setAngleLevelControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setAngleRateControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setVelocityControllerGains(const vector& kp, const vector& ki, const vector& kd); + virtual void setPositionControllerGains(const vector& kp, const vector& ki, const vector& kd); + + /************************* Safety APIs *********************************/ + virtual void setSafetyEval(const shared_ptr safety_eval_ptr); + virtual bool setSafety(SafetyEval::SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_startegy, + float obs_avoidance_vel, const Vector3r& origin, float xy_length, float max_z, float min_z); + + /************************* high level status APIs *********************************/ + RotorTiltableStates getRotorStates() const + { + return rotor_states_; + } + + Kinematics::State getKinematicsGroundTruth() const + { + Kinematics::State state; + state.pose.position = kinematics_->pose.position; + state.pose.orientation = kinematics_->pose.orientation; + state.twist.linear = kinematics_->twist.linear; + state.twist.angular = kinematics_->twist.angular; + return state; + } + + RovState getRovState() const + { + RovState state; + state.kinematics_estimated = getKinematicsEstimated(); + state.kinematics_true = getKinematicsGroundTruth(); + //TODO: add GPS health, accuracy in API + state.gps_location = getGpsLocation(); + state.timestamp = clock()->nowNanos(); + state.landed_state = getLandedState(); + state.rc_data = getRCData(); + state.ready = isReady(state.ready_message); + state.can_arm = canArm(); + return state; + } + + /******************* Task management Apis ********************/ + virtual void cancelLastTask() override + { + token_.cancel(); + } + + /******************* rotors' states setter ********************/ + void setRotorStates(const RotorTiltableStates& rotor_states) + { + rotor_states_ = rotor_states; + } + + protected: //utility methods + typedef std::function WaitFunction; + + //*********************************safe wrapper around low level commands*************************************************** + virtual void moveByRollPitchYawZInternal(float roll, float pitch, float yaw, float z); + virtual void moveByRollPitchYawThrottleInternal(float roll, float pitch, float yaw, float throttle); + virtual void moveByRollPitchYawrateThrottleInternal(float roll, float pitch, float yaw_rate, float throttle); + virtual void moveByRollPitchYawrateZInternal(float roll, float pitch, float yaw_rate, float z); + virtual void moveByAngleRatesZInternal(float roll_rate, float pitch_rate, float yaw_rate, float z); + virtual void moveByAngleRatesThrottleInternal(float roll_rate, float pitch_rate, float yaw_rate, float throttle); + virtual void moveByVelocityInternal(float vx, float vy, float vz, const YawMode& yaw_mode); + virtual void moveByVelocityZInternal(float vx, float vy, float z, const YawMode& yaw_mode); + virtual void moveToPositionInternal(const Vector3r& dest, const YawMode& yaw_mode); + + /************* safety checks & emergency maneuvers ************/ + virtual bool emergencyManeuverIfUnsafe(const SafetyEval::EvalResult& result); + virtual bool safetyCheckVelocity(const Vector3r& velocity); + virtual bool safetyCheckVelocityZ(float vx, float vy, float z); + virtual bool safetyCheckDestination(const Vector3r& dest_loc); + + /************* wait helpers ************/ + // helper function can wait for anything (as defined by the given function) up to the max_wait duration (in seconds). + // returns true if the wait function succeeded, or false if timeout occurred or the timeout is invalid. + Waiter waitForFunction(WaitFunction function, float max_wait); + + //useful for derived class to check after takeoff + bool waitForZ(float timeout_sec, float z, float margin); + + /************* other short hands ************/ + virtual Vector3r getPosition() const + { + return getKinematicsEstimated().pose.position; + } + virtual Vector3r getVelocity() const + { + return getKinematicsEstimated().twist.linear; + } + virtual Quaternionr getOrientation() const + { + return getKinematicsEstimated().pose.orientation; + } + + CancelToken& getCancelToken() + { + return token_; + } + + public: //types + class UnsafeMoveException : public VehicleMoveException + { + public: + const SafetyEval::EvalResult result; + + UnsafeMoveException(const SafetyEval::EvalResult result_val, const std::string& message = "") + : VehicleMoveException(message), result(result_val) + { + } + }; + + protected: //types + class SingleCall + { + public: + SingleCall(RovApiBase* api) + : api_(api) + { + auto& token = api->getCancelToken(); + + //if we can't get lock, cancel previous call + if (!token.try_lock()) { + //TODO: should we worry about spurious failures in try_lock? + token.cancel(); + token.lock(); + } + + if (isRootCall()) + token.reset(); + //else this is not the start of the call + } + + virtual ~SingleCall() + { + auto& token = api_->getCancelToken(); + + if (isRootCall()) + token.reset(); + //else this is not the end of the call + + token.unlock(); + } + + protected: + RovApiBase* getVehicleApi() + { + return api_; + } + + bool isRootCall() + { + return api_->getCancelToken().getRecursionCount() == 1; + } + + private: + RovApiBase* api_; + }; + + class SingleTaskCall : public SingleCall + { + public: + SingleTaskCall(RovApiBase* api) + : SingleCall(api) + { + if (isRootCall()) + api->beforeTask(); + } + + virtual ~SingleTaskCall() + { + if (isRootCall()) + getVehicleApi()->afterTask(); + } + }; + + //use this lock for vehicle status APIs + struct StatusLock + { + //this const correctness gymnastic is required because most + //status update APIs are const + StatusLock(const RovApiBase* api) + : lock_( + *const_cast(&api->status_mutex_)) + { + } + + private: + //we need mutable here because status APIs are const and shouldn't change data members + mutable std::lock_guard lock_; + }; + + private: //types + struct PathPosition + { + uint seg_index; + float offset; + Vector3r position; + }; + + struct PathSegment + { + Vector3r seg_normalized; + Vector3r seg; + float seg_length; + float seg_velocity; + float start_z; + float seg_path_length; + + PathSegment(const Vector3r& start, const Vector3r& end, float velocity, float path_length) + { + seg = end - start; + seg_length = seg.norm(); + seg_normalized = seg.normalized(); + start_z = start.z(); + seg_path_length = path_length; + + seg_velocity = velocity; + } + }; + + //RAII + class ObsStrategyChanger + { + private: + shared_ptr safety_eval_ptr_; + SafetyEval::ObsAvoidanceStrategy old_strategy_; + + public: + ObsStrategyChanger(shared_ptr safety_eval_ptr, SafetyEval::ObsAvoidanceStrategy new_startegy) + { + safety_eval_ptr_ = safety_eval_ptr; + old_strategy_ = safety_eval_ptr_->getObsAvoidanceStrategy(); + safety_eval_ptr_->setObsAvoidanceStrategy(new_startegy); + } + ~ObsStrategyChanger() + { + safety_eval_ptr_->setObsAvoidanceStrategy(old_strategy_); + } + }; + + private: //methods + float setNextPathPosition(const vector& path, const vector& path_segs, + const PathPosition& cur_path_loc, float next_dist, PathPosition& next_path_loc); + void adjustYaw(const Vector3r& heading, DrivetrainType drivetrain, YawMode& yaw_mode); + void adjustYaw(float x, float y, DrivetrainType drivetrain, YawMode& yaw_mode); + void moveToPathPosition(const Vector3r& dest, float velocity, DrivetrainType drivetrain, /* pass by value */ YawMode yaw_mode, float last_z); + bool isYawWithinMargin(float yaw_target, float margin) const; + + private: //variables + CancelToken token_; + std::recursive_mutex status_mutex_; + RCData rc_data_trims_; + shared_ptr safety_eval_ptr_; + float obs_avoidance_vel_ = 0.5f; + + //TODO: make this configurable? + float landing_vel_ = 0.2f; //velocity to use for landing + float approx_zero_vel_ = 0.05f; + float approx_zero_angular_vel_ = 0.01f; + RotorTiltableStates rotor_states_; + + CollisionInfo collision_info_; + const Kinematics::State* kinematics_; + const Environment* environment_; + }; +} +} //namespace +#endif From 30ebffb4e78df6d03937041d4ae1f131836df66e Mon Sep 17 00:00:00 2001 From: shark Date: Tue, 10 Nov 2026 16:30:59 +0800 Subject: [PATCH 7/8] =?UTF-8?q?feat(rov):=20=E5=90=8C=E6=AD=A5=20ROV=20?= =?UTF-8?q?=E6=9C=80=E6=96=B0=E6=BA=90=E7=A0=81=EF=BC=8C=E4=BF=AE=E5=A4=8D?= =?UTF-8?q?=E5=A4=B4=E6=96=87=E4=BB=B6=E4=B8=8E=E6=96=B0=E5=A2=9E=E6=BA=90?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E4=B8=8D=E5=8C=B9=E9=85=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 现象:同步后编译 RovStatusHUD.cpp 报 FRovStatusSnapshot 未定义、 GetStatusSnapshot 不是 URovMuJoCoBridge 的成员。 原因:状态面板、外形参数、障碍场景等改动只更新了 hutb 侧的 AirSim 源码, air 侧仍是旧版;两边同步时头文件被覆盖回旧版,与新的 cpp 不匹配。 处理:把已验证的 17 个 ROV 源文件与 AirSim.Build.cs(新增 Json 依赖) 同步到 air,保证任一侧同步后都能编译。 --- Unreal/Plugins/AirSim/Source/AirSim.Build.cs | 3 + .../Source/Vehicles/Rov/RovFluidParams.cpp | 135 +++++ .../Source/Vehicles/Rov/RovFluidParams.h | 79 +++ .../Source/Vehicles/Rov/RovMuJoCoBridge.cpp | 518 ++++++++++++++++-- .../Source/Vehicles/Rov/RovMuJoCoBridge.h | 139 +++++ .../Source/Vehicles/Rov/RovObstacleField.cpp | 138 +++++ .../Source/Vehicles/Rov/RovObstacleField.h | 81 +++ .../Source/Vehicles/Rov/RovStatusHUD.cpp | 240 ++++++++ .../AirSim/Source/Vehicles/Rov/RovStatusHUD.h | 83 +++ .../Vehicles/Rov/RovUnderwaterPhysics.h | 6 +- .../Source/Vehicles/Rov/SimModeWorldRov.cpp | 32 ++ .../Source/Vehicles/Rov/SimModeWorldRov.h | 8 + 12 files changed, 1409 insertions(+), 53 deletions(-) create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp create mode 100644 Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.h diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 46f8eb2a..2e94b006 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -86,6 +86,9 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) PublicDependencyModuleNames.Add("URLab"); PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "Foliage" }); + // OpenHUTB 追加:ROV 配置文件(RovParams.json)解析需要 Json 模块。 + PrivateDependencyModuleNames.Add("Json"); + string ProjectDir = Target.ProjectFile != null ? Path.GetDirectoryName(Target.ProjectFile.FullName) : ""; string CarlaPluginInProject = Path.Combine(ProjectDir, "Plugins", "Carla"); string CarlaPluginSibling = Path.Combine(AirSimPluginPath, "..", "Carla"); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp new file mode 100644 index 00000000..abdd8ac3 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp @@ -0,0 +1,135 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "Vehicles/Rov/RovFluidParams.h" + +#include "Dom/JsonObject.h" +#include "Dom/JsonValue.h" +#include "Misc/FileHelper.h" +#include "Serialization/JsonReader.h" +#include "Serialization/JsonSerializer.h" + +namespace +{ + struct FShapeParams + { + float Cd; + float AddedMassCoeff; // 相对 ρ·V 的附加质量系数 + }; + + FShapeParams ShapeTable(ERovHullShape Shape) + { + switch (Shape) + { + case ERovHullShape::Sphere: return { 0.47f, 0.50f }; + case ERovHullShape::Box: return { 1.05f, 0.70f }; + case ERovHullShape::Cylinder: return { 0.82f, 0.60f }; + case ERovHullShape::Streamlined: return { 0.08f, 0.10f }; + default: return { 0.50f, 0.50f }; + } + } +} + +FRovFluidEstimate URovFluidLibrary::EstimateFluidParams(ERovHullShape Shape, FVector Dims, float Density) +{ + FRovFluidEstimate Out; + const FShapeParams P = ShapeTable(Shape); + Out.Cd = P.Cd; + + const float X = FMath::Max(Dims.X, KINDA_SMALL_NUMBER); + const float Y = FMath::Max(Dims.Y, KINDA_SMALL_NUMBER); + const float Z = FMath::Max(Dims.Z, KINDA_SMALL_NUMBER); + + switch (Shape) + { + case ERovHullShape::Sphere: + Out.Volume = 4.0f / 3.0f * PI * X * X * X; + Out.RefArea = PI * X * X; + break; + case ERovHullShape::Box: + Out.Volume = 8.0f * X * Y * Z; + Out.RefArea = 4.0f * Y * Z; // 沿 X 迎流 + break; + case ERovHullShape::Cylinder: + Out.Volume = PI * X * X * 2.0f * Y; + Out.RefArea = PI * X * X; + break; + case ERovHullShape::Streamlined: + Out.Volume = 4.0f / 3.0f * PI * X * Y * Z; + Out.RefArea = PI * Y * Z; // 沿长轴 X 迎流 + break; + default: + break; + } + + Out.AddedMass = P.AddedMassCoeff * Density * Out.Volume; + return Out; +} + +float URovFluidLibrary::EstimateDragForce(ERovHullShape Shape, FVector Dims, float Speed, float Density) +{ + const FRovFluidEstimate E = EstimateFluidParams(Shape, Dims, Density); + return 0.5f * Density * E.Cd * E.RefArea * Speed * Speed; +} + +bool RovConfig::LoadKeyValues(const FString& Path, TMap& OutValues, FString& OutError) +{ + OutValues.Reset(); + FString Raw; + if (!FFileHelper::LoadFileToString(Raw, *Path)) + { + OutError = FString::Printf(TEXT("读取失败:%s"), *Path); + return false; + } + + TSharedPtr Root; + const TSharedRef> Reader = TJsonReaderFactory<>::Create(Raw); + if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) + { + OutError = FString::Printf(TEXT("JSON 解析失败:%s"), *Path); + return false; + } + + for (const TPair>& Pair : Root->Values) + { + if (!Pair.Value.IsValid() || Pair.Value->Type != EJson::Number) + { + continue; + } + OutValues.Add(Pair.Key, static_cast(Pair.Value->AsNumber())); + } + return true; +} + +bool RovConfig::LoadNumberArray(const FString& Path, const FString& Field, TArray& OutValues, FString& OutError) +{ + OutValues.Reset(); + FString Raw; + if (!FFileHelper::LoadFileToString(Raw, *Path)) + { + OutError = FString::Printf(TEXT("读取失败:%s"), *Path); + return false; + } + + TSharedPtr Root; + const TSharedRef> Reader = TJsonReaderFactory<>::Create(Raw); + if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) + { + OutError = FString::Printf(TEXT("JSON 解析失败:%s"), *Path); + return false; + } + + const TArray>* Arr = nullptr; + if (!Root->TryGetArrayField(Field, Arr) || !Arr) + { + OutError = FString::Printf(TEXT("缺少数组字段 %s"), *Field); + return false; + } + for (const TSharedPtr& V : *Arr) + { + if (V.IsValid() && V->Type == EJson::Number) + { + OutValues.Add(static_cast(V->AsNumber())); + } + } + return true; +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h new file mode 100644 index 00000000..fbcb12c3 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h @@ -0,0 +1,79 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 流体参数工具:外形 → 阻力系数估算、配置文件读取。 + +#pragma once + +#include "CoreMinimal.h" +#include "Kismet/BlueprintFunctionLibrary.h" +#include "RovFluidParams.generated.h" + +/** 典型外形。 */ +UENUM(BlueprintType) +enum class ERovHullShape : uint8 +{ + Sphere UMETA(DisplayName = "球体"), + Box UMETA(DisplayName = "立方体/长方体"), + Cylinder UMETA(DisplayName = "圆柱"), + Streamlined UMETA(DisplayName = "流线型"), +}; + +/** 由外形估算出的流体受力参数。 */ +USTRUCT(BlueprintType) +struct FRovFluidEstimate +{ + GENERATED_BODY() + + /** 迎流阻力系数 Cd。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float Cd = 0.5f; + + /** 迎流参考面积(m²)。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float RefArea = 0.0f; + + /** 附加质量(kg)。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float AddedMass = 0.0f; + + /** 排水体积(m³)。 */ + UPROPERTY(BlueprintReadOnly, Category = "ROV|Fluid") + float Volume = 0.0f; +}; + +/** + * 外形到流体参数的估算。 + * + * 尺寸约定(单位 m,均为半尺寸): + * 球体 X = 半径 + * 立方体 X/Y/Z = 三个方向的半边长 + * 圆柱 X = 半径,Y = 半长(轴向迎流) + * 流线型 X/Y/Z = 椭球三个半轴,X 为长轴(长轴迎流) + */ +UCLASS() +class AIRSIM_API URovFluidLibrary : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + +public: + /** 由外形与尺寸估算 Cd、迎流面积、附加质量。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|Fluid") + static FRovFluidEstimate EstimateFluidParams(ERovHullShape Shape, FVector Dims, float Density = 1000.0f); + + /** 按估算的 Cd 计算匀速迎流阻力 F = 0.5·ρ·Cd·A·v²(N)。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|Fluid") + static float EstimateDragForce(ERovHullShape Shape, FVector Dims, float Speed, float Density = 1000.0f); +}; + +/** 配置文件读取。 */ +namespace RovConfig +{ + /** + * 读取扁平 JSON 配置文件(键为字符串,值为数值)。 + * 数组与嵌套对象会被忽略,由调用方单独解析。 + */ + bool LoadKeyValues(const FString& Path, TMap& OutValues, FString& OutError); + + /** 读取配置文件里的数值数组字段(如 "Obstacles" 之外的一维数组)。 */ + bool LoadNumberArray(const FString& Path, const FString& Field, TArray& OutValues, FString& OutError); +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp index 96c2b148..159321b0 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp @@ -9,6 +9,9 @@ #include "Components/PrimitiveComponent.h" #include "Kismet/GameplayStatics.h" #include "InputCoreTypes.h" +#include "HAL/FileManager.h" +#include "Misc/FileHelper.h" +#include "Misc/Paths.h" // URLab:MuJoCo 物理 #include "MuJoCo/Core/AMjManager.h" @@ -87,6 +90,12 @@ void URovMuJoCoBridge::BeginPlay() void URovMuJoCoBridge::EndPlay(const EEndPlayReason::Type EndPlayReason) { + if (ContactLogArchive) + { + ContactLogArchive->Close(); + delete ContactLogArchive; + ContactLogArchive = nullptr; + } if (bBridgeActive) { ExternalPoseProvider = nullptr; bBridgeActive = false; @@ -258,6 +267,189 @@ void URovMuJoCoBridge::TickComponent(float DeltaTime, ELevelTick TickType, } } +void URovMuJoCoBridge::ApplyConfigFile() +{ + if (!bLoadConfigFile) + { + UE_LOG(LogRovBridge, Log, TEXT("[配置] bLoadConfigFile=false,跳过配置文件")); + return; + } + + FString Path; + if (!FParse::Value(FCommandLine::Get(), TEXT("RovConfig="), Path)) + { + Path = FPaths::Combine(FPaths::ProjectConfigDir(), TEXT("RovParams.json")); + } + Path = FPaths::ConvertRelativePathToFull(Path); + if (!FPaths::FileExists(Path)) + { + UE_LOG(LogRovBridge, Warning, TEXT("[配置] 未找到配置文件 %s,使用类默认值 + 命令行覆盖"), *Path); + return; + } + + TMap Kv; + FString Err; + if (!RovConfig::LoadKeyValues(Path, Kv, Err)) + { + UE_LOG(LogRovBridge, Warning, TEXT("[配置] %s"), *Err); + return; + } + + auto ApplyF = [&Kv](const TCHAR* Key, float& Target) + { + if (const float* V = Kv.Find(Key)) { Target = *V; } + }; + auto ApplyB = [&Kv](const TCHAR* Key, bool& Target) + { + if (const float* V = Kv.Find(Key)) { Target = (*V) != 0.0f; } + }; + auto ApplyV = [&Kv](const TCHAR* Prefix, FVector& Target) + { + const FString P(Prefix); + if (const float* X = Kv.Find(P + TEXT("X"))) { Target.X = *X; } + if (const float* Y = Kv.Find(P + TEXT("Y"))) { Target.Y = *Y; } + if (const float* Z = Kv.Find(P + TEXT("Z"))) { Target.Z = *Z; } + }; + + ApplyF(TEXT("FluidDensity"), FluidDensity); + ApplyF(TEXT("FluidViscosity"), FluidViscosity); + ApplyF(TEXT("DisplacedVolume"), DisplacedVolume); + ApplyF(TEXT("BuoyancyScale"), BuoyancyScale); + ApplyF(TEXT("BuoyancyArm"), BuoyancyArm); + ApplyF(TEXT("ArmBuoyancyVolume"), ArmBuoyancyVolume); + ApplyF(TEXT("ArmBuoyancyScale"), ArmBuoyancyScale); + ApplyF(TEXT("ArmJointDamping"), ArmJointDamping); + ApplyF(TEXT("ArmJointStiffness"), ArmJointStiffness); + ApplyB(TEXT("ArmGravityComp"), bArmGravityComp); + ApplyF(TEXT("ThrusterMaxRatio"), ThrusterMaxRatio); + ApplyF(TEXT("WaterSurfaceZ"), WaterSurfaceZ); + ApplyF(TEXT("AngularDofDamping"), AngularDofDamping); + ApplyF(TEXT("AngularDampingCoeff"), AngularDampingCoeff); + ApplyF(TEXT("AttitudeBandwidth"), AttitudeBandwidth); + ApplyF(TEXT("AttitudeZeta"), AttitudeZeta); + ApplyF(TEXT("AttitudeIntegralGain"), AttitudeIntegralGain); + ApplyF(TEXT("LevelBandwidth"), LevelBandwidth); + ApplyF(TEXT("LevelZeta"), LevelZeta); + ApplyF(TEXT("YawBandwidth"), YawBandwidth); + ApplyF(TEXT("YawZeta"), YawZeta); + ApplyF(TEXT("YawRateDegPerSec"), YawRateDegPerSec); + ApplyF(TEXT("CurrentDragCoeff"), CurrentDragCoeff); + ApplyF(TEXT("OceanCurrentMeanSpeed"), OceanCurrentMeanSpeed); + ApplyB(TEXT("EnableAttitudeHold"), bEnableAttitudeHold); + ApplyB(TEXT("EnableDepthHold"), bEnableDepthHold); + ApplyB(TEXT("EnableOceanCurrent"), bEnableOceanCurrent); + ApplyB(TEXT("UseNativeFluid"), bUseNativeFluid); + + ApplyB(TEXT("UseExplicitDrag"), bUseExplicitDrag); + ApplyV(TEXT("DragCoeff"), DragCoeff); + ApplyV(TEXT("DragRefArea"), DragRefArea); + ApplyB(TEXT("UseAddedMass"), bUseAddedMass); + ApplyV(TEXT("AddedMass"), AddedMass); + + ApplyB(TEXT("AutoEstimateFluidParams"), bAutoEstimateFluidParams); + ApplyV(TEXT("HullDims"), HullDims); + if (const float* S = Kv.Find(TEXT("HullShape"))) + { + HullShape = static_cast(FMath::Clamp(FMath::RoundToInt(*S), 0, 3)); + } + if (bAutoEstimateFluidParams) + { + const FRovFluidEstimate E = URovFluidLibrary::EstimateFluidParams(HullShape, HullDims, FluidDensity); + DragCoeff = FVector(E.Cd, E.Cd, E.Cd); + DragRefArea = FVector(E.RefArea, E.RefArea, E.RefArea); + AddedMass = FVector(E.AddedMass, E.AddedMass, E.AddedMass); + bUseExplicitDrag = true; + bUseAddedMass = true; + UE_LOG(LogRovBridge, Log, + TEXT("[配置] 外形估算:Cd=%.2f 迎流面积=%.4f m² 附加质量=%.1f kg 排水体积=%.4f m³"), + E.Cd, E.RefArea, E.AddedMass, E.Volume); + } + + ApplyB(TEXT("LogContacts"), bLogContacts); + if (const float* V = Kv.Find(TEXT("ContactLogInterval"))) + { + ContactLogInterval = FMath::Max(1, FMath::RoundToInt(*V)); + } + + LoadedConfigPath = Path; + UE_LOG(LogRovBridge, Log, TEXT("[配置] 已加载 %s,共 %d 个数值项"), *Path, Kv.Num()); +} + +void URovMuJoCoBridge::LogContacts(mjModel_* Model, mjData_* Data) +{ + if (!bLogContacts || !Model || !Data) + { + return; + } + if (++ContactLogStep % FMath::Max(1, ContactLogInterval) != 0) + { + return; + } + + if (!ContactLogArchive) + { + FString Path = ContactLogPath.IsEmpty() + ? FPaths::Combine(FPaths::ProjectSavedDir(), TEXT("RovContactLog.csv")) + : ContactLogPath; + Path = FPaths::ConvertRelativePathToFull(Path); + ContactLogArchive = IFileManager::Get().CreateFileWriter(*Path, FILEWRITE_Append); + if (!ContactLogArchive) + { + UE_LOG(LogRovBridge, Warning, TEXT("[接触日志] 无法写入 %s"), *Path); + bLogContacts = false; + return; + } + UE_LOG(LogRovBridge, Log, TEXT("[接触日志] 写入 %s(每 %d 个物理步一条)"), *Path, ContactLogInterval); + } + + auto WriteLine = [this](const FString& Line) + { + FTCHARToUTF8 Utf8(*Line); + ContactLogArchive->Serialize(static_cast(const_cast(Utf8.Get())), Utf8.Length()); + }; + + if (!bContactLogHeaderWritten) + { + WriteLine(TEXT("time_s,body1,body2,normal_N,pos_x,pos_y,pos_z\n")); + bContactLogHeaderWritten = true; + } + + for (int32 i = 0; i < Data->ncon; ++i) + { + const mjContact& C = Data->contact[i]; + mjtNum F6[6] = { 0, 0, 0, 0, 0, 0 }; + mj_contactForce(Model, Data, i, F6); + + const int32 B1 = Model->geom_bodyid[C.geom1]; + const int32 B2 = Model->geom_bodyid[C.geom2]; + const FString N1 = (B1 >= 0 && Model->name_bodyadr[B1] >= 0) + ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B1])) : FString(TEXT("world")); + const FString N2 = (B2 >= 0 && Model->name_bodyadr[B2] >= 0) + ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B2])) : FString(TEXT("world")); + + WriteLine(FString::Printf(TEXT("%.5f,%s,%s,%.4f,%.5f,%.5f,%.5f\n"), + Data->time, *N1, *N2, F6[0], C.pos[0], C.pos[1], C.pos[2])); + } + + // 立即落盘,避免进程被强杀时缓冲区内容丢失 + ContactLogArchive->Flush(); +} + +void URovMuJoCoBridge::GetStatusSnapshot(FRovStatusSnapshot& Out) const +{ + Out.RollDeg = (float)CachedRollDeg.load(); + Out.PitchDeg = (float)CachedPitchDeg.load(); + Out.YawDeg = (float)CachedYawDeg.load(); + Out.DepthM = (float)CachedDepthM.load(); + Out.ThrustN = (float)CachedThrustN.load(); + Out.ContactForceN = (float)CachedContactForce.load(); + Out.VelocityMps = FVector((float)CachedVelocityX.load(), (float)CachedVelocityY.load(), (float)CachedVelocityZ.load()); + Out.ContactPoint = FVector((float)CachedContactPointX.load(), (float)CachedContactPointY.load(), (float)CachedContactPointZ.load()); + Out.ContactForceDir = FVector((float)CachedContactDirX.load(), (float)CachedContactDirY.load(), (float)CachedContactDirZ.load()); + Out.FlowVelocityMps = FVector((float)CachedFlowX.load(), (float)CachedFlowY.load(), (float)CachedFlowZ.load()); + Out.bValid = bBridgeActive && TargetBody.IsValid(); +} + void URovMuJoCoBridge::ApplyControlForce() { if (!TargetBody.IsValid() || !TargetBody->IsBound()) { @@ -292,6 +484,8 @@ void URovMuJoCoBridge::ApplyControlForce() float V = Default; return FParse::Value(FCommandLine::Get(), Key, V) ? V : Default; }; + // 配置文件先加载,命令行随后覆盖,保证「命令行 > 配置文件 > 默认值」 + ApplyConfigFile(); DisplacedVolume = FloatParam(TEXT("RovDisplacedVolume="), DisplacedVolume); BuoyancyScale = FloatParam(TEXT("RovBuoyancyScale="), BuoyancyScale); BuoyancyArm = FloatParam(TEXT("RovBuoyancyArm="), BuoyancyArm); @@ -302,6 +496,13 @@ void URovMuJoCoBridge::ApplyControlForce() ThrusterMaxRatio = FloatParam(TEXT("RovThrustRatio="), ThrusterMaxRatio); WaterSurfaceZ = FloatParam(TEXT("RovWaterSurfaceZ="), WaterSurfaceZ); if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { bEnableAttitudeHold = false; } + if (FParse::Param(FCommandLine::Get(), TEXT("RovContactTest"))) + { + ContactTestForce = 900.0; + bLogContacts = true; + bEnableDepthHold = false; + UE_LOG(LogRovBridge, Log, TEXT("[接触试验] 施加 %.0f N 下压力,用于产生海床接触"), ContactTestForce); + } UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][调参] 排水体积=%.4f m³ 浮力倍率=%.2f 臂排水=%.2f L×%.2f " "推力比=%.2f 水面Z=%.2f m"), @@ -637,6 +838,9 @@ void URovMuJoCoBridge::ApplyControlForce() } } + // 航向角速度前馈交给物理线程的姿态环 + CachedYawFFRadPerSec.store(FMath::DegreesToRadians(YawCmdRateDeg)); + // 【2】洋流:三层级联求当前流速 const FVector VCurrent = OceanCurrent.GetVelocity(RovPos, (float)FrameDt); @@ -1005,11 +1209,12 @@ void URovMuJoCoBridge::ApplyControlForce() } } - // 【10】级联姿态控制 + // 【10】级联姿态控制(已迁移到物理线程,见 ComputeAttitudeTorque) + // 保留本块仅作参考:它按渲染帧调用,帧率低于约 25 fps 时会与积分项形成 + // 慢振荡直至翻转,因此改为在物理线程按 500 Hz 运行。 // 外环:姿态误差 -> 期望角速度(机体系) // 内环:期望角速度 - 实测角速度(qvel[3:6] 为机体系)-> 力矩 - // 力矩直接写 qfrc_applied[3:5](机体系) - if (bEnableAttitudeHold && !bSpinTest && Model->body_jntnum[BodyId] >= 1) { + if (false && bEnableAttitudeHold && !bSpinTest && Model->body_jntnum[BodyId] >= 1) { const int32 NJ = Model->body_jntadr[BodyId]; if (Model->jnt_type[NJ] == mjJNT_FREE) { const int32 ND = Model->jnt_dofadr[NJ]; @@ -1177,6 +1382,116 @@ void URovMuJoCoBridge::ApplyControlForce() } } +void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int32 BodyId, double Dt, double OutTorque[3]) +{ + OutTorque[0] = 0.0; + OutTorque[1] = 0.0; + OutTorque[2] = 0.0; + + if (!bEnableAttitudeHold || !Model || !Data || BodyId < 0 || BodyId >= Model->nbody) + { + return; + } + if (Model->body_jntnum[BodyId] < 1) + { + return; + } + const int32 J = Model->body_jntadr[BodyId]; + if (Model->jnt_type[J] != mjJNT_FREE) + { + return; + } + const int32 D = Model->jnt_dofadr[J]; + + const double* QQ = &Data->xquat[4 * BodyId]; + const double qw = QQ[0], qx = QQ[1], qy = QQ[2], qz = QQ[3]; + + if (!bAttitudeInitialized) + { + bAttitudeInitialized = true; + const double InitYaw = FMath::Atan2(2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz)); + TargetYawDeg = (float)FMath::RadiansToDegrees(InitYaw); + UE_LOG(LogRovBridge, Log, + TEXT("[RovBridge][姿态] 物理线程级联控制启用(%.0f Hz),锁定航向 %.1f°"), + 1.0 / FMath::Max(Dt, 1e-6), TargetYawDeg); + } + + const double HalfYaw = FMath::DegreesToRadians((double)TargetYawDeg) * 0.5; + const double Qtw = FMath::Cos(HalfYaw), Qtz = FMath::Sin(HalfYaw); + + // q_e = conj(q_c) ⊗ q_t(机体系),取短弧 + const double aw = qw, ax = -qx, ay = -qy, az = -qz; + double Qew = aw * Qtw - az * Qtz; + double Qex = ax * Qtw + ay * Qtz; + double Qey = -ax * Qtz + ay * Qtw; + double Qez = aw * Qtz + az * Qtw; + if (Qew < 0.0) { Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; } + + const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; + const double KpAtt[3] = { 1.5, 1.5, 1.0 }; + const double Wmax[3] = { 0.8, 0.8, 0.6 }; + const double Wn = 5.0; + + const double YawFF = CachedYawFFRadPerSec.load(); + const double WmaxZ = (YawFF != 0.0) ? FMath::Max(Wmax[2], FMath::Abs(YawFF) + 0.2) : Wmax[2]; + + for (int32 Axis = 0; Axis < 3; ++Axis) + { + const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] + : Model->body_inertia[3 * BodyId + Axis]; + const double Wlim = (Axis == 2) ? WmaxZ : Wmax[Axis]; + + // 大角度时临时抬高角速度上限,给回正留出力矩余量。 + // 稳态扰动力矩约 86 N·m;若上限恒为 0.8 rad/s,最大回正力矩只有约 86 N·m, + // 一旦翻过 90° 就会停在「控制器力矩 = 重力矩」的平衡点上,再也回不来。 + const double Boost = FMath::Clamp((FMath::Abs(E3[Axis]) - 0.30) * 1.5, 0.0, 2.0); + const double WlimEff = Wlim + Boost; + + // 积分限幅 + 抗饱和:角速度大时不再累积,并按时间常数快速泄放。 + // 只按角速度判据:若再叠加「误差小才积分」,稳态大倾角就永远消不掉 + // (实测会停在 28° 下不来)。 + const double Omega = (double)Data->qvel[D + 3 + Axis]; + if (FMath::Abs(Omega) < 0.30) + { + AttIntState[Axis] = FMath::Clamp(AttIntState[Axis] + E3[Axis] * Dt, -2.0, 2.0); + } + else + { + AttIntState[Axis] *= FMath::Max(0.0, 1.0 - 3.0 * Dt); + } + + double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis] + + (double)AttitudeIntegralGain * AttIntState[Axis], + -WlimEff, WlimEff); + if (Axis == 2 && YawFF != 0.0) + { + Wsp = FMath::Clamp(Wsp + YawFF, -WlimEff, WlimEff); + } + double T = I * Wn * (Wsp - Omega); + OutTorque[Axis] = FMath::Clamp(T, -1500.0, 1500.0); + } + + // 1 Hz 诊断:倾角、误差、角速度、力矩、积分量 + { + static double LastLogT = -1.0; + if (Data->time - LastLogT > 1.0) + { + LastLogT = Data->time; + const double UpZ = FMath::Clamp((double)Data->xmat[9 * BodyId + 8], -1.0, 1.0); + const double TiltDeg = FMath::RadiansToDegrees(FMath::Acos(UpZ)); + UE_LOG(LogRovBridge, Log, + TEXT("[RovBridge][姿态环] t=%.1f 倾角=%.2f E3=(%+.3f,%+.3f,%+.3f) " + "w=(%+.3f,%+.3f,%+.3f) T=(%+.1f,%+.1f,%+.1f) 积分=(%+.2f,%+.2f,%+.2f)"), + Data->time, TiltDeg, + E3[0], E3[1], E3[2], + Data->qvel[D + 3], Data->qvel[D + 4], Data->qvel[D + 5], + OutTorque[0], OutTorque[1], OutTorque[2], + AttIntState[0], AttIntState[1], AttIntState[2]); + } + } +} + void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) { const int32 BodyId = CachedBodyId.load(); @@ -1195,6 +1510,9 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) } const int32 DofAdr = Model->jnt_dofadr[JntId]; + // 逐接触点日志(4.2.4):此处读到的是上一步求解出的接触 + LogContacts(Model, Data); + // mj_applyFT 是累加语义,先清零本 body 的 6 维广义力 for (int32 k = 0; k < 6; ++k) { Data->qfrc_applied[DofAdr + k] = 0.0; @@ -1202,9 +1520,11 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) // 指令新鲜度:控制量在游戏线程按帧更新,物理线程按 500 Hz 步进。 // 游戏线程卡顿时旧指令会被持续施加,这里按仿真时间做淡出: - // 超过 0.1 s 开始衰减,超过 0.2 s 归零。 + // 超过 0.04 s 开始衰减,超过 0.08 s 完全交给物理线程的兜底控制器。 + // 阈值取得比 0.1 s 更紧,是因为帧率低于约 25 fps 时游戏线程的姿态环 + // 已经跟不上(每帧只更新一次),会与积分项形成慢振荡直至翻转。 const double CmdAge = (double)Data->time - CachedControlTime.load(); - const double Fresh = FMath::Clamp(1.0 - (CmdAge - 0.10) / 0.10, 0.0, 1.0); + const double Fresh = FMath::Clamp(1.0 - (CmdAge - 0.04) / 0.04, 0.0, 1.0); const mjtNum CG[3] = { Data->xipos[3 * BodyId + 0], @@ -1218,10 +1538,53 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) mjtNum F1[3] = { (mjtNum)(CachedForceX.load() * Fresh), (mjtNum)(CachedForceY.load() * Fresh), - (mjtNum)(CachedForceZ.load() * Fresh) + (mjtNum)(CachedForceZ.load() * Fresh - ContactTestForce) }; mj_applyFT(Model, Data, F1, ZeroT, CG, BodyId, Data->qfrc_applied); + // 【1b】显式二次阻力:沿机体系三轴 F = -0.5·ρ·Cd·A·|v|·v。 + // 用于补偿椭球近似对大钝体 / 细长体的偏差,默认关闭。 + if (bUseExplicitDrag) + { + const double QW = Data->xquat[4 * BodyId + 0]; + const double QX = Data->xquat[4 * BodyId + 1]; + const double QY = Data->xquat[4 * BodyId + 2]; + const double QZ = Data->xquat[4 * BodyId + 3]; + double Bx = 0.0, By = 0.0, Bz = 0.0; + RotByQuat(QW, -QX, -QY, -QZ, + Data->qvel[DofAdr + 0], Data->qvel[DofAdr + 1], Data->qvel[DofAdr + 2], + Bx, By, Bz); + const double Half = 0.5 * (double)FluidDensity; + const double Fbx = -Half * (double)DragCoeff.X * (double)DragRefArea.X * FMath::Abs(Bx) * Bx; + const double Fby = -Half * (double)DragCoeff.Y * (double)DragRefArea.Y * FMath::Abs(By) * By; + const double Fbz = -Half * (double)DragCoeff.Z * (double)DragRefArea.Z * FMath::Abs(Bz) * Bz; + double Fwx = 0.0, Fwy = 0.0, Fwz = 0.0; + RotByQuat(QW, QX, QY, QZ, Fbx, Fby, Fbz, Fwx, Fwy, Fwz); + mjtNum F1b[3] = { (mjtNum)Fwx, (mjtNum)Fwy, (mjtNum)Fwz }; + mj_applyFT(Model, Data, F1b, ZeroT, CG, BodyId, Data->qfrc_applied); + } + + // 【1c】附加质量:F = -m_A·a(机体系三轴),默认关闭。 + // 椭球模型内部已含附加质量,本项用于单独调节而不改模型。 + if (bUseAddedMass) + { + const double QW = Data->xquat[4 * BodyId + 0]; + const double QX = Data->xquat[4 * BodyId + 1]; + const double QY = Data->xquat[4 * BodyId + 2]; + const double QZ = Data->xquat[4 * BodyId + 3]; + double Ax = 0.0, Ay = 0.0, Az = 0.0; + RotByQuat(QW, -QX, -QY, -QZ, + Data->qacc[DofAdr + 0], Data->qacc[DofAdr + 1], Data->qacc[DofAdr + 2], + Ax, Ay, Az); + const double Fbx = -(double)AddedMass.X * Ax; + const double Fby = -(double)AddedMass.Y * Ay; + const double Fbz = -(double)AddedMass.Z * Az; + double Fwx = 0.0, Fwy = 0.0, Fwz = 0.0; + RotByQuat(QW, QX, QY, QZ, Fbx, Fby, Fbz, Fwx, Fwy, Fwz); + mjtNum F1c[3] = { (mjtNum)Fwx, (mjtNum)Fwy, (mjtNum)Fwz }; + mj_applyFT(Model, Data, F1c, ZeroT, CG, BodyId, Data->qfrc_applied); + } + // 【2】浮力:作用在整机浮心 = 整机合成质心 + R·(0,0,BG)。 // 排水体积属于整个载体,浮心应跟随整机质量分布,而非艇体自身质心。 const double Buoy = CachedBuoyancy.load(); @@ -1300,48 +1663,25 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) } } - // 【3】姿态力矩:free joint 的 qfrc_applied[3..5] 就是机体系力矩,直接赋值 + // 【3】姿态力矩:free joint 的 qfrc_applied[3..5] 就是机体系力矩,直接赋值。 + // 姿态环已移到物理线程(ComputeAttitudeTorque,500 Hz),这里只叠加 + // 游戏线程给的配平力矩与角阻尼,按 Fresh 淡出。 double Tx = CachedTorqueX.load() * Fresh; double Ty = CachedTorqueY.load() * Fresh; double Tz = CachedTorqueZ.load() * Fresh; - // 【3b】物理线程兜底姿态控制 - // 游戏线程卡顿时【10】不会执行,ROV 处于无控状态。这里用同样的级联律 - // 按 (1-Fresh) 权重接管,卡顿结束后交回游戏线程。 - if (Fresh < 1.0 && bEnableAttitudeHold && Model->body_jntnum[BodyId] >= 1) { - const int32 FJ = Model->body_jntadr[BodyId]; - if (Model->jnt_type[FJ] == mjJNT_FREE) { - const int32 FD = Model->jnt_dofadr[FJ]; - const double* q = &Data->xquat[4 * BodyId]; - const double qw = q[0], qx = q[1], qy = q[2], qz = q[3]; - const double HalfYaw = FMath::DegreesToRadians((double)TargetYawDeg) * 0.5; - const double Qtw = FMath::Cos(HalfYaw), Qtz = FMath::Sin(HalfYaw); - - const double aw = qw, ax = -qx, ay = -qy, az = -qz; - double Qew = aw * Qtw - az * Qtz; - double Qex = ax * Qtw + ay * Qtz; - double Qey = -ax * Qtz + ay * Qtw; - double Qez = aw * Qtz + az * Qtw; - if (Qew < 0.0) { Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; } - - const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; - const double KpAtt[3] = { 1.5, 1.5, 1.0 }; - const double Wmax[3] = { 0.8, 0.8, 0.6 }; - const double Wn = 5.0; - const double Takeover = 1.0 - Fresh; + double AttT[3] = { 0.0, 0.0, 0.0 }; + ComputeAttitudeTorque(Model, Data, BodyId, (double)Model->opt.timestep, AttT); - for (int32 Axis = 0; Axis < 3; ++Axis) { - const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] - : Model->body_inertia[3 * BodyId + Axis]; - const double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis], -Wmax[Axis], Wmax[Axis]); - double T = I * Wn * (Wsp - (double)Data->qvel[FD + 3 + Axis]) * Takeover; - T = FMath::Clamp(T, -1500.0, 1500.0); - if (Axis == 0) { Tx += T; } - else if (Axis == 1) { Ty += T; } - else { Tz += T; } - } - } + // 自检:-RovFlipTest 在 6~6.8 s 施加 500 N·m 横滚力矩把机身掀翻,用于验证回正能力 + static const bool bFlipTest = FParse::Param(FCommandLine::Get(), TEXT("RovFlipTest")); + if (bFlipTest && Data->time > 6.0 && Data->time < 6.8) + { + AttT[0] += 500.0; } + Tx += AttT[0]; + Ty += AttT[1]; + Tz += AttT[2]; Data->qfrc_applied[DofAdr + 3] = (mjtNum)Tx; Data->qfrc_applied[DofAdr + 4] = (mjtNum)Ty; @@ -1451,15 +1791,89 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) Data->qfrc_constraint[DofAdr + 5]); } } - // 诊断 - static int32 ApplyLogCounter = 0; - if (++ApplyLogCounter % 500 == 0) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][通道] mj_applyFT dof=%d F=(%.1f,%.1f,%.1f) N " - "浮力=%.1f N T=(%.1f,%.1f,%.1f) N·m Z=%.3f"), - DofAdr, - Data->qfrc_applied[DofAdr + 0], Data->qfrc_applied[DofAdr + 1], Data->qfrc_applied[DofAdr + 2], - Buoy, Data->qfrc_applied[DofAdr + 3], Data->qfrc_applied[DofAdr + 4], Data->qfrc_applied[DofAdr + 5], - Data->xpos[3 * BodyId + 2]); + // 【6】实时状态缓存:供 HUD 与外部读取(3.4 / 7.3.1) + { + const double* qq = &Data->xquat[4 * BodyId]; + CachedRollDeg.store(FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qq[0] * qq[1] + qq[2] * qq[3]), + 1.0 - 2.0 * (qq[1] * qq[1] + qq[2] * qq[2])))); + CachedPitchDeg.store(FMath::RadiansToDegrees( + FMath::Asin(FMath::Clamp(2.0 * (qq[0] * qq[2] - qq[3] * qq[1]), -1.0, 1.0)))); + CachedYawDeg.store(FMath::RadiansToDegrees( + FMath::Atan2(2.0 * (qq[0] * qq[3] + qq[1] * qq[2]), + 1.0 - 2.0 * (qq[2] * qq[2] + qq[3] * qq[3])))); + CachedDepthM.store((double)WaterSurfaceZ - Data->xpos[3 * BodyId + 2]); + CachedVelocityX.store(Data->qvel[DofAdr + 0]); + CachedVelocityY.store(Data->qvel[DofAdr + 1]); + CachedVelocityZ.store(Data->qvel[DofAdr + 2]); + const double Fx = CachedForceX.load() * Fresh; + const double Fy = CachedForceY.load() * Fresh; + const double Fz = CachedForceZ.load() * Fresh; + CachedThrustN.store(FMath::Sqrt(Fx * Fx + Fy * Fy + Fz * Fz)); + + // 接触力:只统计涉及本体的接触,并累加法向力方向 + double ContactSum = 0.0, Px = 0.0, Py = 0.0, Pz = 0.0; + double Dx = 0.0, Dy = 0.0, Dz = 0.0; + mjtNum F6[6] = { 0, 0, 0, 0, 0, 0 }; + for (int32 i = 0; i < Data->ncon; ++i) + { + const mjContact& C = Data->contact[i]; + const int32 B1 = Model->geom_bodyid[C.geom1]; + const int32 B2 = Model->geom_bodyid[C.geom2]; + bool bTouchesBody = false; + bool bBody1IsRov = false; + for (int32 b : { B1, B2 }) + { + bool bInSubtree = false; + int32 p = b; + while (p > 0) + { + if (p == BodyId) { bInSubtree = true; break; } + p = Model->body_parentid[p]; + } + if (b == BodyId) { bInSubtree = true; } + if (bInSubtree) + { + bTouchesBody = true; + if (b == B1) { bBody1IsRov = true; } + break; + } + } + if (!bTouchesBody) { continue; } + mj_contactForce(Model, Data, i, F6); + ContactSum += FMath::Abs((double)F6[0]); + // frame[0..2] 是从 geom1 指向 geom2 的法向;作用在本体上的方向取反 + const double Sign = bBody1IsRov ? -1.0 : 1.0; + Dx += Sign * C.frame[0] * F6[0]; + Dy += Sign * C.frame[1] * F6[0]; + Dz += Sign * C.frame[2] * F6[0]; + Px = C.pos[0]; Py = C.pos[1]; Pz = C.pos[2]; + } + CachedContactForce.store(ContactSum); + // 接触点存成「相对机体、UE 轴、厘米」的偏移:HUD 直接用 + // 机体 UE 位置 + 该偏移即可,避免 MuJoCo/UE 两套坐标混用。 + const double BX = Data->xpos[3 * BodyId + 0]; + const double BY = Data->xpos[3 * BodyId + 1]; + const double BZ = Data->xpos[3 * BodyId + 2]; + CachedContactPointX.store((Px - BX) * 100.0); + CachedContactPointY.store(-(Py - BY) * 100.0); + CachedContactPointZ.store((Pz - BZ) * 100.0); + const double DNorm = FMath::Sqrt(Dx * Dx + Dy * Dy + Dz * Dz); + if (DNorm > 1e-9) + { + // 法向同样做 Y 镜像,得到 UE 世界系方向 + CachedContactDirX.store(Dx / DNorm); + CachedContactDirY.store(-Dy / DNorm); + CachedContactDirZ.store(Dz / DNorm); + } + else + { + CachedContactDirX.store(0.0); + CachedContactDirY.store(0.0); + CachedContactDirZ.store(0.0); + } + CachedFlowX.store(OceanCurrent.LastVelocity.X); + CachedFlowY.store(-OceanCurrent.LastVelocity.Y); + CachedFlowZ.store(OceanCurrent.LastVelocity.Z); } } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h index 729fd635..b371eb6d 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h @@ -14,12 +14,33 @@ #include "CoreMinimal.h" #include "Components/ActorComponent.h" #include "Vehicles/Rov/RovUnderwaterPhysics.h" +#include "Vehicles/Rov/RovFluidParams.h" #include #include "RovMuJoCoBridge.generated.h" class AMjArticulation; class UMjBody; +/** 实时状态快照,供 HUD 与外部读取。 */ +USTRUCT(BlueprintType) +struct FRovStatusSnapshot +{ + GENERATED_BODY() + + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float RollDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float PitchDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float YawDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float DepthM = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float ThrustN = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float ContactForceN = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float CurrentSpeedMps = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector VelocityMps = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector ContactPoint = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector ContactForceDir = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector FlowVelocityMps = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") bool bValid = false; +}; + // MuJoCo 的 mjModel / mjData 是 struct mjModel_ / struct mjData_ 的 typedef。 // 这里只前向声明,避免把 mujoco.h 拉进 AirSim 的公共头。 struct mjModel_; @@ -260,6 +281,78 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Depth") bool bEnableDepthHold = true; + // ---------------------------- 配置文件 ---------------------------- + + /** + * 是否从配置文件加载参数。 + * 默认读取 /Config/RovParams.json,可用 -RovConfig=<路径> 指定其他文件。 + * 优先级:命令行 > 配置文件 > 类默认值。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Config") + bool bLoadConfigFile = true; + + /** 生效的配置文件绝对路径(加载后回填,便于日志核对)。 */ + UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category = "ROV|MuJoCo|Config") + FString LoadedConfigPath; + + // ---------------------------- 接触日志 ---------------------------- + + /** 是否把逐接触点数据写入 CSV。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Log") + bool bLogContacts = false; + + /** 接触日志路径。留空则写入 /Saved/RovContactLog.csv。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Log") + FString ContactLogPath; + + /** 接触日志降采样间隔(物理步数)。10 = 500 Hz 步长下 50 Hz。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Log", meta = (ClampMin = "1")) + int32 ContactLogInterval = 10; + + // ---------------------------- 显式阻力与附加质量 ---------------------------- + + /** + * 是否叠加显式二次阻力 F = 0.5·ρ·Cd·A·v²(沿机体系三轴)。 + * 与 MuJoCo 原生椭球模型并联,用于补偿椭球近似对大钝体/细长体的偏差。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bUseExplicitDrag = false; + + /** 机体系三轴阻力系数 Cd。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector DragCoeff = FVector(0.82f, 1.15f, 1.05f); + + /** 机体系三轴迎流参考面积(m²)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector DragRefArea = FVector(0.24f, 0.32f, 0.48f); + + /** + * 是否叠加附加质量力 F = -m_A·a(沿机体系三轴)。 + * MuJoCo 椭球模型内部已含附加质量,本项用于在不改模型的前提下单独调整。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bUseAddedMass = false; + + /** 机体系三轴附加质量(kg)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector AddedMass = FVector(0.0f, 0.0f, 0.0f); + + /** 按外形自动估算并覆盖 DragCoeff / DragRefArea / AddedMass。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + bool bAutoEstimateFluidParams = false; + + /** 自动估算用的外形。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + ERovHullShape HullShape = ERovHullShape::Box; + + /** 自动估算用的外形半尺寸(m)。含义见 URovFluidLibrary::EstimateFluidParams。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + FVector HullDims = FVector(0.4f, 0.3f, 0.2f); + + /** 读取实时状态快照(姿态、深度、推力、接触力、洋流)。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|MuJoCo") + void GetStatusSnapshot(FRovStatusSnapshot& Out) const; + /** 桥接是否已生效。 */ UFUNCTION(BlueprintCallable, BlueprintPure, Category = "ROV|MuJoCo") bool IsBridgeActive() const { return bBridgeActive; } @@ -289,6 +382,18 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent /** 关闭 Pawn 物理与碰撞,避免与 MuJoCo 重复计算。 */ void DisablePawnPhysics(); + /** 读取配置文件并覆盖对应参数(命令行稍后覆盖,优先级最低)。 */ + void ApplyConfigFile(); + + /** + * 级联姿态控制(物理线程,500 Hz)。 + * 输出机体系力矩;积分步长取物理步长,因此与渲染帧率无关。 + */ + void ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int32 BodyId, double Dt, double OutTorque[3]); + + /** 逐接触点写 CSV(4.2.4)。 */ + void LogContacts(mjModel_* Model, mjData_* Data); + /** 每帧把键盘输入换算成推力与力矩,写入缓存。 */ void ApplyControlForce(); @@ -342,6 +447,12 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent /** 姿态控制器积分项(机体系,rad·s)。 */ FVector AttitudeIntegral = FVector::ZeroVector; + /** 物理线程姿态环的积分状态(与帧率无关)。 */ + double AttIntState[3] = { 0.0, 0.0, 0.0 }; + + /** 航向角速度前馈(rad/s,机体系),游戏线程写、物理线程读。 */ + std::atomic CachedYawFFRadPerSec{ 0.0 }; + /** 姿态控制器是否已初始化(首次捕获目标航向)。 */ bool bAttitudeInitialized = false; @@ -357,5 +468,33 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent /** 只在第一次解析失败时打印候选列表,避免每帧刷屏。 */ mutable bool bResolveWarned = false; + /** 接触日志文件句柄(EndPlay 时关闭)。 */ + FArchive* ContactLogArchive = nullptr; + int64 ContactLogStep = 0; + bool bContactLogHeaderWritten = false; + + /** 实时状态缓存,物理线程写、游戏线程读。 */ + std::atomic CachedContactForce{ 0.0 }; + std::atomic CachedContactPointX{ 0.0 }; + std::atomic CachedContactPointY{ 0.0 }; + std::atomic CachedContactPointZ{ 0.0 }; + std::atomic CachedContactDirX{ 0.0 }; + std::atomic CachedContactDirY{ 0.0 }; + std::atomic CachedContactDirZ{ 0.0 }; + std::atomic CachedVelocityX{ 0.0 }; + std::atomic CachedVelocityY{ 0.0 }; + std::atomic CachedVelocityZ{ 0.0 }; + std::atomic CachedFlowX{ 0.0 }; + std::atomic CachedFlowY{ 0.0 }; + std::atomic CachedFlowZ{ 0.0 }; + std::atomic CachedRollDeg{ 0.0 }; + std::atomic CachedPitchDeg{ 0.0 }; + std::atomic CachedYawDeg{ 0.0 }; + std::atomic CachedDepthM{ 0.0 }; + std::atomic CachedThrustN{ 0.0 }; + bool bBridgeActive = false; + + /** 调试用:-RovContactTest 时施加的额外下压力(N),把 ROV 压到海床以产生接触。 */ + double ContactTestForce = 0.0; }; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp new file mode 100644 index 00000000..007cc88a --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp @@ -0,0 +1,138 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "Vehicles/Rov/RovObstacleField.h" + +#include "Components/StaticMeshComponent.h" +#include "Components/SceneComponent.h" +#include "Engine/StaticMesh.h" +#include "Materials/MaterialInstanceDynamic.h" +#include "UObject/ConstructorHelpers.h" + +ARovObstacleField::ARovObstacleField() +{ + PrimaryActorTick.bCanEverTick = false; + RootComponent = CreateDefaultSubobject(TEXT("Root")); +} + +void ARovObstacleField::BuildDefaults() +{ + Obstacles.Reset(); + + // 位置与 MJCF 里的障碍几何体一一对应(相对 ROV 出生点,单位 cm)。 + // 换算:UE = 100 * (x, -y, z - z_rov),z_rov 取出生高度 -1.0 m。 + // 圆柱默认轴为 Z:轴向沿 X 用 Pitch=90,沿 Y 用 Pitch=90 + Yaw=90。 + + // 1. 海底主干管道与分支管道 + Obstacles.Add({ ERovObstacleType::Pipe, FVector(0.0f, -320.0f, -160.0f), + FVector(25.0f, 25.0f, 450.0f), FRotator(90.0f, 0.0f, 0.0f) }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(300.0f, -100.0f, -160.0f), + FVector(20.0f, 20.0f, 300.0f), FRotator(90.0f, 90.0f, 0.0f) }); + + // 2. 井口采油树(基座 + 立管) + Obstacles.Add({ ERovObstacleType::Frame, FVector(180.0f, 50.0f, -200.0f), + FVector(60.0f, 60.0f, 50.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(180.0f, 50.0f, -90.0f), + FVector(22.0f, 22.0f, 60.0f), FRotator::ZeroRotator }); + + // 3. 导管架支撑桩(两根立柱 + 横梁) + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, -180.0f, -80.0f), + FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 180.0f, -80.0f), + FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 0.0f, -200.0f), + FVector(12.0f, 12.0f, 180.0f), FRotator(90.0f, 90.0f, 0.0f) }); + + // 4. 检修结构体与礁石 + Obstacles.Add({ ERovObstacleType::Frame, FVector(0.0f, 320.0f, -140.0f), + FVector(50.0f, 80.0f, 60.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Reef, FVector(-50.0f, -150.0f, -210.0f), + FVector(40.0f, 40.0f, 40.0f), FRotator::ZeroRotator }); +} +void ARovObstacleField::OnConstruction(const FTransform& Transform) +{ + Super::OnConstruction(Transform); + if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) + { + BuildDefaults(); + } +} + +void ARovObstacleField::BeginPlay() +{ + Super::BeginPlay(); + if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) + { + BuildDefaults(); + } + Rebuild(); +} + +void ARovObstacleField::Rebuild() +{ + for (UStaticMeshComponent* Comp : SpawnedComponents) + { + if (Comp) + { + Comp->DestroyComponent(); + } + } + SpawnedComponents.Reset(); + + const FVector Base = AnchorActor ? AnchorActor->GetActorLocation() : GetActorLocation(); + + UStaticMesh* MeshPipe = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Cylinder.Cylinder")); + UStaticMesh* MeshBox = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Cube.Cube")); + UStaticMesh* MeshSphere = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Sphere.Sphere")); + + // 引擎基础网格的原始尺寸:圆柱 r=50 h=100,立方体 100³,球 r=50(单位 cm) + for (const FRovObstacleDef& Def : Obstacles) { + UStaticMesh* Mesh = nullptr; + FVector Scale = FVector::OneVector; + FLinearColor Color(0.6f, 0.6f, 0.6f); + + switch (Def.Type) + { + case ERovObstacleType::Pipe: + Mesh = MeshPipe; + Scale = FVector(Def.Size.X / 50.0f, Def.Size.Y / 50.0f, Def.Size.Z / 50.0f); + Color = FLinearColor(0.55f, 0.42f, 0.20f); + break; + case ERovObstacleType::Frame: + Mesh = MeshBox; + Scale = Def.Size / 50.0f; + Color = FLinearColor(0.35f, 0.42f, 0.50f); + break; + case ERovObstacleType::Reef: + Mesh = MeshSphere; + Scale = Def.Size / 50.0f; + Color = FLinearColor(0.45f, 0.38f, 0.30f); + break; + default: + break; + } + + if (!Mesh) + { + continue; + } + + UStaticMeshComponent* Comp = NewObject(this); + Comp->SetStaticMesh(Mesh); + Comp->SetupAttachment(RootComponent); + Comp->RegisterComponent(); + Comp->SetWorldLocation(Base + Def.Location); + Comp->SetWorldRotation(Def.Rotation); + Comp->SetWorldScale3D(Scale); + Comp->SetCollisionEnabled(ECollisionEnabled::NoCollision); + Comp->SetCastShadow(false); + + if (UMaterialInstanceDynamic* MID = Comp->CreateAndSetMaterialInstanceDynamic(0)) + { + MID->SetVectorParameterValue(TEXT("Color"), Color); + } + SpawnedComponents.Add(Comp); + } + + UE_LOG(LogTemp, Log, TEXT("[RovScene] 障碍物 %d 个(管道/框架/礁石),锚点 %s"), + SpawnedComponents.Num(), *Base.ToString()); +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h new file mode 100644 index 00000000..b89b033f --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h @@ -0,0 +1,81 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// 障碍物碰撞场景:管道、框架、礁石三类障碍的运行时摆放。 + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" +#include "RovObstacleField.generated.h" + +class UStaticMeshComponent; + +/** 障碍类型。 */ +UENUM(BlueprintType) +enum class ERovObstacleType : uint8 +{ + Pipe UMETA(DisplayName = "管道"), + Frame UMETA(DisplayName = "框架"), + Reef UMETA(DisplayName = "礁石"), +}; + +/** 单个障碍定义。位置相对锚点(通常是 ROV 出生点),单位 cm。 */ +USTRUCT(BlueprintType) +struct FRovObstacleDef +{ + GENERATED_BODY() + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + ERovObstacleType Type = ERovObstacleType::Pipe; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + FVector Location = FVector::ZeroVector; + + /** 尺寸(cm):圆柱为 (半径, 半径, 半高),立方体为半尺寸,球为半径。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + FVector Size = FVector(50.0f, 50.0f, 200.0f); + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + FRotator Rotation = FRotator::ZeroRotator; +}; + +/** + * 障碍物场。挂在关卡里或由 ROV 仿真模式自动生成。 + * + * 默认摆放:3 段管道、4 根框架立柱、3 块礁石,围绕锚点分布, + * 与 MuJoCo 模型里的 pipeline_* / pile_* / reef_* 碰撞体一一对应。 + * 这些障碍直接复用给 4.1.3 / 7.2.x 的接近与碰撞试验。 + */ +UCLASS() +class AIRSIM_API ARovObstacleField : public AActor +{ + GENERATED_BODY() + +public: + ARovObstacleField(); + + virtual void OnConstruction(const FTransform& Transform) override; + virtual void BeginPlay() override; + + /** 障碍列表。为空且 bSpawnDefaultObstacles=true 时自动生成默认三类障碍。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + TArray Obstacles; + + /** 未显式配置时是否生成默认障碍。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + bool bSpawnDefaultObstacles = true; + + /** 摆放锚点。为空则以自身位置为锚点。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|Obstacle") + AActor* AnchorActor = nullptr; + + /** 重建全部障碍(改配置后调用)。 */ + UFUNCTION(BlueprintCallable, Category = "ROV|Obstacle") + void Rebuild(); + +private: + void BuildDefaults(); + + UPROPERTY() + TArray SpawnedComponents; +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp new file mode 100644 index 00000000..441b8b24 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp @@ -0,0 +1,240 @@ +// Copyright (c) OpenHUTB. All rights reserved. + +#include "Vehicles/Rov/RovStatusHUD.h" + +#include "Engine/Canvas.h" +#include "Engine/Engine.h" +#include "EngineUtils.h" +#include "Kismet/GameplayStatics.h" +#include "GameFramework/Pawn.h" +#include "GameFramework/PlayerController.h" +#include "Vehicles/Rov/RovMuJoCoBridge.h" + +ARovStatusHUD::ARovStatusHUD() +{ + PrimaryActorTick.bCanEverTick = true; +} + +void ARovStatusHUD::BeginPlay() +{ + Super::BeginPlay(); + ResolveBridge(); +} + +void ARovStatusHUD::ResolveBridge() +{ + if (Bridge.IsValid()) + { + return; + } + UWorld* World = GetWorld(); + if (!World) + { + return; + } + for (TActorIterator It(World); It; ++It) + { + APawn* Pawn = *It; + if (!Pawn) + { + continue; + } + URovMuJoCoBridge* Found = Pawn->FindComponentByClass(); + if (Found) + { + Bridge = Found; + RovActor = Pawn; + return; + } + } +} + +void ARovStatusHUD::Tick(float DeltaSeconds) +{ + Super::Tick(DeltaSeconds); + ResolveBridge(); + + if (APlayerController* PC = GetOwningPlayerController()) + { + if (PC->WasInputKeyJustPressed(EKeys::R)) + { + bShowStatusPanel = !bShowStatusPanel; + } + if (PC->WasInputKeyJustPressed(EKeys::V)) + { + bShowVectors = !bShowVectors; + } + } +} + +void ARovStatusHUD::DrawHUD() +{ + Super::DrawHUD(); + if (!Canvas) + { + return; + } + if (bShowStatusPanel) + { + DrawStatusPanel(); + } + if (bShowVectors) + { + DrawVectors(); + } +} + +void ARovStatusHUD::DrawStatusPanel() +{ + FRovStatusSnapshot S; + if (Bridge.IsValid()) + { + Bridge->GetStatusSnapshot(S); + } + if (!S.bValid) + { + DrawText(TEXT("ROV 状态:等待 MuJoCo 桥接…"), FLinearColor(1.0f, 0.8f, 0.2f), PanelOrigin.X, PanelOrigin.Y, nullptr, 1.1f); + return; + } + + const float X = bAnchorRight ? (Canvas->SizeX - 316.0f) : PanelOrigin.X; + float Y = PanelOrigin.Y; + const float LH = 20.0f; + const FLinearColor Title(0.4f, 0.9f, 1.0f); + const FLinearColor Body(0.92f, 0.95f, 1.0f); + const FLinearColor Warn(1.0f, 0.55f, 0.25f); + + DrawRect(FLinearColor(0.02f, 0.06f, 0.10f, 0.55f), X - 8.0f, Y - 8.0f, 300.0f, 8.0f * LH + 16.0f); + + DrawText(TEXT("ROV 实时状态"), Title, X, Y, nullptr, 1.2f); Y += LH * 1.3f; + DrawText(FString::Printf(TEXT("姿态 横滚 %+6.1f° 俯仰 %+6.1f°"), S.RollDeg, S.PitchDeg), Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(FString::Printf(TEXT(" 航向 %+6.1f°"), S.YawDeg), Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(FString::Printf(TEXT("深度 %.2f m"), S.DepthM), Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(FString::Printf(TEXT("推力 %.1f N"), S.ThrustN), Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(FString::Printf(TEXT("接触力 %.1f N"), S.ContactForceN), + S.ContactForceN > 1.0f ? Warn : Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(FString::Printf(TEXT("洋流 %.3f m/s"), S.FlowVelocityMps.Size()), Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(FString::Printf(TEXT("速度 %.3f m/s"), S.VelocityMps.Size()), Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(TEXT("R 面板 V 向量"), FLinearColor(0.6f, 0.65f, 0.7f), X, Y, nullptr, 0.9f); +} + +void ARovStatusHUD::DrawVectors() +{ + if (!RovActor.IsValid() || !Bridge.IsValid() || !Canvas) + { + return; + } + FRovStatusSnapshot S; + Bridge->GetStatusSnapshot(S); + if (!S.bValid) + { + return; + } + + const FVector Origin = RovActor->GetActorLocation(); + + // UE 空间速度:用机体位置差分求方向,保证与屏幕上的运动方向一致; + // 幅值仍取 MuJoCo 的 qvel(更准)。两套坐标系不再混用。 + const float NowTime = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0f; + FVector VelUE = FVector::ZeroVector; + if (bHasLastRov) + { + const float Dt = FMath::Max(NowTime - LastRovTime, 1e-3f); + VelUE = (Origin - LastRovLocation) / Dt; + } + LastRovLocation = Origin; + LastRovTime = NowTime; + bHasLastRov = true; + SmoothedVelocityUE = FMath::Lerp(SmoothedVelocityUE, VelUE, 0.35f); + + // 起终点都投影到屏幕,箭头方向与场景中的真实方向一致 + auto DrawWorldArrow = [this](const FVector& Start, const FVector& Dir, float LengthCm, + const FLinearColor& Color, float Thickness) -> bool + { + if (Dir.IsNearlyZero() || LengthCm <= 1.0f) + { + return false; + } + const FVector End = Start + Dir.GetSafeNormal() * LengthCm; + const FVector P0 = Project(Start); + const FVector P1 = Project(End); + if (P0.Z <= 0.0f || P1.Z <= 0.0f) + { + return false; + } + DrawLine(P0.X, P0.Y, P1.X, P1.Y, Color, Thickness); + + const FVector2D D(P1.X - P0.X, P1.Y - P0.Y); + const float L = D.Size(); + if (L < 6.0f) + { + return true; + } + const FVector2D U = D / L; + const FVector2D N(-U.Y, U.X); + const float Head = FMath::Min(14.0f, L * 0.3f); + const FVector2D Base(P1.X - U.X * Head, P1.Y - U.Y * Head); + DrawLine(P1.X, P1.Y, Base.X + N.X * Head * 0.6f, Base.Y + N.Y * Head * 0.6f, Color, Thickness); + DrawLine(P1.X, P1.Y, Base.X - N.X * Head * 0.6f, Base.Y - N.Y * Head * 0.6f, Color, Thickness); + return true; + }; + + // 接触力:从接触点出发,沿实测法向力方向 + if (S.ContactForceN > 0.5f && !S.ContactForceDir.IsNearlyZero()) + { + const float Len = FMath::Clamp(S.ContactForceN * ForceArrowScale, 30.0f, MaxArrowLength); + // 接触点存的是「相对机体、UE 轴、厘米」的偏移 + const FVector ContactUE = Origin + S.ContactPoint; + if (DrawWorldArrow(ContactUE, S.ContactForceDir, Len, FLinearColor(1.0f, 0.55f, 0.1f), 3.0f)) + { + const FVector Tip = Project(ContactUE + S.ContactForceDir.GetSafeNormal() * Len); + if (Tip.Z > 0.0f) + { + DrawText(FString::Printf(TEXT("F %.0f N"), S.ContactForceN), + FLinearColor(1.0f, 0.65f, 0.2f), Tip.X + 8.0f, Tip.Y - 10.0f, nullptr, 1.0f); + } + } + } + + // 机体速度:方向取 UE 位置差分,幅值取 MuJoCo 速度 + const float Speed = static_cast(S.VelocityMps.Size()); + if (Speed > 0.01f) + { + const float Len = FMath::Clamp(Speed * VelocityArrowScale, 40.0f, MaxArrowLength); + FVector VelDir = SmoothedVelocityUE; + if (VelDir.Size() < 5.0f) + { + // 低速时差分噪声大,退回 MuJoCo 速度(Y 镜像到 UE 轴) + VelDir = FVector(S.VelocityMps.X, -S.VelocityMps.Y, S.VelocityMps.Z); + } + if (DrawWorldArrow(Origin, VelDir, Len, FLinearColor(0.3f, 1.0f, 0.45f), 2.5f)) + { + const FVector Tip = Project(Origin + VelDir.GetSafeNormal() * Len); + if (Tip.Z > 0.0f) + { + DrawText(FString::Printf(TEXT("v %.2f m/s"), Speed), + FLinearColor(0.35f, 1.0f, 0.5f), Tip.X + 8.0f, Tip.Y + 2.0f, nullptr, 1.0f); + } + } + } + + // 洋流速度场:机体周围 3×3 采样,方向与幅值均取自实测洋流 + if (bShowFlowField) + { + const FVector Flow = S.FlowVelocityMps; + const float FlowSpeed = static_cast(Flow.Size()); + if (FlowSpeed > 1e-3f) + { + const FLinearColor FlowColor(0.35f, 0.7f, 1.0f, 0.85f); + const float Len = FMath::Clamp(FlowSpeed * 250.0f, 25.0f, 150.0f); + for (int32 iy = -1; iy <= 1; ++iy) + { + for (int32 iz = -1; iz <= 1; ++iz) + { + const FVector Sample = Origin + FVector(0.0f, iy * 150.0f, iz * 100.0f); + DrawWorldArrow(Sample, Flow, Len, FlowColor, 1.5f); + } + } + } + } +} diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.h new file mode 100644 index 00000000..5c6f9da7 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.h @@ -0,0 +1,83 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 实时状态面板与力/速度向量可视化。 + +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/HUD.h" +#include "RovStatusHUD.generated.h" + +class URovMuJoCoBridge; + +/** + * 屏幕左上角的状态面板 + 世界中的向量箭头。 + * + * 面板内容:姿态角、深度、推力、接触力、洋流速度。 + * 向量:夹爪接触力箭头、机体速度箭头、洋流速度场。 + * 可用 R 键切换面板、V 键切换向量显示。 + */ +UCLASS() +class AIRSIM_API ARovStatusHUD : public AHUD +{ + GENERATED_BODY() + +public: + ARovStatusHUD(); + + virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; + virtual void DrawHUD() override; + + /** 是否显示状态面板。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bShowStatusPanel = true; + + /** 是否显示力/速度向量。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bShowVectors = true; + + /** 是否显示洋流速度场(机体周围 3×3 采样箭头)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bShowFlowField = true; + + /** 面板左上角位置(屏幕像素)。bAnchorRight 为 true 时忽略 X,贴右边缘。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + FVector2D PanelOrigin = FVector2D(24.0f, 24.0f); + + /** 面板贴右上角,避开 AirSim 左上角的消息文字。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + bool bAnchorRight = true; + + /** 力箭头比例(cm / N)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + float ForceArrowScale = 0.5f; + + /** 速度箭头比例(cm / (m/s))。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + float VelocityArrowScale = 250.0f; + + /** 箭头最长长度(cm),避免超出视野。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|HUD") + float MaxArrowLength = 300.0f; + +private: + void DrawStatusPanel(); + void DrawVectors(); + + UPROPERTY() + TWeakObjectPtr Bridge; + + UPROPERTY() + TWeakObjectPtr RovActor; + + /** 上一帧的机体位置与时间,用于求 UE 空间速度方向。 */ + FVector LastRovLocation = FVector::ZeroVector; + float LastRovTime = 0.0f; + bool bHasLastRov = false; + + /** 平滑后的 UE 空间速度(cm/s)。 */ + FVector SmoothedVelocityUE = FVector::ZeroVector; + + void ResolveBridge(); +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h index 8d4d5172..87df8c4a 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h @@ -59,6 +59,9 @@ struct FRovOceanCurrent float CurHoriz = 0.0f; float CurVert = 0.0f; + /** 最近一次 GetVelocity 的结果(MuJoCo 世界系,m/s),供状态显示读取。 */ + FVector LastVelocity = FVector::ZeroVector; + FVector LastPos = FVector::ZeroVector; bool bHasLastPos = false; @@ -162,7 +165,8 @@ struct FRovOceanCurrent const FVector VGM = UpdateGaussMarkov(Dt); const FVector VStrat = StratifiedAt(-Pos.Z); // z 向上 → 深度取反 const FVector VTurb = TurbulentAt(Pos); - return VGM + VStrat + VTurb; + LastVelocity = VGM + VStrat + VTurb; + return LastVelocity; } }; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp index 7c55f50f..7187eb0a 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp @@ -6,6 +6,8 @@ #include "Logging/MessageLog.h" #include "Engine/World.h" #include "GameFramework/PlayerController.h" +#include "Vehicles/Rov/RovObstacleField.h" +#include "Vehicles/Rov/RovStatusHUD.h" #include "AirBlueprintLib.h" #include "vehicles/rov/api/RovApiBase.hpp" @@ -22,6 +24,36 @@ void ASimModeWorldRov::BeginPlay() //let base class setup physics world initializeForPlay(); + + // 障碍物场景(7.3.2):管道 / 框架 / 礁石,可被关卡里手动摆放的实例替代 + if (bSpawnObstacleField && GetWorld()) + { + FActorSpawnParameters SpawnParams; + SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; + ARovObstacleField* Field = GetWorld()->SpawnActor(ARovObstacleField::StaticClass(), FTransform::Identity, SpawnParams); + if (Field) + { + // 以 ROV 出生点为锚点,保证视觉代理与 MJCF 里的碰撞几何体位置一致 + TArray Pawns; + getExistingVehiclePawns(Pawns); + if (Pawns.Num() > 0 && Pawns[0]) + { + Field->AnchorActor = Pawns[0]; + Field->Rebuild(); + } + } + UE_LOG(LogTemp, Log, TEXT("[RovScene] 障碍物场已生成:%s"), Field ? TEXT("成功") : TEXT("失败")); + } + + // 实时状态面板 + 力/速度向量(3.4 / 7.3.1) + if (bSpawnStatusHUD && GetWorld()) + { + if (APlayerController* PC = GetWorld()->GetFirstPlayerController()) + { + PC->ClientSetHUD(ARovStatusHUD::StaticClass()); + UE_LOG(LogTemp, Log, TEXT("[RovScene] 实时状态面板已启用")); + } + } } void ASimModeWorldRov::EndPlay(const EEndPlayReason::Type EndPlayReason) diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h index c357883c..f2138560 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.h @@ -17,6 +17,14 @@ class AIRSIM_API ASimModeWorldRov : public ASimModeWorldBase virtual void BeginPlay() override; virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + /** 是否自动生成障碍物场景(管道 / 框架 / 礁石)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV") + bool bSpawnObstacleField = true; + + /** 是否启用 ROV 实时状态面板与力/速度向量显示。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV") + bool bSpawnStatusHUD = true; + protected: //overrides virtual APawn* createVehiclePawn(const AirSimSettings::VehicleSetting& vehicle_setting) override; virtual void setupClockSpeed() override; From 14d48af7f62d22b82dca76e0dd80a78ce5c35e23 Mon Sep 17 00:00:00 2001 From: shark Date: Tue, 10 Nov 2026 21:26:46 +0800 Subject: [PATCH 8/8] =?UTF-8?q?fix(ci):=20=E4=BF=AE=E5=A4=8D=20clang-forma?= =?UTF-8?q?t=20=E4=B8=8E=20Windows=20=E6=9E=84=E5=BB=BA=E6=A3=80=E6=9F=A5?= =?UTF-8?q?=E5=A4=B1=E8=B4=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 对 PR 变更的 C/C++ 文件按仓库 .clang-format(clang-format 11)重新格式化 - AirLibUnitTests 的 Debug 配置补充屏蔽 4244/4305/4189/4267: ROV 相关头文件里的 double 到 float 字面量、未使用变量属历史写法, 在单元测试工程开启 /WX 后会被当作编译错误 --- AirLib/include/vehicles/rov/RovBodyParams.hpp | 16 +- .../include/vehicles/rov/RovPhysicsBody.hpp | 6 +- AirLibUnitTests/AirLibUnitTests.vcxproj | 12 +- AirLibUnitTests/RovHydrodynamicsTest.hpp | 58 +- AirLibUnitTests/main.cpp | 3 - .../Source/Vehicles/Rov/RovFluidParams.cpp | 64 +- .../Source/Vehicles/Rov/RovFluidParams.h | 16 +- .../Source/Vehicles/Rov/RovMuJoCoBridge.cpp | 873 +++++++++--------- .../Source/Vehicles/Rov/RovMuJoCoBridge.h | 56 +- .../Source/Vehicles/Rov/RovObstacleField.cpp | 53 +- .../Source/Vehicles/Rov/RovObstacleField.h | 6 +- .../AirSim/Source/Vehicles/Rov/RovPawn.cpp | 11 +- .../Source/Vehicles/Rov/RovStatusHUD.cpp | 134 ++- .../Vehicles/Rov/RovUnderwaterPhysics.h | 35 +- .../Source/Vehicles/Rov/SimModeWorldRov.cpp | 15 +- 15 files changed, 687 insertions(+), 671 deletions(-) diff --git a/AirLib/include/vehicles/rov/RovBodyParams.hpp b/AirLib/include/vehicles/rov/RovBodyParams.hpp index 38e3778e..40ef5e8a 100644 --- a/AirLib/include/vehicles/rov/RovBodyParams.hpp +++ b/AirLib/include/vehicles/rov/RovBodyParams.hpp @@ -64,14 +64,14 @@ namespace airlib AeroParams aero_params; // Underwater Fossen Dynamics & Depth Safety Parameters - real_T z_bed = -30.0f; // Seabed depth limit (meters, default -30m) - real_T bed_stiffness = 2000.0f; // Soft boundary spring stiffness kp (N/m) - real_T bed_damping = 300.0f; // Soft boundary contact damping kd (N*s/m) - real_T bed_contact_margin = 0.5f; // Cushion transition margin before bed (m) - real_T bed_clamp_margin = 0.3f; // Hard clamp threshold margin (m) - bool enable_depth_clamp = true; // Whether seabed depth safety boundary is active - bool enable_neutral_buoyancy = true; // Whether neutral buoyancy model is active - bool enable_hydrodynamic_damping = true; // Whether fluid damping is active + real_T z_bed = -30.0f; // Seabed depth limit (meters, default -30m) + real_T bed_stiffness = 2000.0f; // Soft boundary spring stiffness kp (N/m) + real_T bed_damping = 300.0f; // Soft boundary contact damping kd (N*s/m) + real_T bed_contact_margin = 0.5f; // Cushion transition margin before bed (m) + real_T bed_clamp_margin = 0.3f; // Hard clamp threshold margin (m) + bool enable_depth_clamp = true; // Whether seabed depth safety boundary is active + bool enable_neutral_buoyancy = true; // Whether neutral buoyancy model is active + bool enable_hydrodynamic_damping = true; // Whether fluid damping is active }; protected: //must override by derived class diff --git a/AirLib/include/vehicles/rov/RovPhysicsBody.hpp b/AirLib/include/vehicles/rov/RovPhysicsBody.hpp index c6befe8e..d59cdc1a 100644 --- a/AirLib/include/vehicles/rov/RovPhysicsBody.hpp +++ b/AirLib/include/vehicles/rov/RovPhysicsBody.hpp @@ -484,14 +484,12 @@ namespace airlib Vector3r force_damping( -std::abs(p.damping_linear.x()) * vel_body.x() - std::abs(p.damping_linear_q.x()) * std::abs(vel_body.x()) * vel_body.x(), -std::abs(p.damping_linear.y()) * vel_body.y() - std::abs(p.damping_linear_q.y()) * std::abs(vel_body.y()) * vel_body.y(), - -std::abs(p.damping_linear.z()) * vel_body.z() - std::abs(p.damping_linear_q.z()) * std::abs(vel_body.z()) * vel_body.z() - ); + -std::abs(p.damping_linear.z()) * vel_body.z() - std::abs(p.damping_linear_q.z()) * std::abs(vel_body.z()) * vel_body.z()); Vector3r torque_damping( -std::abs(p.damping_angular.x()) * omega_body.x() - std::abs(p.damping_angular_q.x()) * std::abs(omega_body.x()) * omega_body.x(), -std::abs(p.damping_angular.y()) * omega_body.y() - std::abs(p.damping_angular_q.y()) * std::abs(omega_body.y()) * omega_body.y(), - -std::abs(p.damping_angular.z()) * omega_body.z() - std::abs(p.damping_angular_q.z()) * std::abs(omega_body.z()) * omega_body.z() - ); + -std::abs(p.damping_angular.z()) * omega_body.z() - std::abs(p.damping_angular_q.z()) * std::abs(omega_body.z()) * omega_body.z()); wrench.force = force_damping; wrench.torque = torque_damping; diff --git a/AirLibUnitTests/AirLibUnitTests.vcxproj b/AirLibUnitTests/AirLibUnitTests.vcxproj index 2b0da677..4543b4d8 100644 --- a/AirLibUnitTests/AirLibUnitTests.vcxproj +++ b/AirLibUnitTests/AirLibUnitTests.vcxproj @@ -139,7 +139,7 @@ WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include true - 5205%(DisableSpecificWarnings) + 4244;4305;4189;4267;5205%(DisableSpecificWarnings) stdcpp17 @@ -159,7 +159,7 @@ $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include true /w34263 /w34266 %(AdditionalOptions) - 4100;4505;4820;4464;4514;4710;4571;5205%(DisableSpecificWarnings) + 4244;4305;4189;4267;4100;4505;4820;4464;4514;4710;4571;5205%(DisableSpecificWarnings) stdcpp17 @@ -181,7 +181,7 @@ $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include false /w34263 /w34266 %(AdditionalOptions) - 5205%(DisableSpecificWarnings) + 4244;4305;4189;4267;5205%(DisableSpecificWarnings) stdcpp17 @@ -205,7 +205,7 @@ $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include false /w34263 /w34266 %(AdditionalOptions) - 5205%(DisableSpecificWarnings) + 4244;4305;4189;4267;5205%(DisableSpecificWarnings) stdcpp17 false @@ -230,7 +230,7 @@ $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include false /w34263 /w34266 %(AdditionalOptions) - 5205%(DisableSpecificWarnings) + 4244;4305;4189;4267;5205%(DisableSpecificWarnings) stdcpp17 @@ -254,7 +254,7 @@ $(ProjectDir)..\AirLib\deps\eigen3;$(ProjectDir)..\AirLib\include;$(ProjectDir)..\MavLinkCom\include false /w34263 /w34266 %(AdditionalOptions) - 5205%(DisableSpecificWarnings) + 4244;4305;4189;4267;5205%(DisableSpecificWarnings) stdcpp17 false diff --git a/AirLibUnitTests/RovHydrodynamicsTest.hpp b/AirLibUnitTests/RovHydrodynamicsTest.hpp index 71448847..5d962f3d 100644 --- a/AirLibUnitTests/RovHydrodynamicsTest.hpp +++ b/AirLibUnitTests/RovHydrodynamicsTest.hpp @@ -57,9 +57,9 @@ namespace airlib void testWrenchVertexCount(RovPhysicsBody& rov, const RovBodyParams& params) { uint expected_vertices = params.getParams().rotor_count + 3; - testAssert(rov.wrenchVertexCount() == expected_vertices, - "wrenchVertexCount must include rotors + buoyancy + damping + bed_contact vertices"); - std::cout << " [PASS] Wrench vertex count: " << rov.wrenchVertexCount() + testAssert(rov.wrenchVertexCount() == expected_vertices, + "wrenchVertexCount must include rotors + buoyancy + damping + bed_contact vertices"); + std::cout << " [PASS] Wrench vertex count: " << rov.wrenchVertexCount() << " (8 thrusters + 3 hydro vertices)" << std::endl; } @@ -78,18 +78,18 @@ namespace airlib const Wrench& b_wrench = rov.getBuoyancyVertex().getWrench(); // In NED body frame with level attitude, buoyancy force is straight up along -Z: testAssert(std::abs(b_wrench.force.z() - (-expected_buoyancy)) < 1e-2f, - "Buoyancy force in body frame must equal -m*g along Z"); + "Buoyancy force in body frame must equal -m*g along Z"); testAssert(std::abs(b_wrench.force.x()) < 1e-4f && std::abs(b_wrench.force.y()) < 1e-4f, - "Buoyancy force XY components must be zero when level"); + "Buoyancy force XY components must be zero when level"); // Evaluate net body wrench through FastPhysicsEngine Wrench body_wrench = rov.getBodyWrench(); Vector3r net_accel = (body_wrench.force / mass) + environment.getState().gravity; - testAssert(net_accel.norm() < 1e-2f, - "Net linear acceleration under neutral buoyancy and gravity must be zero (canceling gravity)"); + testAssert(net_accel.norm() < 1e-2f, + "Net linear acceleration under neutral buoyancy and gravity must be zero (canceling gravity)"); - std::cout << " [PASS] Neutral buoyancy: Mass=" << mass << "kg, Gravity=" << g - << "m/s^2, BuoyancyForce=" << expected_buoyancy + std::cout << " [PASS] Neutral buoyancy: Mass=" << mass << "kg, Gravity=" << g + << "m/s^2, BuoyancyForce=" << expected_buoyancy << "N, NetAccel=" << net_accel.norm() << "m/s^2 (Zero)" << std::endl; } @@ -109,11 +109,11 @@ namespace airlib Wrench wrench_roll = rov.getBodyWrench(); // Restoring torque about body X must be strictly negative to restore port-side up - testAssert(wrench_roll.torque.x() < -0.01f, - "Roll restoring torque tau_x must be strictly negative when vehicle rolls to starboard"); + testAssert(wrench_roll.torque.x() < -0.01f, + "Roll restoring torque tau_x must be strictly negative when vehicle rolls to starboard"); real_T expected_roll_tau = -std::abs(off_z) * mass * g * std::sin(roll_rad); testAssert(std::abs(wrench_roll.torque.x() - expected_roll_tau) < 0.1f, - "tau_x must match Fossen restoring moment formula: -off_z * m * g * sin(phi)"); + "tau_x must match Fossen restoring moment formula: -off_z * m * g * sin(phi)"); // 2. Pitch Test (+15 deg tilt, nose up) real_T pitch_rad = Utils::degreesToRadians(15.0f); @@ -124,11 +124,11 @@ namespace airlib Wrench wrench_pitch = rov.getBodyWrench(); // Restoring torque about body Y must be strictly negative to restore nose-down - testAssert(wrench_pitch.torque.y() < -0.01f, - "Pitch restoring torque tau_y must be strictly negative when vehicle pitches up"); + testAssert(wrench_pitch.torque.y() < -0.01f, + "Pitch restoring torque tau_y must be strictly negative when vehicle pitches up"); real_T expected_pitch_tau = -std::abs(off_z) * mass * g * std::sin(pitch_rad); testAssert(std::abs(wrench_pitch.torque.y() - expected_pitch_tau) < 0.1f, - "tau_y must match Fossen restoring moment formula: -off_z * m * g * sin(theta)"); + "tau_y must match Fossen restoring moment formula: -off_z * m * g * sin(theta)"); std::cout << " [PASS] Fossen metacentric self-righting restoring moments: " << "Roll tau_x=" << wrench_roll.torque.x() << " Nm (restoring), " @@ -139,8 +139,8 @@ namespace airlib { Kinematics::State state = Kinematics::State::zero(); state.pose.orientation = Quaternionr::Identity(); - state.twist.linear = Vector3r(1.0f, 1.5f, 0.8f); // Surge, sway, heave - state.twist.angular = Vector3r(0.6f, -0.5f, 0.4f); // Roll rate, pitch rate, yaw rate + state.twist.linear = Vector3r(1.0f, 1.5f, 0.8f); // Surge, sway, heave + state.twist.angular = Vector3r(0.6f, -0.5f, 0.4f); // Roll rate, pitch rate, yaw rate kinematics.setState(state); rov.update(); @@ -161,9 +161,9 @@ namespace airlib testAssert(d_wrench.torque.y() > 0.0f, "Pitch damping torque must oppose negative pitch rate"); testAssert(d_wrench.torque.z() < 0.0f, "Yaw damping torque must oppose positive yaw rate"); - std::cout << " [PASS] Hydrodynamic damping: Force=[" << d_wrench.force.x() << ", " + std::cout << " [PASS] Hydrodynamic damping: Force=[" << d_wrench.force.x() << ", " << d_wrench.force.y() << ", " << d_wrench.force.z() << "] N, Torque=[" - << d_wrench.torque.x() << ", " << d_wrench.torque.y() << ", " << d_wrench.torque.z() + << d_wrench.torque.x() << ", " << d_wrench.torque.y() << ", " << d_wrench.torque.z() << "] Nm (All strictly dissipative)" << std::endl; } @@ -178,7 +178,7 @@ namespace airlib kinematics.setState(state); rov.update(); testAssert(rov.getBedContactVertex().getWrench().force.norm() < 1e-4f, - "No bed contact force when vehicle is operating safely above bed"); + "No bed contact force when vehicle is operating safely above bed"); // 2. Approaching cushion (z = 29.8m, vz = 1.0 m/s): Smooth cushion damping active state.pose.position = Vector3r(0.0f, 0.0f, 29.8f); @@ -186,8 +186,8 @@ namespace airlib kinematics.setState(state); rov.update(); const Wrench& cushion_wrench = rov.getBedContactVertex().getWrench(); - testAssert(cushion_wrench.force.z() < 0.0f, - "Cushion damping force must be upward (-Z) to smoothly decelerate descent"); + testAssert(cushion_wrench.force.z() < 0.0f, + "Cushion damping force must be upward (-Z) to smoothly decelerate descent"); // 3. Bed penetration (z = 30.1m, delta_z = 0.1m, vz = 0.5 m/s): Penalty spring-damper reaction state.pose.position = Vector3r(0.0f, 0.0f, 30.1f); @@ -197,20 +197,20 @@ namespace airlib const Wrench& reaction_wrench = rov.getBedContactVertex().getWrench(); real_T expected_rebound = params.getParams().bed_stiffness * 0.1f + params.getParams().bed_damping * 0.5f; testAssert(std::abs(reaction_wrench.force.z() - (-expected_rebound)) < 1.0f, - "Bed contact force must match penalty spring-damper model (-kp*delta_z - kd*vz)"); + "Bed contact force must match penalty spring-damper model (-kp*delta_z - kd*vz)"); // 4. Hard safety clamp (z = 31.0m, beyond hard threshold 30.3m): Hard clamp activates state.pose.position = Vector3r(0.0f, 0.0f, 31.0f); state.twist.linear = Vector3r(0.0f, 0.0f, 2.5f); kinematics.setState(state); rov.update(); // triggers enforceDepthClamp() - testAssert(rov.getPose().position.z() <= bed_depth + 1e-3f, - "Hard depth clamp must clamp vehicle position to bed depth"); - testAssert(rov.getTwist().linear.z() <= 0.0f, - "Hard depth clamp must eliminate downward penetration velocity"); + testAssert(rov.getPose().position.z() <= bed_depth + 1e-3f, + "Hard depth clamp must clamp vehicle position to bed depth"); + testAssert(rov.getTwist().linear.z() <= 0.0f, + "Hard depth clamp must eliminate downward penetration velocity"); - std::cout << " [PASS] Depth safety boundary: Soft cushion + Spring-Damper reaction (" - << reaction_wrench.force.z() << " N) + Hard kinematic clamp to " + std::cout << " [PASS] Depth safety boundary: Soft cushion + Spring-Damper reaction (" + << reaction_wrench.force.z() << " N) + Hard kinematic clamp to " << bed_depth << "m successfully verified!" << std::endl; } }; diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index 80f76434..d8fcc9b9 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -40,6 +40,3 @@ int main() return failed == 0 ? 0 : 1; } - - - diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp index abdd8ac3..75309e90 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp @@ -10,24 +10,28 @@ namespace { - struct FShapeParams - { - float Cd; - float AddedMassCoeff; // 相对 ρ·V 的附加质量系数 - }; +struct FShapeParams +{ + float Cd; + float AddedMassCoeff; // 相对 ρ·V 的附加质量系数 +}; - FShapeParams ShapeTable(ERovHullShape Shape) - { - switch (Shape) - { - case ERovHullShape::Sphere: return { 0.47f, 0.50f }; - case ERovHullShape::Box: return { 1.05f, 0.70f }; - case ERovHullShape::Cylinder: return { 0.82f, 0.60f }; - case ERovHullShape::Streamlined: return { 0.08f, 0.10f }; - default: return { 0.50f, 0.50f }; - } +FShapeParams ShapeTable(ERovHullShape Shape) +{ + switch (Shape) { + case ERovHullShape::Sphere: + return { 0.47f, 0.50f }; + case ERovHullShape::Box: + return { 1.05f, 0.70f }; + case ERovHullShape::Cylinder: + return { 0.82f, 0.60f }; + case ERovHullShape::Streamlined: + return { 0.08f, 0.10f }; + default: + return { 0.50f, 0.50f }; } } +} FRovFluidEstimate URovFluidLibrary::EstimateFluidParams(ERovHullShape Shape, FVector Dims, float Density) { @@ -39,8 +43,7 @@ FRovFluidEstimate URovFluidLibrary::EstimateFluidParams(ERovHullShape Shape, FVe const float Y = FMath::Max(Dims.Y, KINDA_SMALL_NUMBER); const float Z = FMath::Max(Dims.Z, KINDA_SMALL_NUMBER); - switch (Shape) - { + switch (Shape) { case ERovHullShape::Sphere: Out.Volume = 4.0f / 3.0f * PI * X * X * X; Out.RefArea = PI * X * X; @@ -75,24 +78,20 @@ bool RovConfig::LoadKeyValues(const FString& Path, TMap& OutValu { OutValues.Reset(); FString Raw; - if (!FFileHelper::LoadFileToString(Raw, *Path)) - { + if (!FFileHelper::LoadFileToString(Raw, *Path)) { OutError = FString::Printf(TEXT("读取失败:%s"), *Path); return false; } TSharedPtr Root; const TSharedRef> Reader = TJsonReaderFactory<>::Create(Raw); - if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) - { + if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) { OutError = FString::Printf(TEXT("JSON 解析失败:%s"), *Path); return false; } - for (const TPair>& Pair : Root->Values) - { - if (!Pair.Value.IsValid() || Pair.Value->Type != EJson::Number) - { + for (const TPair>& Pair : Root->Values) { + if (!Pair.Value.IsValid() || Pair.Value->Type != EJson::Number) { continue; } OutValues.Add(Pair.Key, static_cast(Pair.Value->AsNumber())); @@ -104,30 +103,25 @@ bool RovConfig::LoadNumberArray(const FString& Path, const FString& Field, TArra { OutValues.Reset(); FString Raw; - if (!FFileHelper::LoadFileToString(Raw, *Path)) - { + if (!FFileHelper::LoadFileToString(Raw, *Path)) { OutError = FString::Printf(TEXT("读取失败:%s"), *Path); return false; } TSharedPtr Root; const TSharedRef> Reader = TJsonReaderFactory<>::Create(Raw); - if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) - { + if (!FJsonSerializer::Deserialize(Reader, Root) || !Root.IsValid()) { OutError = FString::Printf(TEXT("JSON 解析失败:%s"), *Path); return false; } const TArray>* Arr = nullptr; - if (!Root->TryGetArrayField(Field, Arr) || !Arr) - { + if (!Root->TryGetArrayField(Field, Arr) || !Arr) { OutError = FString::Printf(TEXT("缺少数组字段 %s"), *Field); return false; } - for (const TSharedPtr& V : *Arr) - { - if (V.IsValid() && V->Type == EJson::Number) - { + for (const TSharedPtr& V : *Arr) { + if (V.IsValid() && V->Type == EJson::Number) { OutValues.Add(static_cast(V->AsNumber())); } } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h index fbcb12c3..116fae1a 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.h @@ -12,10 +12,10 @@ UENUM(BlueprintType) enum class ERovHullShape : uint8 { - Sphere UMETA(DisplayName = "球体"), - Box UMETA(DisplayName = "立方体/长方体"), - Cylinder UMETA(DisplayName = "圆柱"), - Streamlined UMETA(DisplayName = "流线型"), + Sphere UMETA(DisplayName = "球体"), + Box UMETA(DisplayName = "立方体/长方体"), + Cylinder UMETA(DisplayName = "圆柱"), + Streamlined UMETA(DisplayName = "流线型"), }; /** 由外形估算出的流体受力参数。 */ @@ -68,12 +68,12 @@ class AIRSIM_API URovFluidLibrary : public UBlueprintFunctionLibrary /** 配置文件读取。 */ namespace RovConfig { - /** +/** * 读取扁平 JSON 配置文件(键为字符串,值为数值)。 * 数组与嵌套对象会被忽略,由调用方单独解析。 */ - bool LoadKeyValues(const FString& Path, TMap& OutValues, FString& OutError); +bool LoadKeyValues(const FString& Path, TMap& OutValues, FString& OutError); - /** 读取配置文件里的数值数组字段(如 "Obstacles" 之外的一维数组)。 */ - bool LoadNumberArray(const FString& Path, const FString& Field, TArray& OutValues, FString& OutError); +/** 读取配置文件里的数值数组字段(如 "Obstacles" 之外的一维数组)。 */ +bool LoadNumberArray(const FString& Path, const FString& Field, TArray& OutValues, FString& OutError); } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp index 159321b0..127fd6f1 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp @@ -21,7 +21,8 @@ DEFINE_LOG_CATEGORY_STATIC(LogRovBridge, Log, All); -namespace { +namespace +{ /** 用 MuJoCo 四元数 (w,x,y,z) 旋转向量:v' = v + 2*(w*c + q×c),c = q×v */ void RotByQuat(double qw, double qx, double qy, double qz, double vx, double vy, double vz, @@ -53,8 +54,7 @@ void URovMuJoCoBridge::BeginPlay() Super::BeginPlay(); if (!bEnableBridge) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] 已禁用(bEnableBridge=false),AirSim 保持原生物理")); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 已禁用(bEnableBridge=false),AirSim 保持原生物理")); return; } @@ -75,23 +75,18 @@ void URovMuJoCoBridge::BeginPlay() bBridgeActive = true; if (TargetBody.IsValid()) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] ✅ 桥接已启用,跟随 body '%s'(Articulation='%s')"), - *GetResolvedBodyName(), - Articulation.IsValid() ? *Articulation->GetName() : TEXT("")); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] ✅ 桥接已启用,跟随 body '%s'(Articulation='%s')"), *GetResolvedBodyName(), Articulation.IsValid() ? *Articulation->GetName() : TEXT("")); } else { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] 位姿源已注册,但 MuJoCo 模型尚未就绪" - "(body 后缀 '%s')——将在运行中自动重试"), - *TargetBodyNameSuffix); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 位姿源已注册,但 MuJoCo 模型尚未就绪" + "(body 后缀 '%s')——将在运行中自动重试"), + *TargetBodyNameSuffix); } } void URovMuJoCoBridge::EndPlay(const EEndPlayReason::Type EndPlayReason) { - if (ContactLogArchive) - { + if (ContactLogArchive) { ContactLogArchive->Close(); delete ContactLogArchive; ContactLogArchive = nullptr; @@ -123,9 +118,7 @@ void URovMuJoCoBridge::ResolveTargets() const const FString Name = Body->GetName(); if (Name.EndsWith(TargetBodyNameSuffix, ESearchCase::IgnoreCase)) { TargetBody = Body; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] 匹配到 body: '%s'(后缀 '%s')"), - *Name, *TargetBodyNameSuffix); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 匹配到 body: '%s'(后缀 '%s')"), *Name, *TargetBodyNameSuffix); return; } } @@ -133,9 +126,7 @@ void URovMuJoCoBridge::ResolveTargets() const // 有 articulation 但没匹配到 body:只在第一次打印候选,避免刷屏 if (!bResolveWarned && Bodies.Num() > 0) { bResolveWarned = true; - UE_LOG(LogRovBridge, Warning, - TEXT("[RovMuJoCoBridge] 没有 body 以 '%s' 结尾。候选(%d 个):"), - *TargetBodyNameSuffix, Bodies.Num()); + UE_LOG(LogRovBridge, Warning, TEXT("[RovMuJoCoBridge] 没有 body 以 '%s' 结尾。候选(%d 个):"), *TargetBodyNameSuffix, Bodies.Num()); for (UMjBody* Body : Bodies) { if (Body) { UE_LOG(LogRovBridge, Warning, TEXT(" %s"), *Body->GetName()); @@ -174,25 +165,23 @@ bool URovMuJoCoBridge::ProvidePose(FVector& OutPosition, FQuat& OutRotation) con bLoggedPhysicsDiag = true; if (AAMjManager* Mgr = AAMjManager::GetManager()) { const double Timestep = (Mgr->PhysicsEngine && Mgr->PhysicsEngine->m_model) - ? Mgr->PhysicsEngine->m_model->opt.timestep - : -1.0; + ? Mgr->PhysicsEngine->m_model->opt.timestep + : -1.0; // UMjPhysicsEngine 的 bIsPaused 默认是 true(URLab 让用户在 // MjSimulate 控件里手动点播放)。Pawn 要跟随就必须让仿真跑起来。 if (bAutoStartSimulation && !Mgr->IsRunning()) { Mgr->SetPaused(false); - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] MuJoCo 仿真原本处于暂停状态,已自动启动")); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] MuJoCo 仿真原本处于暂停状态,已自动启动")); } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] 物理诊断: IsRunning=%d IsInitialized=%d " - "SimTime=%.4fs timestep=%.4fs SimSpeed=%.0f%%"), - Mgr->IsRunning() ? 1 : 0, - Mgr->IsInitialized() ? 1 : 0, - Mgr->GetSimTime(), - Timestep, - Mgr->PhysicsEngine ? Mgr->PhysicsEngine->SimSpeedPercent : -1.0f); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 物理诊断: IsRunning=%d IsInitialized=%d " + "SimTime=%.4fs timestep=%.4fs SimSpeed=%.0f%%"), + Mgr->IsRunning() ? 1 : 0, + Mgr->IsInitialized() ? 1 : 0, + Mgr->GetSimTime(), + Timestep, + Mgr->PhysicsEngine ? Mgr->PhysicsEngine->SimSpeedPercent : -1.0f); } } @@ -204,13 +193,7 @@ bool URovMuJoCoBridge::ProvidePose(FVector& OutPosition, FQuat& OutRotation) con const FVector PawnPos = Owner ? Owner->GetActorLocation() : FVector::ZeroVector; AAMjManager* Mgr = AAMjManager::GetManager(); const float SimTime = Mgr ? Mgr->GetSimTime() : -1.0f; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] 跟随中 #%d 仿真时间=%.3fs MuJoCo=(%s) Pawn=(%s) 差值=%.2f cm"), - CallCounter, - SimTime, - *OutPosition.ToString(), - *PawnPos.ToString(), - FVector::Dist(OutPosition, PawnPos)); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 跟随中 #%d 仿真时间=%.3fs MuJoCo=(%s) Pawn=(%s) 差值=%.2f cm"), CallCounter, SimTime, *OutPosition.ToString(), *PawnPos.ToString(), FVector::Dist(OutPosition, PawnPos)); } return true; @@ -239,10 +222,10 @@ void URovMuJoCoBridge::DisablePawnPhysics() } } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] 已关闭 Pawn '%s' 的物理与碰撞(%d 个图元)," - "避免与 MuJoCo 双计"), - *Owner->GetName(), DisabledPrimitives); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 已关闭 Pawn '%s' 的物理与碰撞(%d 个图元)," + "避免与 MuJoCo 双计"), + *Owner->GetName(), + DisabledPrimitives); } FString URovMuJoCoBridge::GetResolvedBodyName() const @@ -269,46 +252,49 @@ void URovMuJoCoBridge::TickComponent(float DeltaTime, ELevelTick TickType, void URovMuJoCoBridge::ApplyConfigFile() { - if (!bLoadConfigFile) - { + if (!bLoadConfigFile) { UE_LOG(LogRovBridge, Log, TEXT("[配置] bLoadConfigFile=false,跳过配置文件")); return; } FString Path; - if (!FParse::Value(FCommandLine::Get(), TEXT("RovConfig="), Path)) - { + if (!FParse::Value(FCommandLine::Get(), TEXT("RovConfig="), Path)) { Path = FPaths::Combine(FPaths::ProjectConfigDir(), TEXT("RovParams.json")); } Path = FPaths::ConvertRelativePathToFull(Path); - if (!FPaths::FileExists(Path)) - { + if (!FPaths::FileExists(Path)) { UE_LOG(LogRovBridge, Warning, TEXT("[配置] 未找到配置文件 %s,使用类默认值 + 命令行覆盖"), *Path); return; } TMap Kv; FString Err; - if (!RovConfig::LoadKeyValues(Path, Kv, Err)) - { + if (!RovConfig::LoadKeyValues(Path, Kv, Err)) { UE_LOG(LogRovBridge, Warning, TEXT("[配置] %s"), *Err); return; } - auto ApplyF = [&Kv](const TCHAR* Key, float& Target) - { - if (const float* V = Kv.Find(Key)) { Target = *V; } + auto ApplyF = [&Kv](const TCHAR* Key, float& Target) { + if (const float* V = Kv.Find(Key)) { + Target = *V; + } }; - auto ApplyB = [&Kv](const TCHAR* Key, bool& Target) - { - if (const float* V = Kv.Find(Key)) { Target = (*V) != 0.0f; } + auto ApplyB = [&Kv](const TCHAR* Key, bool& Target) { + if (const float* V = Kv.Find(Key)) { + Target = (*V) != 0.0f; + } }; - auto ApplyV = [&Kv](const TCHAR* Prefix, FVector& Target) - { + auto ApplyV = [&Kv](const TCHAR* Prefix, FVector& Target) { const FString P(Prefix); - if (const float* X = Kv.Find(P + TEXT("X"))) { Target.X = *X; } - if (const float* Y = Kv.Find(P + TEXT("Y"))) { Target.Y = *Y; } - if (const float* Z = Kv.Find(P + TEXT("Z"))) { Target.Z = *Z; } + if (const float* X = Kv.Find(P + TEXT("X"))) { + Target.X = *X; + } + if (const float* Y = Kv.Find(P + TEXT("Y"))) { + Target.Y = *Y; + } + if (const float* Z = Kv.Find(P + TEXT("Z"))) { + Target.Z = *Z; + } }; ApplyF(TEXT("FluidDensity"), FluidDensity); @@ -348,26 +334,21 @@ void URovMuJoCoBridge::ApplyConfigFile() ApplyB(TEXT("AutoEstimateFluidParams"), bAutoEstimateFluidParams); ApplyV(TEXT("HullDims"), HullDims); - if (const float* S = Kv.Find(TEXT("HullShape"))) - { + if (const float* S = Kv.Find(TEXT("HullShape"))) { HullShape = static_cast(FMath::Clamp(FMath::RoundToInt(*S), 0, 3)); } - if (bAutoEstimateFluidParams) - { + if (bAutoEstimateFluidParams) { const FRovFluidEstimate E = URovFluidLibrary::EstimateFluidParams(HullShape, HullDims, FluidDensity); DragCoeff = FVector(E.Cd, E.Cd, E.Cd); DragRefArea = FVector(E.RefArea, E.RefArea, E.RefArea); AddedMass = FVector(E.AddedMass, E.AddedMass, E.AddedMass); bUseExplicitDrag = true; bUseAddedMass = true; - UE_LOG(LogRovBridge, Log, - TEXT("[配置] 外形估算:Cd=%.2f 迎流面积=%.4f m² 附加质量=%.1f kg 排水体积=%.4f m³"), - E.Cd, E.RefArea, E.AddedMass, E.Volume); + UE_LOG(LogRovBridge, Log, TEXT("[配置] 外形估算:Cd=%.2f 迎流面积=%.4f m² 附加质量=%.1f kg 排水体积=%.4f m³"), E.Cd, E.RefArea, E.AddedMass, E.Volume); } ApplyB(TEXT("LogContacts"), bLogContacts); - if (const float* V = Kv.Find(TEXT("ContactLogInterval"))) - { + if (const float* V = Kv.Find(TEXT("ContactLogInterval"))) { ContactLogInterval = FMath::Max(1, FMath::RoundToInt(*V)); } @@ -377,24 +358,20 @@ void URovMuJoCoBridge::ApplyConfigFile() void URovMuJoCoBridge::LogContacts(mjModel_* Model, mjData_* Data) { - if (!bLogContacts || !Model || !Data) - { + if (!bLogContacts || !Model || !Data) { return; } - if (++ContactLogStep % FMath::Max(1, ContactLogInterval) != 0) - { + if (++ContactLogStep % FMath::Max(1, ContactLogInterval) != 0) { return; } - if (!ContactLogArchive) - { + if (!ContactLogArchive) { FString Path = ContactLogPath.IsEmpty() - ? FPaths::Combine(FPaths::ProjectSavedDir(), TEXT("RovContactLog.csv")) - : ContactLogPath; + ? FPaths::Combine(FPaths::ProjectSavedDir(), TEXT("RovContactLog.csv")) + : ContactLogPath; Path = FPaths::ConvertRelativePathToFull(Path); ContactLogArchive = IFileManager::Get().CreateFileWriter(*Path, FILEWRITE_Append); - if (!ContactLogArchive) - { + if (!ContactLogArchive) { UE_LOG(LogRovBridge, Warning, TEXT("[接触日志] 无法写入 %s"), *Path); bLogContacts = false; return; @@ -402,20 +379,17 @@ void URovMuJoCoBridge::LogContacts(mjModel_* Model, mjData_* Data) UE_LOG(LogRovBridge, Log, TEXT("[接触日志] 写入 %s(每 %d 个物理步一条)"), *Path, ContactLogInterval); } - auto WriteLine = [this](const FString& Line) - { + auto WriteLine = [this](const FString& Line) { FTCHARToUTF8 Utf8(*Line); ContactLogArchive->Serialize(static_cast(const_cast(Utf8.Get())), Utf8.Length()); }; - if (!bContactLogHeaderWritten) - { + if (!bContactLogHeaderWritten) { WriteLine(TEXT("time_s,body1,body2,normal_N,pos_x,pos_y,pos_z\n")); bContactLogHeaderWritten = true; } - for (int32 i = 0; i < Data->ncon; ++i) - { + for (int32 i = 0; i < Data->ncon; ++i) { const mjContact& C = Data->contact[i]; mjtNum F6[6] = { 0, 0, 0, 0, 0, 0 }; mj_contactForce(Model, Data, i, F6); @@ -423,12 +397,20 @@ void URovMuJoCoBridge::LogContacts(mjModel_* Model, mjData_* Data) const int32 B1 = Model->geom_bodyid[C.geom1]; const int32 B2 = Model->geom_bodyid[C.geom2]; const FString N1 = (B1 >= 0 && Model->name_bodyadr[B1] >= 0) - ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B1])) : FString(TEXT("world")); + ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B1])) + : FString(TEXT("world")); const FString N2 = (B2 >= 0 && Model->name_bodyadr[B2] >= 0) - ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B2])) : FString(TEXT("world")); + ? FString(UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[B2])) + : FString(TEXT("world")); WriteLine(FString::Printf(TEXT("%.5f,%s,%s,%.4f,%.5f,%.5f,%.5f\n"), - Data->time, *N1, *N2, F6[0], C.pos[0], C.pos[1], C.pos[2])); + Data->time, + *N1, + *N2, + F6[0], + C.pos[0], + C.pos[1], + C.pos[2])); } // 立即落盘,避免进程被强杀时缓冲区内容丢失 @@ -486,35 +468,37 @@ void URovMuJoCoBridge::ApplyControlForce() }; // 配置文件先加载,命令行随后覆盖,保证「命令行 > 配置文件 > 默认值」 ApplyConfigFile(); - DisplacedVolume = FloatParam(TEXT("RovDisplacedVolume="), DisplacedVolume); - BuoyancyScale = FloatParam(TEXT("RovBuoyancyScale="), BuoyancyScale); - BuoyancyArm = FloatParam(TEXT("RovBuoyancyArm="), BuoyancyArm); + DisplacedVolume = FloatParam(TEXT("RovDisplacedVolume="), DisplacedVolume); + BuoyancyScale = FloatParam(TEXT("RovBuoyancyScale="), BuoyancyScale); + BuoyancyArm = FloatParam(TEXT("RovBuoyancyArm="), BuoyancyArm); ArmBuoyancyVolume = FloatParam(TEXT("RovArmBuoyVolume="), ArmBuoyancyVolume); - ArmBuoyancyScale = FloatParam(TEXT("RovArmBuoyScale="), ArmBuoyancyScale); + ArmBuoyancyScale = FloatParam(TEXT("RovArmBuoyScale="), ArmBuoyancyScale); AngularDampingCoeff = FloatParam(TEXT("RovAngularDamping="), AngularDampingCoeff); ArmJointDamping = FloatParam(TEXT("RovArmDamping="), ArmJointDamping); ThrusterMaxRatio = FloatParam(TEXT("RovThrustRatio="), ThrusterMaxRatio); - WaterSurfaceZ = FloatParam(TEXT("RovWaterSurfaceZ="), WaterSurfaceZ); - if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { bEnableAttitudeHold = false; } - if (FParse::Param(FCommandLine::Get(), TEXT("RovContactTest"))) - { + WaterSurfaceZ = FloatParam(TEXT("RovWaterSurfaceZ="), WaterSurfaceZ); + if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { + bEnableAttitudeHold = false; + } + if (FParse::Param(FCommandLine::Get(), TEXT("RovContactTest"))) { ContactTestForce = 900.0; bLogContacts = true; bEnableDepthHold = false; UE_LOG(LogRovBridge, Log, TEXT("[接触试验] 施加 %.0f N 下压力,用于产生海床接触"), ContactTestForce); } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][调参] 排水体积=%.4f m³ 浮力倍率=%.2f 臂排水=%.2f L×%.2f " - "推力比=%.2f 水面Z=%.2f m"), - DisplacedVolume, BuoyancyScale, ArmBuoyancyVolume, ArmBuoyancyScale, - ThrusterMaxRatio, WaterSurfaceZ); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][调参] 排水体积=%.4f m³ 浮力倍率=%.2f 臂排水=%.2f L×%.2f " + "推力比=%.2f 水面Z=%.2f m"), + DisplacedVolume, + BuoyancyScale, + ArmBuoyancyVolume, + ArmBuoyancyScale, + ThrusterMaxRatio, + WaterSurfaceZ); if (bUseNativeFluid) { Model->opt.density = FluidDensity; Model->opt.viscosity = FluidViscosity; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][流体] 已启用 MuJoCo 椭球流体模型 density=%.1f kg/m³ viscosity=%.6f Pa·s"), - Model->opt.density, Model->opt.viscosity); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][流体] 已启用 MuJoCo 椭球流体模型 density=%.1f kg/m³ viscosity=%.6f Pa·s"), Model->opt.density, Model->opt.viscosity); } else { Model->opt.density = 0.0; @@ -524,27 +508,21 @@ void URovMuJoCoBridge::ApplyControlForce() // 质量诊断:确认 UE 里这个模型到底多重 const double TotalMass = Model->body_subtreemass[BodyId]; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][质量] 目标 body id=%d body_mass=%.3f kg body_subtreemass=%.3f kg nbody=%d"), - BodyId, Model->body_mass[BodyId], TotalMass, Model->nbody); - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][惯量] body_inertia=(%.5f, %.5f, %.5f) kg·m² " - "(角阻尼 Kd = %.1f × 平均惯量 %.5f = %.3f N·m·s/rad)"), - Model->body_inertia[3 * BodyId + 0], - Model->body_inertia[3 * BodyId + 1], - Model->body_inertia[3 * BodyId + 2], - AngularDampingCoeff, - (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] - + Model->body_inertia[3 * BodyId + 2]) / 3.0, - AngularDampingCoeff * (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] - + Model->body_inertia[3 * BodyId + 2]) / 3.0); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][质量] 目标 body id=%d body_mass=%.3f kg body_subtreemass=%.3f kg nbody=%d"), BodyId, Model->body_mass[BodyId], TotalMass, Model->nbody); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][惯量] body_inertia=(%.5f, %.5f, %.5f) kg·m² " + "(角阻尼 Kd = %.1f × 平均惯量 %.5f = %.3f N·m·s/rad)"), + Model->body_inertia[3 * BodyId + 0], + Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + AngularDampingCoeff, + (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + Model->body_inertia[3 * BodyId + 2]) / 3.0, + AngularDampingCoeff * (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + Model->body_inertia[3 * BodyId + 2]) / 3.0); for (int32 i = 1; i < Model->nbody; ++i) { if (Model->body_mass[i] > 0.5) { const char* Name = (Model->names && Model->name_bodyadr[i] >= 0) - ? (Model->names + Model->name_bodyadr[i]) : "?"; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][质量] body[%2d] %-24s mass=%.3f subtreemass=%.3f"), - i, ANSI_TO_TCHAR(Name), Model->body_mass[i], Model->body_subtreemass[i]); + ? (Model->names + Model->name_bodyadr[i]) + : "?"; + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][质量] body[%2d] %-24s mass=%.3f subtreemass=%.3f"), i, ANSI_TO_TCHAR(Name), Model->body_mass[i], Model->body_subtreemass[i]); } } @@ -567,9 +545,7 @@ void URovMuJoCoBridge::ApplyControlForce() Model->jnt_stiffness[j] = (mjtNum)ArmJointStiffness; ++StiffJoints; } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][机械臂] %d 个关节:阻尼 %.1f N·m·s/rad、刚度 %.1f N·m/rad(%d 个自由度)"), - StiffJoints, ArmJointDamping, ArmJointStiffness, DampedDofs); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][机械臂] %d 个关节:阻尼 %.1f N·m·s/rad、刚度 %.1f N·m/rad(%d 个自由度)"), StiffJoints, ArmJointDamping, ArmJointStiffness, DampedDofs); } // 机械臂重力补偿:抵消其自重,消除对艇体的恒定倾覆力矩 @@ -577,14 +553,12 @@ void URovMuJoCoBridge::ApplyControlForce() int32 CompBodies = 0; for (int32 b = 1; b < Model->nbody; ++b) { if (b == BodyId) { - continue; // ROV 本体不补偿 + continue; // ROV 本体不补偿 } Model->body_gravcomp[b] = 1.0; ++CompBodies; } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][机械臂] 已对 %d 个 body 做重力补偿(gravcomp=1.0)"), - CompBodies); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][机械臂] 已对 %d 个 body 做重力补偿(gravcomp=1.0)"), CompBodies); } // 复合惯量:先用本体惯量兜底,等仿真真正跑起来再换成 6×6 质量阵的值。 @@ -596,60 +570,66 @@ void URovMuJoCoBridge::ApplyControlForce() // 洋流模型初始化(三层级联) if (FParse::Param(FCommandLine::Get(), TEXT("RovOceanCurrent"))) { - bEnableOceanCurrent = true; // 命令行强制开启,便于测试 + bEnableOceanCurrent = true; // 命令行强制开启,便于测试 } if (FParse::Param(FCommandLine::Get(), TEXT("RovNoAttitude"))) { - bEnableAttitudeHold = false; // 自检时关掉姿态控制器,避免干扰 + bEnableAttitudeHold = false; // 自检时关掉姿态控制器,避免干扰 } OceanCurrent.bEnabled = bEnableOceanCurrent; OceanCurrent.MeanSpeed = OceanCurrentMeanSpeed; OceanCurrent.Seed = 42; OceanCurrent.Initialize(); - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][洋流] %s 平均流速=%.2f m/s 分层=%d 层 湍流强度=%.2f"), - bEnableOceanCurrent ? TEXT("已启用") : TEXT("已关闭"), - OceanCurrentMeanSpeed, OceanCurrent.Layers.Num(), OceanCurrent.TurbIntensity); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][洋流] %s 平均流速=%.2f m/s 分层=%d 层 湍流强度=%.2f"), bEnableOceanCurrent ? TEXT("已启用") : TEXT("已关闭"), OceanCurrentMeanSpeed, OceanCurrent.Layers.Num(), OceanCurrent.TurbIntensity); // 单位诊断:确认 MuJoCo 模型到底是米还是厘米 { int32 RovGeomId = -1; for (int32 g = 0; g < Model->ngeom; ++g) { const char* GName = (Model->names && Model->name_geomadr[g] >= 0) - ? (Model->names + Model->name_geomadr[g]) : ""; + ? (Model->names + Model->name_geomadr[g]) + : ""; if (FString(ANSI_TO_TCHAR(GName)).EndsWith(TEXT("rov_body"))) { RovGeomId = g; break; } } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][单位] gravity=%.3f timestep=%.5f stat.extent=%.3f " - "模型包围盒中心=(%.2f,%.2f,%.2f)"), - Model->opt.gravity[2], Model->opt.timestep, Model->stat.extent, - Model->stat.center[0], Model->stat.center[1], Model->stat.center[2]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][单位] gravity=%.3f timestep=%.5f stat.extent=%.3f " + "模型包围盒中心=(%.2f,%.2f,%.2f)"), + Model->opt.gravity[2], + Model->opt.timestep, + Model->stat.extent, + Model->stat.center[0], + Model->stat.center[1], + Model->stat.center[2]); if (RovGeomId >= 0) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][单位] rov_body geom size=(%.4f, %.4f, %.4f) " - "(若≈0.4/0.3/0.2 则模型是米;若≈40/30/20 则是厘米)"), - Model->geom_size[3 * RovGeomId + 0], - Model->geom_size[3 * RovGeomId + 1], - Model->geom_size[3 * RovGeomId + 2]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][单位] rov_body geom size=(%.4f, %.4f, %.4f) " + "(若≈0.4/0.3/0.2 则模型是米;若≈40/30/20 则是厘米)"), + Model->geom_size[3 * RovGeomId + 0], + Model->geom_size[3 * RovGeomId + 1], + Model->geom_size[3 * RovGeomId + 2]); } if (AActor* Owner = GetOwner()) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][单位] Pawn 世界坐标=(%.2f, %.2f, %.2f) cm " - "MuJoCo xpos=(%.2f, %.2f, %.2f)"), - Owner->GetActorLocation().X, Owner->GetActorLocation().Y, Owner->GetActorLocation().Z, - Data->xpos[3 * BodyId + 0], Data->xpos[3 * BodyId + 1], Data->xpos[3 * BodyId + 2]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][单位] Pawn 世界坐标=(%.2f, %.2f, %.2f) cm " + "MuJoCo xpos=(%.2f, %.2f, %.2f)"), + Owner->GetActorLocation().X, + Owner->GetActorLocation().Y, + Owner->GetActorLocation().Z, + Data->xpos[3 * BodyId + 0], + Data->xpos[3 * BodyId + 1], + Data->xpos[3 * BodyId + 2]); } } // 受力诊断:三个数摆出来,一眼能看出浮沉 const double Weight = TotalMass * Gravity; const double Buoyancy = FluidDensity * Gravity * DisplacedVolume * BuoyancyScale; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][受力] 重力=%.1f N 浮力=%.1f N 净力=%+.1f N(正=上浮,负=下沉) " - "排水体积=%.4f m³ 浮力倍率=%.2f"), - Weight, Buoyancy, Buoyancy - Weight, DisplacedVolume, BuoyancyScale); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][受力] 重力=%.1f N 浮力=%.1f N 净力=%+.1f N(正=上浮,负=下沉) " + "排水体积=%.4f m³ 浮力倍率=%.2f"), + Weight, + Buoyancy, + Buoyancy - Weight, + DisplacedVolume, + BuoyancyScale); } // 复合惯量延迟计算:qM 只在 mj_forward/mj_step 之后有效, @@ -668,18 +648,25 @@ void URovMuJoCoBridge::ApplyControlForce() for (int32 a = 0; a < 3; ++a) { const double V = M[(3 + a) * Nv + (3 + a)]; // 这台 ROV 的转动惯量量级是几十 kg·m²,小于 1 一律视为无效 - if (!FMath::IsFinite(V) || V < 1.0) { bOk = false; break; } + if (!FMath::IsFinite(V) || V < 1.0) { + bOk = false; + break; + } Vals[a] = V; } } if (bOk) { - for (int32 a = 0; a < 3; ++a) { EffInertia[a] = Vals[a]; } + for (int32 a = 0; a < 3; ++a) { + EffInertia[a] = Vals[a]; + } } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][复合惯量] 延迟计算 t=%.2f " - "EffInertia=(%.3f, %.3f, %.3f) 来源=%s"), - Data->time, EffInertia[0], EffInertia[1], EffInertia[2], - bOk ? TEXT("6x6 质量阵") : TEXT("body_inertia 回退")); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][复合惯量] 延迟计算 t=%.2f " + "EffInertia=(%.3f, %.3f, %.3f) 来源=%s"), + Data->time, + EffInertia[0], + EffInertia[1], + EffInertia[2], + bOk ? TEXT("6x6 质量阵") : TEXT("body_inertia 回退")); } } @@ -692,12 +679,24 @@ void URovMuJoCoBridge::ApplyControlForce() // 注意:A/D 的符号和直觉相反——这个模型的 Y 轴朝左,所以 A 用 +1、D 用 -1。 // 垂直用 Q/E(PageUp/PageDown 在笔记本上常常没有,保留作别名)。 double LocalX = 0.0, LocalY = 0.0, LocalZ = 0.0; - if (PC->IsInputKeyDown(EKeys::W)) { LocalX += 1.0; } - if (PC->IsInputKeyDown(EKeys::S)) { LocalX -= 1.0; } - if (PC->IsInputKeyDown(EKeys::A)) { LocalY += 1.0; } - if (PC->IsInputKeyDown(EKeys::D)) { LocalY -= 1.0; } - if (PC->IsInputKeyDown(EKeys::Q) || PC->IsInputKeyDown(EKeys::PageUp)) { LocalZ += 1.0; } - if (PC->IsInputKeyDown(EKeys::E) || PC->IsInputKeyDown(EKeys::PageDown)) { LocalZ -= 1.0; } + if (PC->IsInputKeyDown(EKeys::W)) { + LocalX += 1.0; + } + if (PC->IsInputKeyDown(EKeys::S)) { + LocalX -= 1.0; + } + if (PC->IsInputKeyDown(EKeys::A)) { + LocalY += 1.0; + } + if (PC->IsInputKeyDown(EKeys::D)) { + LocalY -= 1.0; + } + if (PC->IsInputKeyDown(EKeys::Q) || PC->IsInputKeyDown(EKeys::PageUp)) { + LocalZ += 1.0; + } + if (PC->IsInputKeyDown(EKeys::E) || PC->IsInputKeyDown(EKeys::PageDown)) { + LocalZ -= 1.0; + } // 按键诊断:状态一变就记一条(不节流),用来确认输入到底有没有进到游戏 { @@ -721,9 +720,7 @@ void URovMuJoCoBridge::ApplyControlForce() (IsDown(EKeys::E) ? 32 : 0) | (IsDown(EKeys::Z) ? 64 : 0); if (KeyMask != PrevKeyMask) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][按键] 状态 %d -> %d (W=1 S=2 A=4 D=8 Q=16 E=32 Z=64)"), - PrevKeyMask, KeyMask); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][按键] 状态 %d -> %d (W=1 S=2 A=4 D=8 Q=16 E=32 Z=64)"), PrevKeyMask, KeyMask); PrevKeyMask = KeyMask; } @@ -733,14 +730,13 @@ void URovMuJoCoBridge::ApplyControlForce() const int32 CurTick = (int32)NowSec; if (CurTick != FocusLogTick) { FocusLogTick = CurTick; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][焦点] IsLocalController=%d PlayerInput=%s " - "bShowMouseCursor=%d bIgnoreInput=%d 按键掩码=%d"), - PC->IsLocalController() ? 1 : 0, - PC->PlayerInput ? TEXT("valid") : TEXT("NULL"), - PC->bShowMouseCursor ? 1 : 0, - PC->IsInputKeyDown(EKeys::LeftMouseButton) ? 1 : 0, - KeyMask); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][焦点] IsLocalController=%d PlayerInput=%s " + "bShowMouseCursor=%d bIgnoreInput=%d 按键掩码=%d"), + PC->IsLocalController() ? 1 : 0, + PC->PlayerInput ? TEXT("valid") : TEXT("NULL"), + PC->bShowMouseCursor ? 1 : 0, + PC->IsInputKeyDown(EKeys::LeftMouseButton) ? 1 : 0, + KeyMask); } } @@ -754,8 +750,7 @@ void URovMuJoCoBridge::ApplyControlForce() const double Now = W ? W->GetTimeSeconds() : 0.0; if (AutoTestStartTime < 0.0) { AutoTestStartTime = Now; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][自检] 开始:上浮 2 秒,起始 Z=%.3f"), Data->xpos[3 * BodyId + 2]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][自检] 开始:上浮 2 秒,起始 Z=%.3f"), Data->xpos[3 * BodyId + 2]); } if (Now - AutoTestStartTime < 10.0) { LocalZ = 1.0; @@ -763,10 +758,7 @@ void URovMuJoCoBridge::ApplyControlForce() const int32 Tick = (int32)((Now - AutoTestStartTime) * 2.0); if (Tick != AutoTestLastTick) { AutoTestLastTick = Tick; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][自检] t=%.1fs Z=%.3f 总Fz=%.1f N vz=%.3f"), - Now - AutoTestStartTime, Data->xpos[3 * BodyId + 2], - CachedForceZ.load(), Data->cvel[6 * BodyId + 5]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][自检] t=%.1fs Z=%.3f 总Fz=%.1f N vz=%.3f"), Now - AutoTestStartTime, Data->xpos[3 * BodyId + 2], CachedForceZ.load(), Data->cvel[6 * BodyId + 5]); } } @@ -775,7 +767,7 @@ void URovMuJoCoBridge::ApplyControlForce() return; } - const mjtNum* Q = &Data->xquat[4 * BodyId]; // (w, x, y, z) + const mjtNum* Q = &Data->xquat[4 * BodyId]; // (w, x, y, z) const double Qw = Q[0], Qx = Q[1], Qy = Q[2], Qz = Q[3]; // 只取偏航角,忽略俯仰和横滚 —— 否则 ROV 一翻滚推力方向就乱 @@ -806,9 +798,9 @@ void URovMuJoCoBridge::ApplyControlForce() // 方向按画面定义:MuJoCo→UE 为镜像映射(Y 取反),MuJoCo 的 +yaw 在画面上是 -yaw, // 因此 → = 画面右转 = MuJoCo yaw 减小。 // 按住期间同时给出角速度前馈,避免目标航向跑得比机身快导致误差累积过 180°。 - double YawCmdRateDeg = 0.0; // 本帧航向角速度指令(deg/s,MuJoCo 符号) + double YawCmdRateDeg = 0.0; // 本帧航向角速度指令(deg/s,MuJoCo 符号) if (bEnableAttitudeHold && PC) { - const bool bYawLeft = PC->IsInputKeyDown(EKeys::Left); + const bool bYawLeft = PC->IsInputKeyDown(EKeys::Left); const bool bYawRight = PC->IsInputKeyDown(EKeys::Right); if (bYawLeft != bYawRight) { YawCmdRateDeg = bYawLeft ? +(double)YawRateDegPerSec @@ -828,13 +820,15 @@ void URovMuJoCoBridge::ApplyControlForce() if (YawCmdRateDeg != 0.0) { TargetYawDeg += (float)(YawCmdRateDeg * FrameDt); - while (TargetYawDeg > 180.0f) { TargetYawDeg -= 360.0f; } - while (TargetYawDeg < -180.0f) { TargetYawDeg += 360.0f; } + while (TargetYawDeg > 180.0f) { + TargetYawDeg -= 360.0f; + } + while (TargetYawDeg < -180.0f) { + TargetYawDeg += 360.0f; + } static int32 YawLogCounter = 0; if ((++YawLogCounter % 30) == 0) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][航向] 目标航向 = %.1f° 指令角速度 = %+.0f°/s(←/→)"), - TargetYawDeg, YawCmdRateDeg); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][航向] 目标航向 = %.1f° 指令角速度 = %+.0f°/s(←/→)"), TargetYawDeg, YawCmdRateDeg); } } @@ -853,9 +847,7 @@ void URovMuJoCoBridge::ApplyControlForce() if (bDepthHoldActive) { DepthHoldTargetZ = RovZ; } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][定深] %s(目标 Z=%.3f m)"), - bDepthHoldActive ? TEXT("已锁定") : TEXT("已解除"), DepthHoldTargetZ); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深] %s(目标 Z=%.3f m)"), bDepthHoldActive ? TEXT("已锁定") : TEXT("已解除"), DepthHoldTargetZ); } bPrevDepthKeyDown = bDepthKeyDown; } @@ -881,8 +873,7 @@ void URovMuJoCoBridge::ApplyControlForce() bDepthHoldActive = true; DepthPID.Reset(); DepthHoldTargetZ = RovZ; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][定深自检] 锁定 Z=%.3f m"), DepthHoldTargetZ); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深自检] 锁定 Z=%.3f m"), DepthHoldTargetZ); } } @@ -904,7 +895,7 @@ void URovMuJoCoBridge::ApplyControlForce() Data->qpos[QPosAdr + 0] = 0.0; Data->qpos[QPosAdr + 1] = 0.0; Data->qpos[QPosAdr + 2] = -5.0; - Data->qpos[QPosAdr + 3] = 1.0; // quat w + Data->qpos[QPosAdr + 3] = 1.0; // quat w Data->qpos[QPosAdr + 4] = 0.0; Data->qpos[QPosAdr + 5] = 0.0; Data->qpos[QPosAdr + 6] = 0.0; @@ -914,15 +905,14 @@ void URovMuJoCoBridge::ApplyControlForce() Data->qvel[SpinDof + 3] = 3.0; Data->qvel[SpinDof + 4] = 3.0; Data->qvel[SpinDof + 5] = 3.0; - DepthHoldTargetZ = -5.0; // 定深跟着走,别把 ROV 拉回出生高度 + DepthHoldTargetZ = -5.0; // 定深跟着走,别把 ROV 拉回出生高度 // 关掉浮心高度:BG≠0 会形成"单摆"回复力矩,角速度会来回摆, // 没法拟合成指数衰减。自检只要纯阻尼,浮力大小不变(仍然中性附近)。 BuoyancyArm = 0.0f; bDepthHoldActive = false; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][旋转自检] 移至水中 Z=-5m,施加 3 rad/s 三轴角速度脉冲" - "(dof_damping=%.2f)"), - (double)Model->dof_damping[SpinDof + 3]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][旋转自检] 移至水中 Z=-5m,施加 3 rad/s 三轴角速度脉冲" + "(dof_damping=%.2f)"), + (double)Model->dof_damping[SpinDof + 3]); } if (bSpinApplied) { const int32 Tick = (int32)(Now * 4.0); @@ -931,10 +921,7 @@ void URovMuJoCoBridge::ApplyControlForce() const double Wx = Data->qvel[SpinDof + 3]; const double Wy = Data->qvel[SpinDof + 4]; const double Wz = Data->qvel[SpinDof + 5]; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][旋转自检] t=%.2fs wx=%.4f wy=%.4f wz=%.4f |w|=%.4f rad/s"), - Now - 2.0, Wx, Wy, Wz, - FMath::Sqrt(Wx * Wx + Wy * Wy + Wz * Wz)); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][旋转自检] t=%.2fs wx=%.4f wy=%.4f wz=%.4f |w|=%.4f rad/s"), Now - 2.0, Wx, Wy, Wz, FMath::Sqrt(Wx * Wx + Wy * Wy + Wz * Wz)); } } } @@ -954,8 +941,8 @@ void URovMuJoCoBridge::ApplyControlForce() if (!bTiltApplied && Now > 2.0) { bTiltApplied = true; - const double HalfAngle = 10.0 * PI / 180.0; // 20° 的一半 - Data->qpos[TQPos + 2] = 4.0; // 抬到水中,避免接触干扰 + const double HalfAngle = 10.0 * PI / 180.0; // 20° 的一半 + Data->qpos[TQPos + 2] = 4.0; // 抬到水中,避免接触干扰 Data->qpos[TQPos + 3] = FMath::Cos(HalfAngle); // w Data->qpos[TQPos + 4] = FMath::Sin(HalfAngle); // x(绕 X 轴倾斜) Data->qpos[TQPos + 5] = 0.0; @@ -963,8 +950,7 @@ void URovMuJoCoBridge::ApplyControlForce() for (int32 k = 0; k < 6; ++k) { Data->qvel[TDof + k] = 0.0; } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][倾斜自检] 已倾斜 20° 并释放(BG=%.3f m)"), BuoyancyArm); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][倾斜自检] 已倾斜 20° 并释放(BG=%.3f m)"), BuoyancyArm); } if (bTiltApplied) { @@ -980,9 +966,7 @@ void URovMuJoCoBridge::ApplyControlForce() const double UpZ = 1.0 - 2.0 * (Qx2 * Qx2 + Qy2 * Qy2); const double TiltDeg = FMath::RadiansToDegrees( FMath::Acos(FMath::Clamp(UpZ, -1.0, 1.0))); - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][倾斜自检] t=%.2fs 倾角=%.2f° ω=%.4f rad/s"), - Now - 2.0, TiltDeg, Data->qvel[TDof + 4]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][倾斜自检] t=%.2fs 倾角=%.2f° ω=%.4f rad/s"), Now - 2.0, TiltDeg, Data->qvel[TDof + 4]); } } } @@ -996,11 +980,11 @@ void URovMuJoCoBridge::ApplyControlForce() int32 V = 0; if (FParse::Value(FCommandLine::Get(), TEXT("RovTorqueAxis="), V)) { TorqueTestAxis = V; - } else { + } + else { TorqueTestAxis = 0; } - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][力矩轴自检] 将只给轴 %d 施加 100 N·m"), TorqueTestAxis); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][力矩轴自检] 将只给轴 %d 施加 100 N·m"), TorqueTestAxis); } if (bDepthHoldActive) { @@ -1015,10 +999,14 @@ void URovMuJoCoBridge::ApplyControlForce() const int32 Tick = (int32)(Now * 2.0); if (Tick != DepthTestLastTick) { DepthTestLastTick = Tick; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][定深自检] t=%.1fs 目标=%.3f 当前=%.3f 误差=%+.3f m " - "PID推力=%.1f N 洋流Vz=%.3f m/s"), - Now, DepthHoldTargetZ, RovZ, DepthHoldTargetZ - RovZ, ThrusterZ, VCurrent.Z); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][定深自检] t=%.1fs 目标=%.3f 当前=%.3f 误差=%+.3f m " + "PID推力=%.1f N 洋流Vz=%.3f m/s"), + Now, + DepthHoldTargetZ, + RovZ, + DepthHoldTargetZ - RovZ, + ThrusterZ, + VCurrent.Z); } } } @@ -1060,12 +1048,19 @@ void URovMuJoCoBridge::ApplyControlForce() const int32 Tick = (int32)(GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0); if (Tick != OceanLogTick) { OceanLogTick = Tick; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][洋流诊断] V_current=(%.4f,%.4f,%.4f) |Vc|=%.4f " - "V_robot=(%.4f,%.4f,%.4f) |Vr|=%.4f F=(%.1f,%.1f,%.1f) N"), - VCurrent.X, VCurrent.Y, VCurrent.Z, VCurrent.Size(), - Vrx, Vry, Vrz, FMath::Sqrt(Vrx * Vrx + Vry * Vry + Vrz * Vrz), - CurrentX, CurrentY, CurrentZ); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][洋流诊断] V_current=(%.4f,%.4f,%.4f) |Vc|=%.4f " + "V_robot=(%.4f,%.4f,%.4f) |Vr|=%.4f F=(%.1f,%.1f,%.1f) N"), + VCurrent.X, + VCurrent.Y, + VCurrent.Z, + VCurrent.Size(), + Vrx, + Vry, + Vrz, + FMath::Sqrt(Vrx * Vrx + Vry * Vry + Vrz * Vrz), + CurrentX, + CurrentY, + CurrentZ); } } } @@ -1074,7 +1069,7 @@ void URovMuJoCoBridge::ApplyControlForce() const double Fx = ThrusterX + CurrentX; const double Fy = ThrusterY + CurrentY; - const double Fz = ThrusterZ + CurrentZ; // 浮力单独走 mj_applyFT(作用在浮心) + const double Fz = ThrusterZ + CurrentZ; // 浮力单独走 mj_applyFT(作用在浮心) // 【8】浮力的回复力矩由 mj_applyFT 在浮心处施加时自动产生,这里不再手算。 @@ -1108,9 +1103,7 @@ void URovMuJoCoBridge::ApplyControlForce() const double InitYaw = FMath::Atan2(2.0 * (Qw * Qz + Qx * Qy), 1.0 - 2.0 * (Qy * Qy + Qz * Qz)); TargetYawDeg = FMath::RadiansToDegrees(InitYaw); - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][姿态] 已锁定目标航向 %.1f°,带宽 %.2f rad/s,阻尼比 %.2f"), - TargetYawDeg, AttitudeBandwidth, AttitudeZeta); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][姿态] 已锁定目标航向 %.1f°,带宽 %.2f rad/s,阻尼比 %.2f"), TargetYawDeg, AttitudeBandwidth, AttitudeZeta); } const double HalfYaw = FMath::DegreesToRadians(TargetYawDeg) * 0.5; @@ -1118,9 +1111,7 @@ void URovMuJoCoBridge::ApplyControlForce() const double Qtz = FMath::Sin(HalfYaw); // q_e = conj(q_c) ⊗ q_t (机体系误差) - auto QMul = [](double aw, double ax, double ay, double az, - double bw, double bx, double by, double bz, - double& ow, double& ox, double& oy, double& oz) { + auto QMul = [](double aw, double ax, double ay, double az, double bw, double bx, double by, double bz, double& ow, double& ox, double& oy, double& oz) { ow = aw * bw - ax * bx - ay * by - az * bz; ox = aw * bx + ax * bw + ay * bz - az * by; oy = aw * by - ax * bz + ay * bw + az * bx; @@ -1128,8 +1119,11 @@ void URovMuJoCoBridge::ApplyControlForce() }; double Qew = 0.0, Qex = 0.0, Qey = 0.0, Qez = 0.0; QMul(Qw, -Qx, -Qy, -Qz, Qtw, 0.0, 0.0, Qtz, Qew, Qex, Qey, Qez); - if (Qew < 0.0) { // 取短弧 - Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; + if (Qew < 0.0) { // 取短弧 + Qew = -Qew; + Qex = -Qex; + Qey = -Qey; + Qez = -Qez; } const double E[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; @@ -1159,22 +1153,24 @@ void URovMuJoCoBridge::ApplyControlForce() double vz = (a == 2) ? 1.0 : 0.0; double px = 0.0, py = 0.0, pz = 0.0; RotByQuat(IQ[0], -IQ[1], -IQ[2], -IQ[3], vx, vy, vz, px, py, pz); - Iaxis[a] = Model->body_inertia[3 * BodyId + 0] * px * px - + Model->body_inertia[3 * BodyId + 1] * py * py - + Model->body_inertia[3 * BodyId + 2] * pz * pz; + Iaxis[a] = Model->body_inertia[3 * BodyId + 0] * px * px + Model->body_inertia[3 * BodyId + 1] * py * py + Model->body_inertia[3 * BodyId + 2] * pz * pz; } static bool bLoggedIquat = false; if (!bLoggedIquat) { bLoggedIquat = true; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][惯量诊断] body_iquat=(%.4f, %.4f, %.4f, %.4f) " - "主轴主值=(%.3f, %.3f, %.3f) 机体轴等效惯量=(%.3f, %.3f, %.3f)"), - IQ[0], IQ[1], IQ[2], IQ[3], - Model->body_inertia[3 * BodyId + 0], - Model->body_inertia[3 * BodyId + 1], - Model->body_inertia[3 * BodyId + 2], - Iaxis[0], Iaxis[1], Iaxis[2]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][惯量诊断] body_iquat=(%.4f, %.4f, %.4f, %.4f) " + "主轴主值=(%.3f, %.3f, %.3f) 机体轴等效惯量=(%.3f, %.3f, %.3f)"), + IQ[0], + IQ[1], + IQ[2], + IQ[3], + Model->body_inertia[3 * BodyId + 0], + Model->body_inertia[3 * BodyId + 1], + Model->body_inertia[3 * BodyId + 2], + Iaxis[0], + Iaxis[1], + Iaxis[2]); } // 姿态诊断:每秒打印一次欧拉角、目标、误差、角速度和力矩 @@ -1183,13 +1179,10 @@ void URovMuJoCoBridge::ApplyControlForce() const int32 TickNow = (int32)(GetWorld()->GetTimeSeconds()); if (TickNow != AttLogTick) { AttLogTick = TickNow; - const double RollD = FMath::RadiansToDegrees(FMath::Atan2(2.0*(Qw*Qx + Qy*Qz), 1.0 - 2.0*(Qx*Qx + Qy*Qy))); - const double PitchD = FMath::RadiansToDegrees(FMath::Asin(FMath::Clamp(2.0*(Qw*Qy - Qz*Qx), -1.0, 1.0))); - const double YawD = FMath::RadiansToDegrees(FMath::Atan2(2.0*(Qw*Qz + Qx*Qy), 1.0 - 2.0*(Qy*Qy + Qz*Qz))); - UE_LOG(LogRovBridge, Log, - TEXT("[%s][姿态诊断] roll=%.1f pitch=%.1f yaw=%.1f targetYaw=%.1f |E|=(%.3f,%.3f,%.3f) omega=(%.2f,%.2f,%.2f)") - , TEXT("RovMuJoCoBridge"), RollD, PitchD, YawD, TargetYawDeg, E[0], E[1], E[2], - Data->qvel[AttDof+3], Data->qvel[AttDof+4], Data->qvel[AttDof+5]); + const double RollD = FMath::RadiansToDegrees(FMath::Atan2(2.0 * (Qw * Qx + Qy * Qz), 1.0 - 2.0 * (Qx * Qx + Qy * Qy))); + const double PitchD = FMath::RadiansToDegrees(FMath::Asin(FMath::Clamp(2.0 * (Qw * Qy - Qz * Qx), -1.0, 1.0))); + const double YawD = FMath::RadiansToDegrees(FMath::Atan2(2.0 * (Qw * Qz + Qx * Qy), 1.0 - 2.0 * (Qy * Qy + Qz * Qz))); + UE_LOG(LogRovBridge, Log, TEXT("[%s][姿态诊断] roll=%.1f pitch=%.1f yaw=%.1f targetYaw=%.1f |E|=(%.3f,%.3f,%.3f) omega=(%.2f,%.2f,%.2f)"), TEXT("RovMuJoCoBridge"), RollD, PitchD, YawD, TargetYawDeg, E[0], E[1], E[2], Data->qvel[AttDof + 3], Data->qvel[AttDof + 4], Data->qvel[AttDof + 5]); } } @@ -1201,10 +1194,16 @@ void URovMuJoCoBridge::ApplyControlForce() const double Ki = I * W * W * W * KiF; const double Omega = Data->qvel[AttDof + 3 + Axis]; double T = Kp * E[Axis] + Ki * IntArr[Axis] - Kd * Omega; - T = FMath::Clamp(T, -1500.0, 1500.0); // 力矩限幅,防止饱和发散 - if (Axis == 0) { TorqueX += T; } - else if (Axis == 1) { TorqueY += T; } - else { TorqueZ += T; } + T = FMath::Clamp(T, -1500.0, 1500.0); // 力矩限幅,防止饱和发散 + if (Axis == 0) { + TorqueX += T; + } + else if (Axis == 1) { + TorqueY += T; + } + else { + TorqueZ += T; + } } } } @@ -1226,8 +1225,7 @@ void URovMuJoCoBridge::ApplyControlForce() const double InitYaw = FMath::Atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz)); TargetYawDeg = FMath::RadiansToDegrees(InitYaw); - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][姿态] 级联控制启用,锁定航向 %.1f°"), TargetYawDeg); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][姿态] 级联控制启用,锁定航向 %.1f°"), TargetYawDeg); } const double HalfYaw = FMath::DegreesToRadians(TargetYawDeg) * 0.5; @@ -1240,11 +1238,16 @@ void URovMuJoCoBridge::ApplyControlForce() double Qex = aw * 0.0 + ax * Qtw + ay * Qtz - az * 0.0; double Qey = aw * 0.0 - ax * Qtz + ay * Qtw + az * 0.0; double Qez = aw * Qtz + ax * 0.0 - ay * 0.0 + az * Qtw; - if (Qew < 0.0) { Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; } + if (Qew < 0.0) { + Qew = -Qew; + Qex = -Qex; + Qey = -Qey; + Qez = -Qez; + } const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; const double KpAtt[3] = { 1.5, 1.5, 1.0 }; - const double Wmax[3] = { 0.8, 0.8, 0.6 }; + const double Wmax[3] = { 0.8, 0.8, 0.6 }; const double Wn = 5.0; static double AttInt[3] = { 0.0, 0.0, 0.0 }; @@ -1266,8 +1269,8 @@ void URovMuJoCoBridge::ApplyControlForce() // 上限也要一起放大,否则前馈会被 Wmax 截掉,目标航向又会跑飞。 const double YawFF = FMath::DegreesToRadians(YawCmdRateDeg); const double WmaxZ = (YawFF != 0.0) - ? FMath::Max(Wmax[2], FMath::Abs(YawFF) + 0.2) - : Wmax[2]; + ? FMath::Max(Wmax[2], FMath::Abs(YawFF) + 0.2) + : Wmax[2]; for (int32 Axis = 0; Axis < 3; ++Axis) { const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] @@ -1277,15 +1280,22 @@ void URovMuJoCoBridge::ApplyControlForce() // 机械臂残余重量等),否则纯 P 会停在一个固定倾角上。 AttInt[Axis] = FMath::Clamp(AttInt[Axis] + E3[Axis] * IntDt, -2.0, 2.0); double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis] + AttitudeIntegralGain * AttInt[Axis], - -Wlim, Wlim); + -Wlim, + Wlim); if (Axis == 2 && YawFF != 0.0) { Wsp = FMath::Clamp(Wsp + YawFF, -WmaxZ, WmaxZ); } double T = I * Wn * (Wsp - Data->qvel[ND + 3 + Axis]); T = FMath::Clamp(T, -1500.0, 1500.0); - if (Axis == 0) { TorqueX += T; } - else if (Axis == 1) { TorqueY += T; } - else { TorqueZ += T; } + if (Axis == 0) { + TorqueX += T; + } + else if (Axis == 1) { + TorqueY += T; + } + else { + TorqueZ += T; + } } // 航向诊断(每秒一次):目标 / 实测 / 误差 / 机体 Z 角速度 / 前馈 @@ -1297,19 +1307,30 @@ void URovMuJoCoBridge::ApplyControlForce() FMath::Atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))); double YawErrDeg = (double)TargetYawDeg - CurYawDeg; - while (YawErrDeg > 180.0) { YawErrDeg -= 360.0; } - while (YawErrDeg < -180.0) { YawErrDeg += 360.0; } + while (YawErrDeg > 180.0) { + YawErrDeg -= 360.0; + } + while (YawErrDeg < -180.0) { + YawErrDeg += 360.0; + } // 真实倾角:机体 Z 轴与世界 Z 的夹角,不受欧拉角约定影响 const double UpZ = FMath::Clamp((double)Data->xmat[9 * BodyId + 8], -1.0, 1.0); const double TiltDeg = FMath::RadiansToDegrees(FMath::Acos(UpZ)); - UE_LOG(LogRovBridge, Log, - TEXT("[%s][姿态诊断] t=%.1f 倾角=%.1f° 目标=%.1f° 实测=%.1f° 误差=%.1f° " - "E3=(%.3f, %.3f, %.3f) w=(%.3f, %.3f, %.3f) 前馈=%+.0f°/s"), - TEXT("RovMuJoCoBridge"), Data->time, TiltDeg, - TargetYawDeg, CurYawDeg, YawErrDeg, - E3[0], E3[1], E3[2], - Data->qvel[ND + 3], Data->qvel[ND + 4], Data->qvel[ND + 5], - YawCmdRateDeg); + UE_LOG(LogRovBridge, Log, TEXT("[%s][姿态诊断] t=%.1f 倾角=%.1f° 目标=%.1f° 实测=%.1f° 误差=%.1f° " + "E3=(%.3f, %.3f, %.3f) w=(%.3f, %.3f, %.3f) 前馈=%+.0f°/s"), + TEXT("RovMuJoCoBridge"), + Data->time, + TiltDeg, + TargetYawDeg, + CurYawDeg, + YawErrDeg, + E3[0], + E3[1], + E3[2], + Data->qvel[ND + 3], + Data->qvel[ND + 4], + Data->qvel[ND + 5], + YawCmdRateDeg); } } } @@ -1317,7 +1338,7 @@ void URovMuJoCoBridge::ApplyControlForce() // 【11】机械臂静力矩配平:常值前馈,把控制器硬扛的那部分稳态力矩抵掉 if (bEnableAttitudeHold && !bSpinTest) { float TrimPitch = TrimTorquePitch; - float TrimRoll = TrimTorqueRoll; + float TrimRoll = TrimTorqueRoll; FParse::Value(FCommandLine::Get(), TEXT("RovTrimPitch="), TrimPitch); FParse::Value(FCommandLine::Get(), TEXT("RovTrimRoll="), TrimRoll); TorqueX += TrimRoll; @@ -1343,9 +1364,7 @@ void URovMuJoCoBridge::ApplyControlForce() const int32 J = Model->body_jntadr[BodyId]; if (Model->jnt_type[J] == mjJNT_FREE) { const int32 D = Model->jnt_dofadr[J]; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][力矩轴自检] 给轴%d 100N·m -> omega=(%.4f, %.4f, %.4f)"), - TorqueTestAxis, Data->qvel[D + 3], Data->qvel[D + 4], Data->qvel[D + 5]); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][力矩轴自检] 给轴%d 100N·m -> omega=(%.4f, %.4f, %.4f)"), TorqueTestAxis, Data->qvel[D + 3], Data->qvel[D + 4], Data->qvel[D + 5]); } } } @@ -1365,20 +1384,31 @@ void URovMuJoCoBridge::ApplyControlForce() } }); bControlCallbackRegistered = true; - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge] 已注册物理线程施力回调(PreStepCallbacks)")); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge] 已注册物理线程施力回调(PreStepCallbacks)")); } // 节流日志 static int32 CtrlLogCounter = 0; if ((LocalX != 0.0 || LocalY != 0.0 || LocalZ != 0.0) && (++CtrlLogCounter % 30 == 0)) { - UE_LOG(LogRovBridge, Log, - TEXT("[RovMuJoCoBridge][受力] 输入=(%.0f,%.0f,%.0f) 推力=(%.1f,%.1f,%.1f) N " - "浮力=%.1f N 洋流拖曳=(%.1f,%.1f,%.1f) N 合计Fz=%.1f N " - "洋流=(%.2f,%.2f,%.2f) m/s 定深=%d Z=%.3f"), - LocalX, LocalY, LocalZ, ThrusterX, ThrusterY, ThrusterZ, - Buoyancy, CurrentX, CurrentY, CurrentZ, Fz, - VCurrent.X, VCurrent.Y, VCurrent.Z, bDepthHoldActive ? 1 : 0, RovZ); + UE_LOG(LogRovBridge, Log, TEXT("[RovMuJoCoBridge][受力] 输入=(%.0f,%.0f,%.0f) 推力=(%.1f,%.1f,%.1f) N " + "浮力=%.1f N 洋流拖曳=(%.1f,%.1f,%.1f) N 合计Fz=%.1f N " + "洋流=(%.2f,%.2f,%.2f) m/s 定深=%d Z=%.3f"), + LocalX, + LocalY, + LocalZ, + ThrusterX, + ThrusterY, + ThrusterZ, + Buoyancy, + CurrentX, + CurrentY, + CurrentZ, + Fz, + VCurrent.X, + VCurrent.Y, + VCurrent.Z, + bDepthHoldActive ? 1 : 0, + RovZ); } } @@ -1388,17 +1418,14 @@ void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int OutTorque[1] = 0.0; OutTorque[2] = 0.0; - if (!bEnableAttitudeHold || !Model || !Data || BodyId < 0 || BodyId >= Model->nbody) - { + if (!bEnableAttitudeHold || !Model || !Data || BodyId < 0 || BodyId >= Model->nbody) { return; } - if (Model->body_jntnum[BodyId] < 1) - { + if (Model->body_jntnum[BodyId] < 1) { return; } const int32 J = Model->body_jntadr[BodyId]; - if (Model->jnt_type[J] != mjJNT_FREE) - { + if (Model->jnt_type[J] != mjJNT_FREE) { return; } const int32 D = Model->jnt_dofadr[J]; @@ -1406,15 +1433,12 @@ void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int const double* QQ = &Data->xquat[4 * BodyId]; const double qw = QQ[0], qx = QQ[1], qy = QQ[2], qz = QQ[3]; - if (!bAttitudeInitialized) - { + if (!bAttitudeInitialized) { bAttitudeInitialized = true; const double InitYaw = FMath::Atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz)); TargetYawDeg = (float)FMath::RadiansToDegrees(InitYaw); - UE_LOG(LogRovBridge, Log, - TEXT("[RovBridge][姿态] 物理线程级联控制启用(%.0f Hz),锁定航向 %.1f°"), - 1.0 / FMath::Max(Dt, 1e-6), TargetYawDeg); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][姿态] 物理线程级联控制启用(%.0f Hz),锁定航向 %.1f°"), 1.0 / FMath::Max(Dt, 1e-6), TargetYawDeg); } const double HalfYaw = FMath::DegreesToRadians((double)TargetYawDeg) * 0.5; @@ -1426,7 +1450,12 @@ void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int double Qex = ax * Qtw + ay * Qtz; double Qey = -ax * Qtz + ay * Qtw; double Qez = aw * Qtz + az * Qtw; - if (Qew < 0.0) { Qew = -Qew; Qex = -Qex; Qey = -Qey; Qez = -Qez; } + if (Qew < 0.0) { + Qew = -Qew; + Qex = -Qex; + Qey = -Qey; + Qez = -Qez; + } const double E3[3] = { 2.0 * Qex, 2.0 * Qey, 2.0 * Qez }; const double KpAtt[3] = { 1.5, 1.5, 1.0 }; @@ -1436,8 +1465,7 @@ void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int const double YawFF = CachedYawFFRadPerSec.load(); const double WmaxZ = (YawFF != 0.0) ? FMath::Max(Wmax[2], FMath::Abs(YawFF) + 0.2) : Wmax[2]; - for (int32 Axis = 0; Axis < 3; ++Axis) - { + for (int32 Axis = 0; Axis < 3; ++Axis) { const double I = (EffInertia[Axis] > 0.0) ? EffInertia[Axis] : Model->body_inertia[3 * BodyId + Axis]; const double Wlim = (Axis == 2) ? WmaxZ : Wmax[Axis]; @@ -1452,20 +1480,17 @@ void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int // 只按角速度判据:若再叠加「误差小才积分」,稳态大倾角就永远消不掉 // (实测会停在 28° 下不来)。 const double Omega = (double)Data->qvel[D + 3 + Axis]; - if (FMath::Abs(Omega) < 0.30) - { + if (FMath::Abs(Omega) < 0.30) { AttIntState[Axis] = FMath::Clamp(AttIntState[Axis] + E3[Axis] * Dt, -2.0, 2.0); } - else - { + else { AttIntState[Axis] *= FMath::Max(0.0, 1.0 - 3.0 * Dt); } - double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis] - + (double)AttitudeIntegralGain * AttIntState[Axis], - -WlimEff, WlimEff); - if (Axis == 2 && YawFF != 0.0) - { + double Wsp = FMath::Clamp(KpAtt[Axis] * E3[Axis] + (double)AttitudeIntegralGain * AttIntState[Axis], + -WlimEff, + WlimEff); + if (Axis == 2 && YawFF != 0.0) { Wsp = FMath::Clamp(Wsp + YawFF, -WlimEff, WlimEff); } double T = I * Wn * (Wsp - Omega); @@ -1475,19 +1500,26 @@ void URovMuJoCoBridge::ComputeAttitudeTorque(mjModel_* Model, mjData_* Data, int // 1 Hz 诊断:倾角、误差、角速度、力矩、积分量 { static double LastLogT = -1.0; - if (Data->time - LastLogT > 1.0) - { + if (Data->time - LastLogT > 1.0) { LastLogT = Data->time; const double UpZ = FMath::Clamp((double)Data->xmat[9 * BodyId + 8], -1.0, 1.0); const double TiltDeg = FMath::RadiansToDegrees(FMath::Acos(UpZ)); - UE_LOG(LogRovBridge, Log, - TEXT("[RovBridge][姿态环] t=%.1f 倾角=%.2f E3=(%+.3f,%+.3f,%+.3f) " - "w=(%+.3f,%+.3f,%+.3f) T=(%+.1f,%+.1f,%+.1f) 积分=(%+.2f,%+.2f,%+.2f)"), - Data->time, TiltDeg, - E3[0], E3[1], E3[2], - Data->qvel[D + 3], Data->qvel[D + 4], Data->qvel[D + 5], - OutTorque[0], OutTorque[1], OutTorque[2], - AttIntState[0], AttIntState[1], AttIntState[2]); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][姿态环] t=%.1f 倾角=%.2f E3=(%+.3f,%+.3f,%+.3f) " + "w=(%+.3f,%+.3f,%+.3f) T=(%+.1f,%+.1f,%+.1f) 积分=(%+.2f,%+.2f,%+.2f)"), + Data->time, + TiltDeg, + E3[0], + E3[1], + E3[2], + Data->qvel[D + 3], + Data->qvel[D + 4], + Data->qvel[D + 5], + OutTorque[0], + OutTorque[1], + OutTorque[2], + AttIntState[0], + AttIntState[1], + AttIntState[2]); } } } @@ -1544,16 +1576,13 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) // 【1b】显式二次阻力:沿机体系三轴 F = -0.5·ρ·Cd·A·|v|·v。 // 用于补偿椭球近似对大钝体 / 细长体的偏差,默认关闭。 - if (bUseExplicitDrag) - { + if (bUseExplicitDrag) { const double QW = Data->xquat[4 * BodyId + 0]; const double QX = Data->xquat[4 * BodyId + 1]; const double QY = Data->xquat[4 * BodyId + 2]; const double QZ = Data->xquat[4 * BodyId + 3]; double Bx = 0.0, By = 0.0, Bz = 0.0; - RotByQuat(QW, -QX, -QY, -QZ, - Data->qvel[DofAdr + 0], Data->qvel[DofAdr + 1], Data->qvel[DofAdr + 2], - Bx, By, Bz); + RotByQuat(QW, -QX, -QY, -QZ, Data->qvel[DofAdr + 0], Data->qvel[DofAdr + 1], Data->qvel[DofAdr + 2], Bx, By, Bz); const double Half = 0.5 * (double)FluidDensity; const double Fbx = -Half * (double)DragCoeff.X * (double)DragRefArea.X * FMath::Abs(Bx) * Bx; const double Fby = -Half * (double)DragCoeff.Y * (double)DragRefArea.Y * FMath::Abs(By) * By; @@ -1566,16 +1595,13 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) // 【1c】附加质量:F = -m_A·a(机体系三轴),默认关闭。 // 椭球模型内部已含附加质量,本项用于单独调节而不改模型。 - if (bUseAddedMass) - { + if (bUseAddedMass) { const double QW = Data->xquat[4 * BodyId + 0]; const double QX = Data->xquat[4 * BodyId + 1]; const double QY = Data->xquat[4 * BodyId + 2]; const double QZ = Data->xquat[4 * BodyId + 3]; double Ax = 0.0, Ay = 0.0, Az = 0.0; - RotByQuat(QW, -QX, -QY, -QZ, - Data->qacc[DofAdr + 0], Data->qacc[DofAdr + 1], Data->qacc[DofAdr + 2], - Ax, Ay, Az); + RotByQuat(QW, -QX, -QY, -QZ, Data->qacc[DofAdr + 0], Data->qacc[DofAdr + 1], Data->qacc[DofAdr + 2], Ax, Ay, Az); const double Fbx = -(double)AddedMass.X * Ax; const double Fby = -(double)AddedMass.Y * Ay; const double Fbz = -(double)AddedMass.Z * Az; @@ -1595,11 +1621,16 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) if (!bInSubtree) { int32 p = Model->body_parentid[b]; while (p > 0) { - if (p == BodyId) { bInSubtree = true; break; } + if (p == BodyId) { + bInSubtree = true; + break; + } p = Model->body_parentid[p]; } } - if (!bInSubtree) { continue; } + if (!bInSubtree) { + continue; + } const double m = Model->body_mass[b]; TotalMass += m; Cx += m * Data->xipos[3 * b + 0]; @@ -1607,9 +1638,14 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) Cz += m * Data->xipos[3 * b + 2]; } if (TotalMass > 1e-6) { - Cx /= TotalMass; Cy /= TotalMass; Cz /= TotalMass; - } else { - Cx = CG[0]; Cy = CG[1]; Cz = CG[2]; + Cx /= TotalMass; + Cy /= TotalMass; + Cz /= TotalMass; + } + else { + Cx = CG[0]; + Cy = CG[1]; + Cz = CG[2]; } // 优先使用 MuJoCo 计算的子树质心,保证与重力作用点一致。 if (Data->subtree_com) { @@ -1620,9 +1656,7 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) if (Buoy > 0.0) { double Rx = 0.0, Ry = 0.0, Rz = 0.0; - RotByQuat(Data->xquat[4 * BodyId + 0], Data->xquat[4 * BodyId + 1], - Data->xquat[4 * BodyId + 2], Data->xquat[4 * BodyId + 3], - 0.0, 0.0, (double)BuoyancyArm, Rx, Ry, Rz); + RotByQuat(Data->xquat[4 * BodyId + 0], Data->xquat[4 * BodyId + 1], Data->xquat[4 * BodyId + 2], Data->xquat[4 * BodyId + 3], 0.0, 0.0, (double)BuoyancyArm, Rx, Ry, Rz); mjtNum F2[3] = { 0.0, 0.0, (mjtNum)Buoy }; mjtNum CBw[3] = { (mjtNum)(Cx + Rx), @@ -1640,10 +1674,15 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) int32 p = Model->body_parentid[b]; bool bDescendant = false; while (p > 0) { - if (p == BodyId) { bDescendant = true; break; } + if (p == BodyId) { + bDescendant = true; + break; + } p = Model->body_parentid[p]; } - if (!bDescendant) { continue; } + if (!bDescendant) { + continue; + } const double m = Model->body_mass[b]; ArmMass += m; Cx += m * Data->xipos[3 * b + 0]; @@ -1675,8 +1714,7 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) // 自检:-RovFlipTest 在 6~6.8 s 施加 500 N·m 横滚力矩把机身掀翻,用于验证回正能力 static const bool bFlipTest = FParse::Param(FCommandLine::Get(), TEXT("RovFlipTest")); - if (bFlipTest && Data->time > 6.0 && Data->time < 6.8) - { + if (bFlipTest && Data->time > 6.0 && Data->time < 6.8) { AttT[0] += 500.0; } Tx += AttT[0]; @@ -1692,22 +1730,18 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) if (!bCtlInitDone) { bCtlInitDone = true; Model->opt.integrator = mjINT_IMPLICIT; - const double Iavg = 0.5 * (Model->body_inertia[3 * BodyId + 0] - + Model->body_inertia[3 * BodyId + 1] - + Model->body_inertia[3 * BodyId + 2]) / 3.0; + const double Iavg = 0.5 * (Model->body_inertia[3 * BodyId + 0] + Model->body_inertia[3 * BodyId + 1] + Model->body_inertia[3 * BodyId + 2]) / 3.0; // 阻尼取值优先级:命令行清零(做流体基线对照)> 属性 > 0.5×平均惯量兜底 static const bool bNoDofDamp = FParse::Param(FCommandLine::Get(), TEXT("RovNoDofDamp")); double Idamp = (AngularDofDamping > 0.0f) ? (double)AngularDofDamping : Iavg; - if (bNoDofDamp) { Idamp = 0.0; } + if (bNoDofDamp) { + Idamp = 0.0; + } Model->dof_damping[DofAdr + 3] = Idamp; Model->dof_damping[DofAdr + 4] = Idamp; Model->dof_damping[DofAdr + 5] = Idamp; UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][版本] mj_versionString=%s"), UTF8_TO_TCHAR(mj_versionString())); - UE_LOG(LogRovBridge, Log, - TEXT("[RovBridge][惯量] body_inertia=(%.3f, %.3f, %.3f) EffInertia=(%.3f, %.3f, %.3f) body_mass=%.3f"), - Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], - Model->body_inertia[3 * BodyId + 2], - EffInertia[0], EffInertia[1], EffInertia[2], Model->body_mass[BodyId]); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][惯量] body_inertia=(%.3f, %.3f, %.3f) EffInertia=(%.3f, %.3f, %.3f) body_mass=%.3f"), Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], Model->body_inertia[3 * BodyId + 2], EffInertia[0], EffInertia[1], EffInertia[2], Model->body_mass[BodyId]); UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][控制] integrator=implicit dof_damping=%.2f N.m.s/rad"), Idamp); // 机械臂质量分布诊断:-RovArmDiag @@ -1717,51 +1751,51 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) if (bArmDiag) { const double* RovCg = &Data->xipos[3 * BodyId]; double ArmMass = 0.0, Mx = 0.0, My = 0.0, Mz = 0.0; - UE_LOG(LogRovBridge, Log, - TEXT("[RovBridge][臂诊断] ROV body[%d]='%s' mass=%.3f subtree=%.3f 惯量=(%.3f, %.3f, %.3f)"), - BodyId, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[BodyId]), - Model->body_mass[BodyId], Model->body_subtreemass[BodyId], - Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], - Model->body_inertia[3 * BodyId + 2]); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][臂诊断] ROV body[%d]='%s' mass=%.3f subtree=%.3f 惯量=(%.3f, %.3f, %.3f)"), BodyId, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[BodyId]), Model->body_mass[BodyId], Model->body_subtreemass[BodyId], Model->body_inertia[3 * BodyId + 0], Model->body_inertia[3 * BodyId + 1], Model->body_inertia[3 * BodyId + 2]); for (int32 b = 0; b < Model->nbody; ++b) { int32 p = Model->body_parentid[b]; bool bDescendant = false; while (p > 0) { - if (p == BodyId) { bDescendant = true; break; } + if (p == BodyId) { + bDescendant = true; + break; + } p = Model->body_parentid[p]; } - if (!bDescendant) { continue; } + if (!bDescendant) { + continue; + } const double m = Model->body_mass[b]; const double* x = &Data->xipos[3 * b]; const double rx = x[0] - RovCg[0], ry = x[1] - RovCg[1], rz = x[2] - RovCg[2]; ArmMass += m; - Mx += m * rx; My += m * ry; Mz += m * rz; - UE_LOG(LogRovBridge, Log, - TEXT(" [臂] body[%d]='%s' mass=%.4f 相对ROV重心=(%.4f, %.4f, %.4f) m 惯量=(%.5f, %.5f, %.5f)"), - b, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[b]), m, rx, ry, rz, - Model->body_inertia[3 * b + 0], Model->body_inertia[3 * b + 1], - Model->body_inertia[3 * b + 2]); + Mx += m * rx; + My += m * ry; + Mz += m * rz; + UE_LOG(LogRovBridge, Log, TEXT(" [臂] body[%d]='%s' mass=%.4f 相对ROV重心=(%.4f, %.4f, %.4f) m 惯量=(%.5f, %.5f, %.5f)"), b, UTF8_TO_TCHAR(Model->names + Model->name_bodyadr[b]), m, rx, ry, rz, Model->body_inertia[3 * b + 0], Model->body_inertia[3 * b + 1], Model->body_inertia[3 * b + 2]); // 该 body 的碰撞几何尺寸,用来判断几何有没有被整体放大 for (int32 g = 0; g < Model->ngeom; ++g) { - if (Model->geom_bodyid[g] != b) { continue; } - UE_LOG(LogRovBridge, Log, - TEXT(" [geom] '%s' type=%d size=(%.4f, %.4f, %.4f) mass=%.4f"), - UTF8_TO_TCHAR(Model->names + Model->name_geomadr[g]), - (int32)Model->geom_type[g], - Model->geom_size[3 * g + 0], Model->geom_size[3 * g + 1], - Model->geom_size[3 * g + 2], Model->body_mass[b]); + if (Model->geom_bodyid[g] != b) { + continue; + } + UE_LOG(LogRovBridge, Log, TEXT(" [geom] '%s' type=%d size=(%.4f, %.4f, %.4f) mass=%.4f"), UTF8_TO_TCHAR(Model->names + Model->name_geomadr[g]), (int32)Model->geom_type[g], Model->geom_size[3 * g + 0], Model->geom_size[3 * g + 1], Model->geom_size[3 * g + 2], Model->body_mass[b]); } } if (ArmMass > 1e-6) { const double G = 9.81; // 重力矩 = Σ r × (0,0,-mg) = (-g·Σm·ry, +g·Σm·rx, 0) - UE_LOG(LogRovBridge, Log, - TEXT("[RovBridge][臂诊断] 合计 mass=%.3f 合成质心相对ROV=(%.4f, %.4f, %.4f) m " - "重力矩=(%.1f, %.1f, %.1f) N·m 重量=%.1f N"), - ArmMass, Mx / ArmMass, My / ArmMass, Mz / ArmMass, - -G * My, G * Mx, 0.0, ArmMass * G); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][臂诊断] 合计 mass=%.3f 合成质心相对ROV=(%.4f, %.4f, %.4f) m " + "重力矩=(%.1f, %.1f, %.1f) N·m 重量=%.1f N"), + ArmMass, + Mx / ArmMass, + My / ArmMass, + Mz / ArmMass, + -G * My, + G * Mx, + 0.0, + ArmMass * G); } } } @@ -1777,18 +1811,26 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) 1.0 - 2.0 * (qq[1] * qq[1] + qq[2] * qq[2]))); const double PitchDeg = FMath::RadiansToDegrees( FMath::Asin(FMath::Clamp(2.0 * (qq[0] * qq[2] - qq[3] * qq[1]), -1.0, 1.0))); - UE_LOG(LogRovBridge, Log, - TEXT("[RovBridge][qfrc] t=%.2f applied=(%.1f, %.1f, %.1f, %.1f, %.1f, %.1f) " - "roll=%.1f° pitch=%.1f° Z=%.3f ncon=%d bias=(%.1f, %.1f, %.1f) " - "constr=(%.1f, %.1f, %.1f)"), - Data->time, - Data->qfrc_applied[DofAdr + 0], Data->qfrc_applied[DofAdr + 1], - Data->qfrc_applied[DofAdr + 2], Data->qfrc_applied[DofAdr + 3], - Data->qfrc_applied[DofAdr + 4], Data->qfrc_applied[DofAdr + 5], - RollDeg, PitchDeg, Data->xpos[3 * BodyId + 2], (int32)Data->ncon, - Data->qfrc_bias[DofAdr + 3], Data->qfrc_bias[DofAdr + 4], Data->qfrc_bias[DofAdr + 5], - Data->qfrc_constraint[DofAdr + 3], Data->qfrc_constraint[DofAdr + 4], - Data->qfrc_constraint[DofAdr + 5]); + UE_LOG(LogRovBridge, Log, TEXT("[RovBridge][qfrc] t=%.2f applied=(%.1f, %.1f, %.1f, %.1f, %.1f, %.1f) " + "roll=%.1f° pitch=%.1f° Z=%.3f ncon=%d bias=(%.1f, %.1f, %.1f) " + "constr=(%.1f, %.1f, %.1f)"), + Data->time, + Data->qfrc_applied[DofAdr + 0], + Data->qfrc_applied[DofAdr + 1], + Data->qfrc_applied[DofAdr + 2], + Data->qfrc_applied[DofAdr + 3], + Data->qfrc_applied[DofAdr + 4], + Data->qfrc_applied[DofAdr + 5], + RollDeg, + PitchDeg, + Data->xpos[3 * BodyId + 2], + (int32)Data->ncon, + Data->qfrc_bias[DofAdr + 3], + Data->qfrc_bias[DofAdr + 4], + Data->qfrc_bias[DofAdr + 5], + Data->qfrc_constraint[DofAdr + 3], + Data->qfrc_constraint[DofAdr + 4], + Data->qfrc_constraint[DofAdr + 5]); } } // 【6】实时状态缓存:供 HUD 与外部读取(3.4 / 7.3.1) @@ -1815,31 +1857,36 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) double ContactSum = 0.0, Px = 0.0, Py = 0.0, Pz = 0.0; double Dx = 0.0, Dy = 0.0, Dz = 0.0; mjtNum F6[6] = { 0, 0, 0, 0, 0, 0 }; - for (int32 i = 0; i < Data->ncon; ++i) - { + for (int32 i = 0; i < Data->ncon; ++i) { const mjContact& C = Data->contact[i]; const int32 B1 = Model->geom_bodyid[C.geom1]; const int32 B2 = Model->geom_bodyid[C.geom2]; bool bTouchesBody = false; bool bBody1IsRov = false; - for (int32 b : { B1, B2 }) - { + for (int32 b : { B1, B2 }) { bool bInSubtree = false; int32 p = b; - while (p > 0) - { - if (p == BodyId) { bInSubtree = true; break; } + while (p > 0) { + if (p == BodyId) { + bInSubtree = true; + break; + } p = Model->body_parentid[p]; } - if (b == BodyId) { bInSubtree = true; } - if (bInSubtree) - { + if (b == BodyId) { + bInSubtree = true; + } + if (bInSubtree) { bTouchesBody = true; - if (b == B1) { bBody1IsRov = true; } + if (b == B1) { + bBody1IsRov = true; + } break; } } - if (!bTouchesBody) { continue; } + if (!bTouchesBody) { + continue; + } mj_contactForce(Model, Data, i, F6); ContactSum += FMath::Abs((double)F6[0]); // frame[0..2] 是从 geom1 指向 geom2 的法向;作用在本体上的方向取反 @@ -1847,7 +1894,9 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) Dx += Sign * C.frame[0] * F6[0]; Dy += Sign * C.frame[1] * F6[0]; Dz += Sign * C.frame[2] * F6[0]; - Px = C.pos[0]; Py = C.pos[1]; Pz = C.pos[2]; + Px = C.pos[0]; + Py = C.pos[1]; + Pz = C.pos[2]; } CachedContactForce.store(ContactSum); // 接触点存成「相对机体、UE 轴、厘米」的偏移:HUD 直接用 @@ -1859,15 +1908,13 @@ void URovMuJoCoBridge::ApplyCachedForce(mjModel_* Model, mjData_* Data) CachedContactPointY.store(-(Py - BY) * 100.0); CachedContactPointZ.store((Pz - BZ) * 100.0); const double DNorm = FMath::Sqrt(Dx * Dx + Dy * Dy + Dz * Dz); - if (DNorm > 1e-9) - { + if (DNorm > 1e-9) { // 法向同样做 Y 镜像,得到 UE 世界系方向 CachedContactDirX.store(Dx / DNorm); CachedContactDirY.store(-Dy / DNorm); CachedContactDirZ.store(Dz / DNorm); } - else - { + else { CachedContactDirX.store(0.0); CachedContactDirY.store(0.0); CachedContactDirZ.store(0.0); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h index b371eb6d..10f0fb68 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h @@ -27,18 +27,30 @@ struct FRovStatusSnapshot { GENERATED_BODY() - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float RollDeg = 0.0f; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float PitchDeg = 0.0f; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float YawDeg = 0.0f; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float DepthM = 0.0f; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float ThrustN = 0.0f; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float ContactForceN = 0.0f; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") float CurrentSpeedMps = 0.0f; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector VelocityMps = FVector::ZeroVector; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector ContactPoint = FVector::ZeroVector; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector ContactForceDir = FVector::ZeroVector; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") FVector FlowVelocityMps = FVector::ZeroVector; - UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") bool bValid = false; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float RollDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float PitchDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float YawDeg = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float DepthM = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float ThrustN = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float ContactForceN = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + float CurrentSpeedMps = 0.0f; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector VelocityMps = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector ContactPoint = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector ContactForceDir = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + FVector FlowVelocityMps = FVector::ZeroVector; + UPROPERTY(BlueprintReadOnly, Category = "ROV|Status") + bool bValid = false; }; // MuJoCo 的 mjModel / mjData 是 struct mjModel_ / struct mjData_ 的 typedef。 @@ -111,7 +123,7 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent * MJCF 为米制,桥接输出到 UE 时 ×100 转厘米。 */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") - float WaterSurfaceZ = -2.0f; // 地图海面 Z = -200 cm + float WaterSurfaceZ = -2.0f; // 地图海面 Z = -200 cm // ---------------------------- 流体 ---------------------------- @@ -405,26 +417,26 @@ class AIRSIM_API URovMuJoCoBridge : public UActorComponent void ApplyCachedForce(mjModel_* Model, mjData_* Data); /** 期望推力(N),游戏线程写、物理线程读。 */ - std::atomic CachedForceX{0.0}; - std::atomic CachedForceY{0.0}; - std::atomic CachedForceZ{0.0}; + std::atomic CachedForceX{ 0.0 }; + std::atomic CachedForceY{ 0.0 }; + std::atomic CachedForceZ{ 0.0 }; /** 期望力矩(N·m),**世界系**(mj_applyFT 的入参就是世界系)。 */ - std::atomic CachedTorqueX{0.0}; - std::atomic CachedTorqueY{0.0}; - std::atomic CachedTorqueZ{0.0}; + std::atomic CachedTorqueX{ 0.0 }; + std::atomic CachedTorqueY{ 0.0 }; + std::atomic CachedTorqueZ{ 0.0 }; /** 浮力大小(N)。单独存,因为要作用在浮心而不是质心。 */ - std::atomic CachedBuoyancy{0.0}; + std::atomic CachedBuoyancy{ 0.0 }; - std::atomic CachedBodyId{-1}; + std::atomic CachedBodyId{ -1 }; /** * 控制指令时间戳(MuJoCo 仿真时间)。 * 指令在游戏线程按帧更新、物理在独立线程按 500 Hz 步进; * 物理线程据此判断指令是否过期,过期时把推力与力矩归零。 */ - std::atomic CachedControlTime{0.0}; + std::atomic CachedControlTime{ 0.0 }; /** pre-step 回调只注册一次。 */ bool bControlCallbackRegistered = false; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp index 007cc88a..d5611cb5 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp @@ -23,36 +23,26 @@ void ARovObstacleField::BuildDefaults() // 圆柱默认轴为 Z:轴向沿 X 用 Pitch=90,沿 Y 用 Pitch=90 + Yaw=90。 // 1. 海底主干管道与分支管道 - Obstacles.Add({ ERovObstacleType::Pipe, FVector(0.0f, -320.0f, -160.0f), - FVector(25.0f, 25.0f, 450.0f), FRotator(90.0f, 0.0f, 0.0f) }); - Obstacles.Add({ ERovObstacleType::Pipe, FVector(300.0f, -100.0f, -160.0f), - FVector(20.0f, 20.0f, 300.0f), FRotator(90.0f, 90.0f, 0.0f) }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(0.0f, -320.0f, -160.0f), FVector(25.0f, 25.0f, 450.0f), FRotator(90.0f, 0.0f, 0.0f) }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(300.0f, -100.0f, -160.0f), FVector(20.0f, 20.0f, 300.0f), FRotator(90.0f, 90.0f, 0.0f) }); // 2. 井口采油树(基座 + 立管) - Obstacles.Add({ ERovObstacleType::Frame, FVector(180.0f, 50.0f, -200.0f), - FVector(60.0f, 60.0f, 50.0f), FRotator::ZeroRotator }); - Obstacles.Add({ ERovObstacleType::Pipe, FVector(180.0f, 50.0f, -90.0f), - FVector(22.0f, 22.0f, 60.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Frame, FVector(180.0f, 50.0f, -200.0f), FVector(60.0f, 60.0f, 50.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(180.0f, 50.0f, -90.0f), FVector(22.0f, 22.0f, 60.0f), FRotator::ZeroRotator }); // 3. 导管架支撑桩(两根立柱 + 横梁) - Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, -180.0f, -80.0f), - FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); - Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 180.0f, -80.0f), - FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); - Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 0.0f, -200.0f), - FVector(12.0f, 12.0f, 180.0f), FRotator(90.0f, 90.0f, 0.0f) }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, -180.0f, -80.0f), FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 180.0f, -80.0f), FVector(22.0f, 22.0f, 180.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Pipe, FVector(-220.0f, 0.0f, -200.0f), FVector(12.0f, 12.0f, 180.0f), FRotator(90.0f, 90.0f, 0.0f) }); // 4. 检修结构体与礁石 - Obstacles.Add({ ERovObstacleType::Frame, FVector(0.0f, 320.0f, -140.0f), - FVector(50.0f, 80.0f, 60.0f), FRotator::ZeroRotator }); - Obstacles.Add({ ERovObstacleType::Reef, FVector(-50.0f, -150.0f, -210.0f), - FVector(40.0f, 40.0f, 40.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Frame, FVector(0.0f, 320.0f, -140.0f), FVector(50.0f, 80.0f, 60.0f), FRotator::ZeroRotator }); + Obstacles.Add({ ERovObstacleType::Reef, FVector(-50.0f, -150.0f, -210.0f), FVector(40.0f, 40.0f, 40.0f), FRotator::ZeroRotator }); } void ARovObstacleField::OnConstruction(const FTransform& Transform) { Super::OnConstruction(Transform); - if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) - { + if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) { BuildDefaults(); } } @@ -60,8 +50,7 @@ void ARovObstacleField::OnConstruction(const FTransform& Transform) void ARovObstacleField::BeginPlay() { Super::BeginPlay(); - if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) - { + if (Obstacles.Num() == 0 && bSpawnDefaultObstacles) { BuildDefaults(); } Rebuild(); @@ -69,10 +58,8 @@ void ARovObstacleField::BeginPlay() void ARovObstacleField::Rebuild() { - for (UStaticMeshComponent* Comp : SpawnedComponents) - { - if (Comp) - { + for (UStaticMeshComponent* Comp : SpawnedComponents) { + if (Comp) { Comp->DestroyComponent(); } } @@ -85,13 +72,12 @@ void ARovObstacleField::Rebuild() UStaticMesh* MeshSphere = LoadObject(nullptr, TEXT("/Engine/BasicShapes/Sphere.Sphere")); // 引擎基础网格的原始尺寸:圆柱 r=50 h=100,立方体 100³,球 r=50(单位 cm) - for (const FRovObstacleDef& Def : Obstacles) { + for (const FRovObstacleDef& Def : Obstacles) { UStaticMesh* Mesh = nullptr; FVector Scale = FVector::OneVector; FLinearColor Color(0.6f, 0.6f, 0.6f); - switch (Def.Type) - { + switch (Def.Type) { case ERovObstacleType::Pipe: Mesh = MeshPipe; Scale = FVector(Def.Size.X / 50.0f, Def.Size.Y / 50.0f, Def.Size.Z / 50.0f); @@ -111,8 +97,7 @@ void ARovObstacleField::Rebuild() break; } - if (!Mesh) - { + if (!Mesh) { continue; } @@ -126,13 +111,11 @@ void ARovObstacleField::Rebuild() Comp->SetCollisionEnabled(ECollisionEnabled::NoCollision); Comp->SetCastShadow(false); - if (UMaterialInstanceDynamic* MID = Comp->CreateAndSetMaterialInstanceDynamic(0)) - { + if (UMaterialInstanceDynamic* MID = Comp->CreateAndSetMaterialInstanceDynamic(0)) { MID->SetVectorParameterValue(TEXT("Color"), Color); } SpawnedComponents.Add(Comp); } - UE_LOG(LogTemp, Log, TEXT("[RovScene] 障碍物 %d 个(管道/框架/礁石),锚点 %s"), - SpawnedComponents.Num(), *Base.ToString()); + UE_LOG(LogTemp, Log, TEXT("[RovScene] 障碍物 %d 个(管道/框架/礁石),锚点 %s"), SpawnedComponents.Num(), *Base.ToString()); } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h index b89b033f..8477cf6f 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.h @@ -14,9 +14,9 @@ class UStaticMeshComponent; UENUM(BlueprintType) enum class ERovObstacleType : uint8 { - Pipe UMETA(DisplayName = "管道"), - Frame UMETA(DisplayName = "框架"), - Reef UMETA(DisplayName = "礁石"), + Pipe UMETA(DisplayName = "管道"), + Frame UMETA(DisplayName = "框架"), + Reef UMETA(DisplayName = "礁石"), }; /** 单个障碍定义。位置相对锚点(通常是 ROV 出生点),单位 cm。 */ diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp index 26a8ab46..6d47d9f3 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp @@ -188,10 +188,13 @@ void ARovPawn::Tick(float DeltaSeconds) if (bHasInput) { std::vector pwms(8, 0.0f); - pwms[0] = vert; pwms[1] = vert; pwms[2] = vert; pwms[3] = vert; - pwms[4] += fwd * 1.0f + lat * -1.0f + yaw * 1.0f; - pwms[5] += fwd * 1.0f + lat * 1.0f + yaw * -1.0f; - pwms[6] += fwd * -1.0f + lat * 1.0f + yaw * -1.0f; + pwms[0] = vert; + pwms[1] = vert; + pwms[2] = vert; + pwms[3] = vert; + pwms[4] += fwd * 1.0f + lat * -1.0f + yaw * 1.0f; + pwms[5] += fwd * 1.0f + lat * 1.0f + yaw * -1.0f; + pwms[6] += fwd * -1.0f + lat * 1.0f + yaw * -1.0f; pwms[7] += fwd * -1.0f + lat * -1.0f + yaw * 1.0f; for (int i = 0; i < 8; ++i) { diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp index 441b8b24..dd38ecf3 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp @@ -23,25 +23,20 @@ void ARovStatusHUD::BeginPlay() void ARovStatusHUD::ResolveBridge() { - if (Bridge.IsValid()) - { + if (Bridge.IsValid()) { return; } UWorld* World = GetWorld(); - if (!World) - { + if (!World) { return; } - for (TActorIterator It(World); It; ++It) - { + for (TActorIterator It(World); It; ++It) { APawn* Pawn = *It; - if (!Pawn) - { + if (!Pawn) { continue; } URovMuJoCoBridge* Found = Pawn->FindComponentByClass(); - if (Found) - { + if (Found) { Bridge = Found; RovActor = Pawn; return; @@ -54,14 +49,11 @@ void ARovStatusHUD::Tick(float DeltaSeconds) Super::Tick(DeltaSeconds); ResolveBridge(); - if (APlayerController* PC = GetOwningPlayerController()) - { - if (PC->WasInputKeyJustPressed(EKeys::R)) - { + if (APlayerController* PC = GetOwningPlayerController()) { + if (PC->WasInputKeyJustPressed(EKeys::R)) { bShowStatusPanel = !bShowStatusPanel; } - if (PC->WasInputKeyJustPressed(EKeys::V)) - { + if (PC->WasInputKeyJustPressed(EKeys::V)) { bShowVectors = !bShowVectors; } } @@ -70,16 +62,13 @@ void ARovStatusHUD::Tick(float DeltaSeconds) void ARovStatusHUD::DrawHUD() { Super::DrawHUD(); - if (!Canvas) - { + if (!Canvas) { return; } - if (bShowStatusPanel) - { + if (bShowStatusPanel) { DrawStatusPanel(); } - if (bShowVectors) - { + if (bShowVectors) { DrawVectors(); } } @@ -87,12 +76,10 @@ void ARovStatusHUD::DrawHUD() void ARovStatusHUD::DrawStatusPanel() { FRovStatusSnapshot S; - if (Bridge.IsValid()) - { + if (Bridge.IsValid()) { Bridge->GetStatusSnapshot(S); } - if (!S.bValid) - { + if (!S.bValid) { DrawText(TEXT("ROV 状态:等待 MuJoCo 桥接…"), FLinearColor(1.0f, 0.8f, 0.2f), PanelOrigin.X, PanelOrigin.Y, nullptr, 1.1f); return; } @@ -106,28 +93,38 @@ void ARovStatusHUD::DrawStatusPanel() DrawRect(FLinearColor(0.02f, 0.06f, 0.10f, 0.55f), X - 8.0f, Y - 8.0f, 300.0f, 8.0f * LH + 16.0f); - DrawText(TEXT("ROV 实时状态"), Title, X, Y, nullptr, 1.2f); Y += LH * 1.3f; - DrawText(FString::Printf(TEXT("姿态 横滚 %+6.1f° 俯仰 %+6.1f°"), S.RollDeg, S.PitchDeg), Body, X, Y, nullptr, 1.0f); Y += LH; - DrawText(FString::Printf(TEXT(" 航向 %+6.1f°"), S.YawDeg), Body, X, Y, nullptr, 1.0f); Y += LH; - DrawText(FString::Printf(TEXT("深度 %.2f m"), S.DepthM), Body, X, Y, nullptr, 1.0f); Y += LH; - DrawText(FString::Printf(TEXT("推力 %.1f N"), S.ThrustN), Body, X, Y, nullptr, 1.0f); Y += LH; + DrawText(TEXT("ROV 实时状态"), Title, X, Y, nullptr, 1.2f); + Y += LH * 1.3f; + DrawText(FString::Printf(TEXT("姿态 横滚 %+6.1f° 俯仰 %+6.1f°"), S.RollDeg, S.PitchDeg), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT(" 航向 %+6.1f°"), S.YawDeg), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("深度 %.2f m"), S.DepthM), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("推力 %.1f N"), S.ThrustN), Body, X, Y, nullptr, 1.0f); + Y += LH; DrawText(FString::Printf(TEXT("接触力 %.1f N"), S.ContactForceN), - S.ContactForceN > 1.0f ? Warn : Body, X, Y, nullptr, 1.0f); Y += LH; - DrawText(FString::Printf(TEXT("洋流 %.3f m/s"), S.FlowVelocityMps.Size()), Body, X, Y, nullptr, 1.0f); Y += LH; - DrawText(FString::Printf(TEXT("速度 %.3f m/s"), S.VelocityMps.Size()), Body, X, Y, nullptr, 1.0f); Y += LH; + S.ContactForceN > 1.0f ? Warn : Body, + X, + Y, + nullptr, + 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("洋流 %.3f m/s"), S.FlowVelocityMps.Size()), Body, X, Y, nullptr, 1.0f); + Y += LH; + DrawText(FString::Printf(TEXT("速度 %.3f m/s"), S.VelocityMps.Size()), Body, X, Y, nullptr, 1.0f); + Y += LH; DrawText(TEXT("R 面板 V 向量"), FLinearColor(0.6f, 0.65f, 0.7f), X, Y, nullptr, 0.9f); } void ARovStatusHUD::DrawVectors() { - if (!RovActor.IsValid() || !Bridge.IsValid() || !Canvas) - { + if (!RovActor.IsValid() || !Bridge.IsValid() || !Canvas) { return; } FRovStatusSnapshot S; Bridge->GetStatusSnapshot(S); - if (!S.bValid) - { + if (!S.bValid) { return; } @@ -137,8 +134,7 @@ void ARovStatusHUD::DrawVectors() // 幅值仍取 MuJoCo 的 qvel(更准)。两套坐标系不再混用。 const float NowTime = GetWorld() ? GetWorld()->GetTimeSeconds() : 0.0f; FVector VelUE = FVector::ZeroVector; - if (bHasLastRov) - { + if (bHasLastRov) { const float Dt = FMath::Max(NowTime - LastRovTime, 1e-3f); VelUE = (Origin - LastRovLocation) / Dt; } @@ -148,26 +144,21 @@ void ARovStatusHUD::DrawVectors() SmoothedVelocityUE = FMath::Lerp(SmoothedVelocityUE, VelUE, 0.35f); // 起终点都投影到屏幕,箭头方向与场景中的真实方向一致 - auto DrawWorldArrow = [this](const FVector& Start, const FVector& Dir, float LengthCm, - const FLinearColor& Color, float Thickness) -> bool - { - if (Dir.IsNearlyZero() || LengthCm <= 1.0f) - { + auto DrawWorldArrow = [this](const FVector& Start, const FVector& Dir, float LengthCm, const FLinearColor& Color, float Thickness) -> bool { + if (Dir.IsNearlyZero() || LengthCm <= 1.0f) { return false; } const FVector End = Start + Dir.GetSafeNormal() * LengthCm; const FVector P0 = Project(Start); const FVector P1 = Project(End); - if (P0.Z <= 0.0f || P1.Z <= 0.0f) - { + if (P0.Z <= 0.0f || P1.Z <= 0.0f) { return false; } DrawLine(P0.X, P0.Y, P1.X, P1.Y, Color, Thickness); const FVector2D D(P1.X - P0.X, P1.Y - P0.Y); const float L = D.Size(); - if (L < 6.0f) - { + if (L < 6.0f) { return true; } const FVector2D U = D / L; @@ -180,57 +171,54 @@ void ARovStatusHUD::DrawVectors() }; // 接触力:从接触点出发,沿实测法向力方向 - if (S.ContactForceN > 0.5f && !S.ContactForceDir.IsNearlyZero()) - { + if (S.ContactForceN > 0.5f && !S.ContactForceDir.IsNearlyZero()) { const float Len = FMath::Clamp(S.ContactForceN * ForceArrowScale, 30.0f, MaxArrowLength); // 接触点存的是「相对机体、UE 轴、厘米」的偏移 const FVector ContactUE = Origin + S.ContactPoint; - if (DrawWorldArrow(ContactUE, S.ContactForceDir, Len, FLinearColor(1.0f, 0.55f, 0.1f), 3.0f)) - { + if (DrawWorldArrow(ContactUE, S.ContactForceDir, Len, FLinearColor(1.0f, 0.55f, 0.1f), 3.0f)) { const FVector Tip = Project(ContactUE + S.ContactForceDir.GetSafeNormal() * Len); - if (Tip.Z > 0.0f) - { + if (Tip.Z > 0.0f) { DrawText(FString::Printf(TEXT("F %.0f N"), S.ContactForceN), - FLinearColor(1.0f, 0.65f, 0.2f), Tip.X + 8.0f, Tip.Y - 10.0f, nullptr, 1.0f); + FLinearColor(1.0f, 0.65f, 0.2f), + Tip.X + 8.0f, + Tip.Y - 10.0f, + nullptr, + 1.0f); } } } // 机体速度:方向取 UE 位置差分,幅值取 MuJoCo 速度 const float Speed = static_cast(S.VelocityMps.Size()); - if (Speed > 0.01f) - { + if (Speed > 0.01f) { const float Len = FMath::Clamp(Speed * VelocityArrowScale, 40.0f, MaxArrowLength); FVector VelDir = SmoothedVelocityUE; - if (VelDir.Size() < 5.0f) - { + if (VelDir.Size() < 5.0f) { // 低速时差分噪声大,退回 MuJoCo 速度(Y 镜像到 UE 轴) VelDir = FVector(S.VelocityMps.X, -S.VelocityMps.Y, S.VelocityMps.Z); } - if (DrawWorldArrow(Origin, VelDir, Len, FLinearColor(0.3f, 1.0f, 0.45f), 2.5f)) - { + if (DrawWorldArrow(Origin, VelDir, Len, FLinearColor(0.3f, 1.0f, 0.45f), 2.5f)) { const FVector Tip = Project(Origin + VelDir.GetSafeNormal() * Len); - if (Tip.Z > 0.0f) - { + if (Tip.Z > 0.0f) { DrawText(FString::Printf(TEXT("v %.2f m/s"), Speed), - FLinearColor(0.35f, 1.0f, 0.5f), Tip.X + 8.0f, Tip.Y + 2.0f, nullptr, 1.0f); + FLinearColor(0.35f, 1.0f, 0.5f), + Tip.X + 8.0f, + Tip.Y + 2.0f, + nullptr, + 1.0f); } } } // 洋流速度场:机体周围 3×3 采样,方向与幅值均取自实测洋流 - if (bShowFlowField) - { + if (bShowFlowField) { const FVector Flow = S.FlowVelocityMps; const float FlowSpeed = static_cast(Flow.Size()); - if (FlowSpeed > 1e-3f) - { + if (FlowSpeed > 1e-3f) { const FLinearColor FlowColor(0.35f, 0.7f, 1.0f, 0.85f); const float Len = FMath::Clamp(FlowSpeed * 250.0f, 25.0f, 150.0f); - for (int32 iy = -1; iy <= 1; ++iy) - { - for (int32 iz = -1; iz <= 1; ++iz) - { + for (int32 iy = -1; iy <= 1; ++iy) { + for (int32 iz = -1; iz <= 1; ++iz) { const FVector Sample = Origin + FVector(0.0f, iy * 150.0f, iz * 100.0f); DrawWorldArrow(Sample, Flow, Len, FlowColor, 1.5f); } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h index 87df8c4a..a6c1a3a9 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h @@ -21,15 +21,15 @@ struct FRovOceanCurrent /** 分层剖面的一层 */ struct FLayer { - float DepthM = 0.0f; // 深度(负值表示水下) - float Speed = 0.0f; // 流速幅值 m/s - float HorizAngle = 0.0f; // 水平角 rad - float VertAngle = 0.0f; // 垂直角 rad(正 = 向下) + float DepthM = 0.0f; // 深度(负值表示水下) + float Speed = 0.0f; // 流速幅值 m/s + float HorizAngle = 0.0f; // 水平角 rad + float VertAngle = 0.0f; // 垂直角 rad(正 = 向下) }; // ---------------------------- 配置 ---------------------------- - bool bEnabled = true; + bool bEnabled = true; // Gauss-Markov float MeanSpeed = 0.5f; @@ -72,9 +72,9 @@ struct FRovOceanCurrent Rng.Initialize(Seed); if (Layers.Num() == 0) { // 与 config.json 的 stratified_layers 一致 - Layers.Add({ 0.0f, 0.0f, 0.0f, 0.0f }); - Layers.Add({ -10.0f, 0.5f, 0.3f, 0.0f }); - Layers.Add({ -50.0f, 1.0f, 0.5f, 0.1f }); + Layers.Add({ 0.0f, 0.0f, 0.0f, 0.0f }); + Layers.Add({ -10.0f, 0.5f, 0.3f, 0.0f }); + Layers.Add({ -50.0f, 1.0f, 0.5f, 0.1f }); } // 按深度升序(最深在前) Layers.Sort([](const FLayer& A, const FLayer& B) { return A.DepthM < B.DepthM; }); @@ -95,21 +95,19 @@ struct FRovOceanCurrent { const float SqrtDt = FMath::Sqrt(FMath::Max(Dt, 1e-6f)); - CurSpeed += Mu * (MeanSpeed - CurSpeed) * Dt - + NoiseAmp * SqrtDt * Rng.GetFraction() * 2.0f - NoiseAmp * SqrtDt; + CurSpeed += Mu * (MeanSpeed - CurSpeed) * Dt + NoiseAmp * SqrtDt * Rng.GetFraction() * 2.0f - NoiseAmp * SqrtDt; CurSpeed = FMath::Clamp(CurSpeed, 0.0f, MaxSpeed); - CurHoriz += (Mu * (MeanHorizAngle - CurHoriz) - + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; - CurVert += (Mu * (MeanVertAngle - CurVert) - + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; + CurHoriz += (Mu * (MeanHorizAngle - CurHoriz) + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; + CurVert += (Mu * (MeanVertAngle - CurVert) + NoiseAmp * SqrtDt * (Rng.GetFraction() * 2.0f - 1.0f)) * Dt; const float CosV = FMath::Cos(CurVert); // NED(Z 向下) → MuJoCo(Z 向上):vz 取反 return FVector( CurSpeed * CosV * FMath::Cos(CurHoriz), CurSpeed * CosV * FMath::Sin(CurHoriz), - -CurSpeed * FMath::Sin(CurVert)); } + -CurSpeed * FMath::Sin(CurVert)); + } /** 分层层:按深度线性插值 */ FVector StratifiedAt(float DepthM) const @@ -131,7 +129,8 @@ struct FRovOceanCurrent if (DepthM >= Layers[i].DepthM && DepthM <= Layers[i + 1].DepthM) { const float Span = Layers[i + 1].DepthM - Layers[i].DepthM; const float T = (Span > KINDA_SMALL_NUMBER) - ? (DepthM - Layers[i].DepthM) / Span : 0.0f; + ? (DepthM - Layers[i].DepthM) / Span + : 0.0f; return FMath::Lerp(LayerVec(Layers[i]), LayerVec(Layers[i + 1]), T); } } @@ -163,7 +162,7 @@ struct FRovOceanCurrent if (!bEnabled) return FVector::ZeroVector; const FVector VGM = UpdateGaussMarkov(Dt); - const FVector VStrat = StratifiedAt(-Pos.Z); // z 向上 → 深度取反 + const FVector VStrat = StratifiedAt(-Pos.Z); // z 向上 → 深度取反 const FVector VTurb = TurbulentAt(Pos); LastVelocity = VGM + VStrat + VTurb; return LastVelocity; @@ -207,7 +206,7 @@ struct FRovPIDDepthController const float Error = TargetZ - CurrentZ; IntegralError += Error * Dt; - IntegralError = FMath::Clamp(IntegralError, -100.0f, 100.0f); // anti-windup + IntegralError = FMath::Clamp(IntegralError, -100.0f, 100.0f); // anti-windup const float Derivative = (Dt > KINDA_SMALL_NUMBER) ? (Error - PrevError) / Dt : 0.0f; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp index 7187eb0a..7fdcdb38 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp @@ -26,18 +26,15 @@ void ASimModeWorldRov::BeginPlay() initializeForPlay(); // 障碍物场景(7.3.2):管道 / 框架 / 礁石,可被关卡里手动摆放的实例替代 - if (bSpawnObstacleField && GetWorld()) - { + if (bSpawnObstacleField && GetWorld()) { FActorSpawnParameters SpawnParams; SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn; ARovObstacleField* Field = GetWorld()->SpawnActor(ARovObstacleField::StaticClass(), FTransform::Identity, SpawnParams); - if (Field) - { + if (Field) { // 以 ROV 出生点为锚点,保证视觉代理与 MJCF 里的碰撞几何体位置一致 TArray Pawns; getExistingVehiclePawns(Pawns); - if (Pawns.Num() > 0 && Pawns[0]) - { + if (Pawns.Num() > 0 && Pawns[0]) { Field->AnchorActor = Pawns[0]; Field->Rebuild(); } @@ -46,10 +43,8 @@ void ASimModeWorldRov::BeginPlay() } // 实时状态面板 + 力/速度向量(3.4 / 7.3.1) - if (bSpawnStatusHUD && GetWorld()) - { - if (APlayerController* PC = GetWorld()->GetFirstPlayerController()) - { + if (bSpawnStatusHUD && GetWorld()) { + if (APlayerController* PC = GetWorld()->GetFirstPlayerController()) { PC->ClientSetHUD(ARovStatusHUD::StaticClass()); UE_LOG(LogTemp, Log, TEXT("[RovScene] 实时状态面板已启用")); }