diff --git a/AirLib/include/vehicles/rov/RovBodyParams.hpp b/AirLib/include/vehicles/rov/RovBodyParams.hpp index ee320501..40ef5e8a 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..d59cdc1a 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,173 @@ 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/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/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/AirLibUnitTests/AirLibUnitTests.vcxproj b/AirLibUnitTests/AirLibUnitTests.vcxproj index 29a618ed..4543b4d8 100644 --- a/AirLibUnitTests/AirLibUnitTests.vcxproj +++ b/AirLibUnitTests/AirLibUnitTests.vcxproj @@ -1,4 +1,4 @@ - + @@ -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 @@ -288,7 +288,8 @@ - + + diff --git a/AirLibUnitTests/RovHydrodynamicsTest.hpp b/AirLibUnitTests/RovHydrodynamicsTest.hpp new file mode 100644 index 00000000..5d962f3d --- /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..d8fcc9b9 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -1,27 +1,42 @@ - #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; } 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/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 diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 59cd29b0..2e94b006 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -79,8 +79,16 @@ 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" }); + // 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/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/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)); 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..75309e90 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovFluidParams.cpp @@ -0,0 +1,129 @@ +// 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..116fae1a --- /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 new file mode 100644 index 00000000..127fd6f1 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.cpp @@ -0,0 +1,1926 @@ +// 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" +#include "HAL/FileManager.h" +#include "Misc/FileHelper.h" +#include "Misc/Paths.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 (ContactLogArchive) { + ContactLogArchive->Close(); + delete ContactLogArchive; + ContactLogArchive = nullptr; + } + 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(); +} + +// 键盘推进 +// +// 推力通过 MuJoCo 的 xfrc_applied 施加:桥接关闭了 ARovPawn 的物理与碰撞, +// AirSim 的 RC 通道没有物理体承接;模型里的 actuator 只有机械臂关节。 +// xfrc_applied 不会被 mj_step 自动清零,每帧需先清再写。 +// 力的量级按质量缩放,换不同质量的 ROV 手感一致。 + +void URovMuJoCoBridge::TickComponent(float DeltaTime, ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) +{ + Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + + if (bBridgeActive && bEnableKeyboardControl) { + ApplyControlForce(); + } +} + +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()) { + 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; + }; + // 配置文件先加载,命令行随后覆盖,保证「命令行 > 配置文件 > 默认值」 + ApplyConfigFile(); + 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; + } + 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); + + 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 质量阵的值。 + // (qM 只有在 mj_forward/mj_step 之后才有效,首帧读会拿到垃圾值; + // 打包版首帧时序和编辑器不同,曾读到极小值 → 控制器力矩≈0 → ROV 缓慢翻转。) + for (int32 a = 0; a < 3; ++a) { + EffInertia[a] = Model->body_inertia[3 * BodyId + a]; + } + + // 洋流模型初始化(三层级联) + 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); + } + + // 复合惯量延迟计算:qM 只在 mj_forward/mj_step 之后有效, + // 因此等仿真时间过 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; + } + + // 本体坐标系下的期望方向 + // 注意: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; + + // 【航向调整】← / → 连续改变目标航向(姿态保持开启时生效) + // + // 方向按画面定义: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); + const bool bYawRight = PC->IsInputKeyDown(EKeys::Right); + 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); + } + } + + // 航向角速度前馈交给物理线程的姿态环 + CachedYawFFRadPerSec.store(FMath::DegreesToRadians(YawCmdRateDeg)); + + // 【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; + } + + // 出生自动锁定深度:避免浮力略小于重力时一路沉到海底,保证演示时停在水面附近 + 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 = + 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=-5m)并清零线速度,避免海底接触和浮力突变污染测量。 + // 注意:姿态控制在自检期间会被跳过,否则闭环会把脉冲立刻压掉。 + const int32 QPosAdr = Model->jnt_qposadr[SpinJnt]; + 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 + 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; + 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]); + } + 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 wx=%.4f wy=%.4f wz=%.4f |w|=%.4f rad/s"), Now - 2.0, Wx, Wy, Wz, 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·ω(机体系),与流体模型叠加。 + // 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 只含本体,不含机械臂。 + 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]; + } + } + + // 旧的四元数 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) { + 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]); + } + + // 姿态诊断:每秒打印一次欧拉角、目标、误差、角速度和力矩 + { + 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]; + 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]; + 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; + } + } + } + } + + // 【10】级联姿态控制(已迁移到物理线程,见 ComputeAttitudeTorque) + // 保留本块仅作参考:它按渲染帧调用,帧率低于约 25 fps 时会与积分项形成 + // 慢振荡直至翻转,因此改为在物理线程按 500 Hz 运行。 + // 外环:姿态误差 -> 期望角速度(机体系) + // 内环:期望角速度 - 实测角速度(qvel[3:6] 为机体系)-> 力矩 + 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]; + 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); + + // 力矩轴自检:覆盖掉所有力矩项,只给指定轴 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]); + } + } + } + + // 力矩就是机体系,直接缓存;ApplyCachedForce 里直写 qfrc_applied[3..5] + CachedTorqueX.store(TorqueX); + CachedTorqueY.store(TorqueY); + CachedTorqueZ.store(TorqueZ); + 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::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(); + if (BodyId < 0 || !Model || !Data || BodyId >= Model->nbody) { + return; + } + + // 力与力矩统一用 mj_applyFT 施加:force / torque / point 分开传参、全部世界系, + // 自动补 r×F,避免 xfrc_applied 的元素顺序歧义,也能让浮力作用在浮心。 + 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]; + + // 逐接触点日志(4.2.4):此处读到的是上一步求解出的接触 + LogContacts(Model, Data); + + // mj_applyFT 是累加语义,先清零本 body 的 6 维广义力 + for (int32 k = 0; k < 6; ++k) { + Data->qfrc_applied[DofAdr + k] = 0.0; + } + + // 指令新鲜度:控制量在游戏线程按帧更新,物理线程按 500 Hz 步进。 + // 游戏线程卡顿时旧指令会被持续施加,这里按仿真时间做淡出: + // 超过 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.04) / 0.04, 0.0, 1.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() * Fresh), + (mjtNum)(CachedForceY.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(); + + 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 计算的子树质心,保证与重力作用点一致。 + 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], 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), + (mjtNum)(Cy + Ry), + (mjtNum)(Cz + Rz) + }; + mj_applyFT(Model, Data, F2, ZeroT, CBw, BodyId, Data->qfrc_applied); + } + + // 【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) { + 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] 就是机体系力矩,直接赋值。 + // 姿态环已移到物理线程(ComputeAttitudeTorque,500 Hz),这里只叠加 + // 游戏线程给的配平力矩与角阻尼,按 Fresh 淡出。 + double Tx = CachedTorqueX.load() * Fresh; + double Ty = CachedTorqueY.load() * Fresh; + double Tz = CachedTorqueZ.load() * Fresh; + + double AttT[3] = { 0.0, 0.0, 0.0 }; + ComputeAttitudeTorque(Model, Data, BodyId, (double)Model->opt.timestep, AttT); + + // 自检:-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; + 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]); + } + } + // 【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 new file mode 100644 index 00000000..10f0fb68 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovMuJoCoBridge.h @@ -0,0 +1,512 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 位姿跟随桥接。 +// +// MuJoCo 是唯一的位姿真值源,本组件每帧把 ROV 根 body 的世界位姿提供给 +// RovPawnSimApi,后者据此驱动 Pawn。AirSim 自身的 ROV 物理与 UE 侧碰撞全部关闭, +// 避免同一台 ROV 被两套物理重复积分。 +// +// 位姿直接读 MuJoCo 的 xpos / xquat 数组,不依赖 UMjBody 的 Tick 顺序, +// 因此不需要 AddTickPrerequisiteComponent。 + +#pragma once + +#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_; +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 默认暂停,仿真不跑 Pawn 就不会动,因此桥接激活时自动取消暂停。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + bool bAutoStartSimulation = true; + + /** + * MuJoCo 世界坐标到 UE 世界坐标的平移偏移(cm)。 + * Pawn 位姿每帧由 MuJoCo 覆盖,摆放位置需要通过该偏移调整。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + FVector WorldOffset = FVector::ZeroVector; + + /** 键盘推进:W/S 前后、A/D 左右、Q/E 上浮下潜。键盘只给出推力比例。 */ + 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 为米制,桥接输出到 UE 时 ×100 转厘米。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Control") + float WaterSurfaceZ = -2.0f; // 地图海面 Z = -200 cm + + // ---------------------------- 流体 ---------------------------- + + /** 是否启用 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。 + * 按 0.192 m³ 排水、整机 460.5 kg 配平,中性约需 2.34。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float BuoyancyScale = 2.34f; + + /** + * 机械臂排水体积(升)。 + * UR5e 连杆 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。 + * 倾斜时浮力产生回复力矩 τ = r_CB × F_buoy,回复刚度 ≈ B × BG。 + * 设为 0 则浮力加在质心,无被动稳定。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float BuoyancyArm = 0.02f; + + /** + * 机械臂关节阻尼(N·m·s/rad),施加到所有非自由关节。 + * 关节无 ctrl 输入时会自由摆动,反作用力矩会持续激励艇体。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + float ArmJointDamping = 20000.0f; + + /** + * 机械臂关节刚度(N·m/rad),相当于在每个关节上加位置伺服弹簧, + * 把关节锁在 MJCF 的 qpos0 上。只加阻尼时机械臂仍会在重力下缓慢下垂, + * 偏置姿态产生恒定扰动力矩。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo") + float ArmJointStiffness = 0.0f; + + /** + * 机械臂重力补偿:把机械臂各 body 的 gravcomp 设为 1.0,抵消其自重。 + * 保留惯量与碰撞。 + */ + 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 = 1.0f; + + /** 阻尼比。1.0 = 临界阻尼,不超调。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float AttitudeZeta = 0.8f; + + /** + * 姿态积分增益(rad/s per rad·s)。消除恒定力矩造成的固定倾角, + * 积分值限幅 ±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.0f; + + /** 目标航向(度)。启动时自动捕获当前航向。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float TargetYawDeg = 0.0f; + + /** 航向调整速率(度/秒)。按住 ← / → 连续改变目标航向。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Attitude") + float YawRateDegPerSec = 60.0f; + + /** + * 机械臂静力矩配平(机体系,N·m),常值前馈。 + * 臂的重力矩随俯仰角变化,常值前馈无法完全抵消,默认 0。 + */ + 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)。 */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Current") + float CurrentDragCoeff = 10.0f; + + /** + * 附加角阻尼(1/s),与流体模型叠加,作为独立受力项。 + * 默认 0,旋转阻尼由 AngularDofDamping 提供。 + */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "ROV|MuJoCo|Fluid") + float AngularDampingCoeff = 0.0f; + + /** + * 转动自由度阻尼(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 + 分层 + 湍流)。 */ + 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; + + // ---------------------------- 配置文件 ---------------------------- + + /** + * 是否从配置文件加载参数。 + * 默认读取 /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; } + + /** 当前跟随的 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。 + * 模型通常在 Pawn 生成之后才编译完成,因此声明为 const + mutable, + * 由 ProvidePose 在运行中惰性重试。 + */ + void ResolveTargets() const; + + /** 关闭 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(); + + /** + * 物理线程内施力:从缓存读出推力/力矩并写入 MuJoCo。 + * 通过 UMjPhysicsEngine 的 PreStepCallbacks 在 mj_step 之前执行, + * 与物理步同锁,避免被 mj_step 覆盖。 + */ + 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 }; + + /** + * 控制指令时间戳(MuJoCo 仿真时间)。 + * 指令在游戏线程按帧更新、物理在独立线程按 500 Hz 步进; + * 物理线程据此判断指令是否过期,过期时把推力与力矩归零。 + */ + std::atomic CachedControlTime{ 0.0 }; + + /** 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; + + /** 物理线程姿态环的积分状态(与帧率无关)。 */ + double AttIntState[3] = { 0.0, 0.0, 0.0 }; + + /** 航向角速度前馈(rad/s,机体系),游戏线程写、物理线程读。 */ + std::atomic CachedYawFFRadPerSec{ 0.0 }; + + /** 姿态控制器是否已初始化(首次捕获目标航向)。 */ + 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; + + /** 接触日志文件句柄(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..d5611cb5 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovObstacleField.cpp @@ -0,0 +1,121 @@ +// 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..8477cf6f --- /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/RovPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp index da1ce4e0..6d47d9f3 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawn.cpp @@ -3,15 +3,20 @@ #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")); + // MuJoCo 位姿跟随桥接 + MuJoCoBridge_ = CreateDefaultSubobject(TEXT("MuJoCoBridge")); + body_mesh_ = CreateDefaultSubobject(TEXT("BodyMesh")); body_mesh_->SetupAttachment(RootComponent); @@ -128,10 +133,82 @@ 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..44646ac9 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,10 @@ class AIRSIM_API ARovPawn : public APawn UPROPERTY() class UStaticMeshComponent* body_mesh_; + // MuJoCo 位姿跟随桥接组件 + UPROPERTY() + class URovMuJoCoBridge* MuJoCoBridge_; + UPROPERTY() class UChildActorComponent* camera_front_right_comp_; UPROPERTY() @@ -75,4 +97,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/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/RovPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovPawnSimApi.cpp index d6a4268f..bfd0cca0 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,27 @@ void RovPawnSimApi::updateRenderedState(float dt) const CollisionInfo& collision_info = getCollisionInfo(); rov_physics_body_->setCollisionInfo(collision_info); - last_phys_pose_ = rov_physics_body_->getPose(); + // 设置外部位姿源后本类退化为纯跟随者,不再使用 AirSim 物理体的状态; + // 坐标转换统一走基类的 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..7b1e20a5 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,22 @@ class RovPawnSimApi : public PawnSimApi return vehicle_api_.get(); } + void onKeyboardPwm(const std::vector& pwms); + + // 外部位姿源。设置后 updateRenderedState() 优先使用它,不再读取 + // rov_physics_body_,避免与外部物理引擎重复积分位姿。 + // provider 返回 false 时回退原有逻辑;坐标为 UE 世界系, + // 由基类 toPoseFromUnreal() 统一转换为 NED。 + 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_; 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..dd38ecf3 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovStatusHUD.cpp @@ -0,0 +1,228 @@ +// 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 new file mode 100644 index 00000000..a6c1a3a9 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/RovUnderwaterPhysics.h @@ -0,0 +1,221 @@ +// Copyright (c) OpenHUTB. All rights reserved. +// +// ROV 水下物理模型: +// - FRovOceanCurrent 三层级联洋流 +// - FRovPIDDepthController 定深 PID +// +// 头文件内联实现,不新增 .cpp。 + +#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)。Gauss-Markov 无均值回归时速度会无界增长, + * 这里加物理上限。 + */ + float MaxSpeed = 2.0f; + + int32 Seed = 42; + + // ---------------------------- 状态 ---------------------------- + + float CurSpeed = 0.0f; + float CurHoriz = 0.0f; + float CurVert = 0.0f; + + /** 最近一次 GetVelocity 的结果(MuJoCo 世界系,m/s),供状态显示读取。 */ + FVector LastVelocity = FVector::ZeroVector; + + 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); + LastVelocity = VGM + VStrat + VTurb; + return LastVelocity; + } +}; + +// ============================================================================= +// 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); + } +}; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Rov/SimModeWorldRov.cpp index 7c55f50f..7fdcdb38 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,31 @@ 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;