diff --git a/examples/worlds/lighter_than_air_blimp.sdf b/examples/worlds/lighter_than_air_blimp.sdf new file mode 100644 index 0000000000..b3558ddff2 --- /dev/null +++ b/examples/worlds/lighter_than_air_blimp.sdf @@ -0,0 +1,78 @@ + + + + + + 0.001 + + 1 + + + 0 0 -9.81 + + + + + + + + + + 1.097 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + https://fuel.gazebosim.org/1.0/hkotze/models/airship + + + + diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 842e287025..18037896a7 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -277,6 +278,14 @@ namespace gz public: std::optional WorldInertiaMatrix( const EntityComponentManager &_ecm) const; + /// \brief Get the fluid added mass matrix in the world frame. + /// \param[in] _ecm Entity-component manager. + /// \return Fluide added matrix in world frame, returns nullopt if link + /// does not have components components::Inertial and + /// components::WorldPose. + public: std::optional WorldFluidAddedMassMatrix( + const EntityComponentManager &_ecm) const; + /// \brief Get the rotational and translational kinetic energy of the /// link with respect to the world frame. /// \param[in] _ecm Entity-component manager. diff --git a/src/Link.cc b/src/Link.cc index 4fef211baf..348923a44a 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -356,6 +356,18 @@ std::optional Link::WorldInertiaMatrix( math::Inertiald(inertial->Data().MassMatrix(), comWorldPose).Moi()); } +std::optional Link::WorldFluidAddedMassMatrix( + const EntityComponentManager &_ecm) const +{ + auto inertial = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.Component(this->dataPtr->id); + + if (!worldPose || !inertial) + return std::nullopt; + + return inertial->Data().FluidAddedMass(); +} + ////////////////////////////////////////////////// std::optional Link::WorldKineticEnergy( const EntityComponentManager &_ecm) const diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 1603a13930..078b891319 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -131,6 +131,7 @@ add_subdirectory(kinetic_energy_monitor) add_subdirectory(label) add_subdirectory(lens_flare) add_subdirectory(lift_drag) +add_subdirectory(lighter_than_air_dynamics) add_subdirectory(log) add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 652c793265..71a2e2800b 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -181,10 +181,10 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, // note: gz-sensors does its own throttling. Here the check is mainly // to avoid doing work in the ForceTorquePrivate::Update function bool needsUpdate = false; - for (auto &it : this->dataPtr->entitySensorMap) + for (const auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap) { - if (it.second->NextDataUpdateTime() <= _info.simTime && - it.second->HasConnections()) + if (sensor->NextDataUpdateTime() <= _info.simTime && + sensor->HasConnections()) { needsUpdate = true; break; @@ -193,11 +193,16 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, if (!needsUpdate) return; + // Transform joint wrench to sensor wrench and write to sensor this->dataPtr->Update(_ecm); - for (auto &it : this->dataPtr->entitySensorMap) + for (auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap) { - it.second->Update(_info.simTime, false); + // Call gz::sensors::ForceTorqueSensor::Update + // * Convert to user-specified frame + // * Apply noise + // * Publish to gz-transport topic + sensor->Update(_info.simTime, false); } } @@ -269,7 +274,8 @@ void ForceTorquePrivate::Update(const EntityComponentManager &_ecm) return true; } - // Appropriate components haven't been populated by physics yet + // Return early if JointTransmittedWrench component has not yet been + // populated by the Physics system auto jointWrench = _ecm.Component( jointLinkIt->second.joint); if (nullptr == jointWrench) diff --git a/src/systems/lighter_than_air_dynamics/CMakeLists.txt b/src/systems/lighter_than_air_dynamics/CMakeLists.txt new file mode 100644 index 0000000000..71e42f7f36 --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(lighter_than_air_dynamics + SOURCES + LighterThanAirDynamics.cc + PUBLIC_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + gz-math${GZ_MATH_VER}::eigen3 +) diff --git a/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc new file mode 100644 index 0000000000..c895235e22 --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc @@ -0,0 +1,464 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#include + +#include + +#include +#include + +#include + +#include "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/Environment.hh" +#include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/components/Wind.hh" +#include "gz/sim/components/Inertial.hh" + +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/System.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" + +#include + +#include "gz/math/Matrix3.hh" + +#include "gz/transport/Node.hh" + +#include +#include + +#include "LighterThanAirDynamics.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +/// \brief Private LighterThanAirDynamics data class. +class gz::sim::systems::LighterThanAirDynamicsPrivateData +{ + + /// \brief air density [kg/m^3]. + public: double airDensity; + + /// \brief force coefficient of the viscous flow contribution to the hull + public: double forceViscCoeff; + + /// \brief force coefficient of the inviscid flow contribution to the hull + public: double forceInviscCoeff; + + /// \brief moment coefficient of the viscous flow contribution to the hull + public: double momentViscCoeff; + + /// \brief moment coefficient of the inviscid flow contribution to the hull + public: double momentInviscCoeff; + + /// \brief Top left [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m11; + + /// \brief Top right [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m12; + + /// \brief Bottom left [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m21; + + /// \brief Bottom right [3x3] matrix of the [6x6] added mass matrix + public: math::Matrix3d m22; + + /// \brief distance measured from the nose, where the flow stopped + /// being potential + public: double epsV; + + /// \brief axial drag coefficient of the hull + public: double axialDragCoeff; + + /// \brief The Gazebo Transport node + public: transport::Node node; + + /// \brief Link entity + public: Entity linkEntity; + + /// \brief World frame wind + public: math::Vector3d windVector {0, 0, 0}; + + /// \brief wind current callback + public: void UpdateWind(const msgs::Vector3d &_msg); + + /// \brief Mutex + public: std::mutex mtx; +}; + +///////////////////////////////////////////////// +void LighterThanAirDynamicsPrivateData::UpdateWind(const msgs::Vector3d &_msg) +{ + std::lock_guard lock(this->mtx); + this->windVector = gz::msgs::Convert(_msg); +} + +///////////////////////////////////////////////// +void AddAngularVelocityComponent( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, + gz::sim::components::AngularVelocity()); + } + + // Create an angular velocity component if one is not present. + if (!_ecm.Component( + _entity)) + { + _ecm.CreateComponent(_entity, + gz::sim::components::WorldAngularVelocity()); + } +} + +///////////////////////////////////////////////// +void AddWorldPose( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, gz::sim::components::WorldPose()); + } +} + +///////////////////////////////////////////////// +void AddWorldInertial( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component(_entity)) + { + _ecm.CreateComponent(_entity, gz::sim::components::Inertial()); + } +} + +///////////////////////////////////////////////// +void AddWorldLinearVelocity( + const gz::sim::Entity &_entity, + gz::sim::EntityComponentManager &_ecm) +{ + if (!_ecm.Component( + _entity)) + { + _ecm.CreateComponent(_entity, + gz::sim::components::WorldLinearVelocity()); + } +} + +///////////////////////////////////////////////// +double SdfParamDouble( + const std::shared_ptr &_sdf, + const std::string& _field, + double _default) +{ + return _sdf->Get(_field, _default).first; +} + +math::Matrix3d LighterThanAirDynamics::SkewSymmetricMatrix(math::Vector3d mat) +{ + math::Matrix3d skewSymmetric(0, -1.0*mat[2], mat[1], + mat[2], 0, -1.0*mat[0], + -1.0*mat[1], mat[0], 0); + + + return skewSymmetric; +} + + +math::Vector3d LighterThanAirDynamics::AddedMassForce( + math::Vector3d lin_vel, math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12) +{ + auto skewAngVel = this->SkewSymmetricMatrix(ang_vel); + math::Vector3d forces = skewAngVel * (m11 * lin_vel + m12 * ang_vel); + + return forces; +} + +math::Vector3d LighterThanAirDynamics::AddedMassTorque( + math::Vector3d lin_vel, math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12, math::Matrix3d m21, + math::Matrix3d m22) +{ + auto skewAngVel = this->SkewSymmetricMatrix(ang_vel); + auto skewLinVel = this->SkewSymmetricMatrix(lin_vel); + + // note: the m11*lin_vel term: it is already accounted in the + // inviscous term see [2], and thus removed by the zero multiplication, + // but is added here for visibility. + math::Vector3d torque = skewLinVel * (m11 * lin_vel*0 + m12 * ang_vel) + + skewAngVel * (m21 * lin_vel + m22 * ang_vel); + + return torque; +} + +math::Vector3d LighterThanAirDynamics::LocalVelocity(math::Vector3d lin_vel, + math::Vector3d ang_vel, math::Vector3d dist) +{ + return lin_vel + ang_vel.Cross(dist); +} + +double LighterThanAirDynamics::DynamicPressure( + math::Vector3d vec, double air_density) +{ + return 0.5 * air_density * vec.SquaredLength(); +} + +///////////////////////////////////////////////// +LighterThanAirDynamics::LighterThanAirDynamics() +{ + this->dataPtr = std::make_unique(); +} + +///////////////////////////////////////////////// +LighterThanAirDynamics::~LighterThanAirDynamics() +{ + // Do nothing +} + +///////////////////////////////////////////////// +void LighterThanAirDynamics::Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*_eventMgr*/ +) +{ + if (_sdf->HasElement("air_density")) + { + this->dataPtr->airDensity = SdfParamDouble(_sdf, "air_density", 1.225); + } + + // Create model object, to access convenient functions + auto model = gz::sim::Model(_entity); + + if (!_sdf->HasElement("link_name")) + { + gzerr << "You must specify a for the lighter than air" + << " plugin to act upon"; + return; + } + auto linkName = _sdf->Get("link_name"); + this->dataPtr->linkEntity = model.LinkByName(_ecm, linkName); + if (!_ecm.HasEntity(this->dataPtr->linkEntity)) + { + gzerr << "Link name" << linkName << "does not exist"; + return; + } + + if(_sdf->HasElement("moment_inviscid_coeff")) + { + this->dataPtr->momentInviscCoeff = + _sdf->Get("moment_inviscid_coeff"); + + gzdbg << "moment_inviscid_coeff: " + << this->dataPtr->momentInviscCoeff << "\n"; + }else{ + gzerr << "moment_inviscid_coeff not found \n"; + } + + if(_sdf->HasElement("moment_viscous_coeff")) + { + this->dataPtr->momentViscCoeff = _sdf->Get("moment_viscous_coeff"); + gzdbg << "moment_viscous_coeff: " + << this->dataPtr->momentViscCoeff << "\n"; + }else{ + gzerr << "moment_viscous_coeff not found \n"; + return; + } + + if(_sdf->HasElement("force_inviscid_coeff")) + { + this->dataPtr->forceInviscCoeff = + _sdf->Get("force_inviscid_coeff"); + + gzdbg << "force_inviscid_coeff: " + << this->dataPtr->forceInviscCoeff << "\n"; + }else{ + gzerr << "force_inviscid_coeff not found \n"; + return; + } + + if(_sdf->HasElement("force_viscous_coeff")) + { + this->dataPtr->forceViscCoeff = _sdf->Get("force_viscous_coeff"); + + gzdbg << "force_inviscous_coeff: " << this->dataPtr->forceViscCoeff << "\n"; + }else{ + gzerr << "force_inviscous_coeff not found \n"; + return; + } + + if(_sdf->HasElement("eps_v")) + { + this->dataPtr->epsV = _sdf->Get("eps_v"); + gzdbg << "eps_v: " << this->dataPtr->epsV << "\n"; + }else{ + gzerr << "eps_v not found \n"; + return; + } + + if(_sdf->HasElement("axial_drag_coeff")) + { + this->dataPtr->axialDragCoeff = _sdf->Get("axial_drag_coeff"); + gzdbg << "axial_drag_coeff: " << this->dataPtr->axialDragCoeff << "\n"; + }else{ + gzerr << "axial_drag_coeff not found \n"; + return; + } + + AddWorldPose(this->dataPtr->linkEntity, _ecm); + AddWorldInertial(this->dataPtr->linkEntity, _ecm); + AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm); + AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm); + + gz::sim::Link baseLink(this->dataPtr->linkEntity); + + math::Matrix6d added_mass_matrix = + (baseLink.WorldFluidAddedMassMatrix(_ecm)).value(); + + this->dataPtr->m11 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::TOP_LEFT); + this->dataPtr->m12 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::TOP_RIGHT); + this->dataPtr->m21 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::BOTTOM_LEFT); + this->dataPtr->m22 = added_mass_matrix.Submatrix( + math::Matrix6d::Matrix6Corner::BOTTOM_RIGHT); +} + +///////////////////////////////////////////////// +void LighterThanAirDynamics::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if (_info.paused) + return; + + // Get vehicle state + gz::sim::Link baseLink(this->dataPtr->linkEntity); + auto linearVelocity = _ecm.Component( + this->dataPtr->linkEntity); + auto rotationalVelocity = baseLink.WorldAngularVelocity(_ecm); + + if (!linearVelocity) + { + gzerr << "no linear vel" <<"\n"; + return; + } + + // Transform state to local frame + auto pose = baseLink.WorldPose(_ecm); + // Since we are transforming angular and linear velocity we only care about + // rotation. Also this is where we apply the effects of current to the link + auto linearVel = pose->Rot().Inverse() * (linearVelocity->Data()); + auto angularVelocity = pose->Rot().Inverse() * *rotationalVelocity; + + // Calculate viscous forces + math::Vector3d velEpsV = LocalVelocity(linearVel, angularVelocity, + math::Vector3d(-this->dataPtr->epsV, 0, 0)); + + double q0EpsV = DynamicPressure(velEpsV, this->dataPtr->airDensity); + double gammaEpsV = 0.0f; + + gammaEpsV = std::atan2(std::sqrt(velEpsV[1]*velEpsV[1] + + velEpsV[2]*velEpsV[2]), velEpsV[0]); + + double forceInviscCoeff = this->dataPtr->forceInviscCoeff; + double forceViscCoeff = this->dataPtr->forceViscCoeff; + double momentInviscCoeff = this->dataPtr->momentInviscCoeff; + double momentViscCoeff = this->dataPtr->momentViscCoeff; + + double forceViscMag_ = q0EpsV*(-forceInviscCoeff*std::sin(2*gammaEpsV) + + forceViscCoeff*std::sin(gammaEpsV)*std::sin(gammaEpsV)); + + double momentViscMag_ = q0EpsV*(-momentInviscCoeff*std::sin(2*gammaEpsV) + + momentViscCoeff*std::sin(gammaEpsV)*std::sin(gammaEpsV)); + + double viscNormalMag_ = std::sqrt(velEpsV[1]*velEpsV[1] + + velEpsV[2]*velEpsV[2]); + + double viscNormalY_ = 0.0; + double viscNormalZ_ = 0.0; + + if(viscNormalMag_ > std::numeric_limits::epsilon()){ + + viscNormalY_ = velEpsV[1]/viscNormalMag_; + viscNormalZ_ = velEpsV[2]/viscNormalMag_; + } + + auto forceVisc = forceViscMag_ * math::Vector3d(0, + -viscNormalY_, + -viscNormalZ_); + + auto momentVisc = momentViscMag_ * math::Vector3d(0, + viscNormalZ_, + -viscNormalY_); + + // Added Mass forces & Torques + auto forceAddedMass = -1.0 * this->AddedMassForce(linearVel, + angularVelocity, + this->dataPtr->m11, + this->dataPtr->m12); + + auto momentAddedMass = -1.0 * this->AddedMassTorque(linearVel, + angularVelocity, + this->dataPtr->m11, + this->dataPtr->m12, + this->dataPtr->m21, + this->dataPtr->m22); + + // Axial drag + double q0 = DynamicPressure(linearVel, this->dataPtr->airDensity); + double angleOfAttack = std::atan2(linearVel[2], linearVel[0]); + double axialDragCoeff = this->dataPtr->axialDragCoeff; + + auto forceAxialDrag = math::Vector3d(-q0 * axialDragCoeff * + std::cos(angleOfAttack) * std::cos(angleOfAttack), 0, 0); + + math::Vector3d totalForce = forceAddedMass + forceVisc + forceAxialDrag; + math::Vector3d totalTorque = momentAddedMass + momentVisc; + + baseLink.AddWorldWrench(_ecm, + pose->Rot()*(totalForce), + pose->Rot()*totalTorque); +} + +GZ_ADD_PLUGIN( + LighterThanAirDynamics, System, + LighterThanAirDynamics::ISystemConfigure, + LighterThanAirDynamics::ISystemPreUpdate +) + +GZ_ADD_PLUGIN_ALIAS( + LighterThanAirDynamics, + "gz::sim::systems::LighterThanAirDynamics") + +// TODO(CH3): Deprecated, remove on version 8 +GZ_ADD_PLUGIN_ALIAS( + LighterThanAirDynamics, + "ignition::gazebo::systems::LighterThanAirDynamics") diff --git a/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh new file mode 100644 index 0000000000..a7c0971d55 --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef GZ_SIM_SYSTEMS_LighterThanAirDynamics_HH_ +#define GZ_SIM_SYSTEMS_LighterThanAirDynamics_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + class LighterThanAirDynamicsPrivateData; + + /// \brief This class provides the effect of viscousity on the hull of + /// lighter-than-air vehicles such as airships. The equations implemented + /// is based on the work published in [1], which describes a modeling + /// approach for the nonlinear dynamics simulation of airships and [2] + /// providing more insight of the modelling of an airship. + /// + /// ## System Parameters + /// * `` sets the density of air that surrounds + /// the buoyant object. [Units: kgm^-3] + + /// * `` is the second coefficient in Eq (11) in [1]: + /// \f$ \eta C_{DC} \cdot \int_{\varepsilon_{0}}^{L} 2R \,d\varepsilon \f$ + + /// * `` is the second coefficient in Eq (14) in [1]: + /// \f$ \eta C_{DC} \cdot \int_{\varepsilon_{0}}^{L} 2R (\varepsilon_{m} - + /// \varepsilon) \,d\varepsilon \f$ + + /// * `` is the point on the hull where the flow ceases being + /// potential + + /// * `` the actual drag coefficient of the hull + + /// ## Notes + /// + /// This class only implements the viscous effects on the hull of an + /// airship and currently does not take into the account wind. + /// This class should be used in combination with the boyuancy, added mass + /// and gravity plugins to simulate the behaviour of an airship. + /// Its important to provide a collision property to the hull, since this is + /// from which the buoyancy plugin determines the volume. + /// + /// # Citations + /// [1] Li, Y., & Nahon, M. (2007). Modeling and simulation of airship + /// dynamics. Journal of Guidance, Control, and Dynamics, 30(6), 1691–1700. + /// + /// [2] Li, Y., Nahon, M., & Sharf, I. (2011). Airship dynamics modeling: + /// A literature review. Progress in Aerospace Sciences, 47(3), 217–239. + + class LighterThanAirDynamics: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate + { + /// \brief Constructor + public: LighterThanAirDynamics(); + + /// \brief Destructor + public: ~LighterThanAirDynamics() override; + + /// Documentation inherited + public: void Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*_eventMgr*/) override; + + /// Documentation inherited + public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + ///////////////////////////////////////////////// + /// \brief Calculates the local velocity at an offset from a origin + /// \param[in] lin_vel - The linear body velocity + /// \param[in] ang_vel - The angular body velocity + /// \param[in] dist - The distance vector from the origin + /// \return The local velocity at the distance vector + public: math::Vector3d LocalVelocity(math::Vector3d lin_vel, + math::Vector3d ang_vel, math::Vector3d dist); + + ///////////////////////////////////////////////// + /// \brief Calculates dynamic pressure + /// \param[in] vec - The linear velocity + /// \param[in] air_density - The air density [kg/m^3] + /// \return The dynamic pressure, q + public: double DynamicPressure(math::Vector3d vec, double air_density); + + ///////////////////////////////////////////////// + /// \brief Calculates the potential flow aerodynamic forces that a LTA + /// vehicle experience when moving in a potential fluid. The aerodynamic + /// force is derived using Kirchoff's equation. + /// \param[in] lin_vel - The body linear velocity + /// \param[in] ang_vel - The body angular velocity + /// \param[in] m11 - The left upper [3x3] matrix of the added mass matrix + /// \param[in] m12 - The right upper [3x3] matrix of the added mass matrix + /// \param[in] m21 - The left lower [3x3] matrix of the added mass matrix + /// \param[in] m22 - The right lower [3x3] matrix of the added mass matrix + /// \return The aerodynamic force. + public: math::Vector3d AddedMassTorque(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12, + math::Matrix3d m21, math::Matrix3d m22); + + ///////////////////////////////////////////////// + /// \brief Calculates the potential flow aerodynamic torques that a LTA + /// vehicle experience when moving in a potential fluid. The aerodynamic + /// torques is derived using Kirchoff's equation. + /// \param[in] lin_vel - The body linear velocity + /// \param[in] ang_vel - The body angular velocity + /// \param[in] m11 - The left upper [3x3] matrix of the added mass matrix + /// \param[in] m12 - The right upper [3x3] matrix of the added mass matrix + /// \return The aerodynamic torque + public: math::Vector3d AddedMassForce(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12); + + ///////////////////////////////////////////////// + /// \brief Skew-symmetric matrices can be used to represent cross products + /// as matrix multiplications. + /// \param[in] mat - A [3x1] vector + /// \return The skew-symmetric matrix of mat + public: math::Matrix3d SkewSymmetricMatrix(math::Vector3d mat); + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 61f350bd73..639674e4a3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -44,6 +44,7 @@ set(tests level_manager.cc level_manager_runtime_performers.cc light.cc + lighter_than_air_dynamics.cc link.cc logical_camera_system.cc logical_audio_sensor_plugin.cc diff --git a/test/integration/lighter_than_air_dynamics.cc b/test/integration/lighter_than_air_dynamics.cc new file mode 100644 index 0000000000..b2edf7cf0c --- /dev/null +++ b/test/integration/lighter_than_air_dynamics.cc @@ -0,0 +1,365 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/TestFixture.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/World.hh" + +#include "test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class LighterThanAirDynamicsTest : public InternalFixture<::testing::Test> +{ + /// \brief Test a world file + /// \param[in] _world Path to world file + /// \param[in] _namespace Namespace for topic + public: std::vector TestWorld(const std::string &_world, + const std::string &_namespace); + + public: std::vector> + TestUnstableYaw( + const std::string &_world, const std::string &_namespace); + + public: std::vector> + TestUnstablePitch( + const std::string &_world, const std::string &_namespace); + + private: std::vector worldPoses; + private: std::vector bodyAngularVels; + private: std::vector> state; +}; + + +////////////////////////////////////////////////// +std::vector> +LighterThanAirDynamicsTest::TestUnstablePitch( +const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + + worldPoses.clear(); + bodyAngularVels.clear(); + state.clear(); + + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + // Small disturbance to enduce munk moment + math::Vector3d body_z_force(0, 0, 100); + math::Vector3d world_frame_force = DCM * body_z_force; + + body.AddWorldForce(_ecm, world_frame_force); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto worldPose = body.WorldPose(_ecm); + math::Vector3d bodyAngularVel = \ + (body.WorldPose(_ecm)).value().Rot().Inverse() * \ + body.WorldAngularVelocity(_ecm).value(); + ASSERT_TRUE(worldPose); + worldPoses.push_back(worldPose.value()); + bodyAngularVels.push_back(bodyAngularVel); + state.push_back(std::make_pair(worldPose.value(), bodyAngularVel)); + }). + OnPreUpdate([&](const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) + { + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + math::Vector3d body_x_force(100, 0, 0); + + math::Vector3d world_frame_force = DCM * body_x_force; + + body.AddWorldForce(_ecm, world_frame_force); + }). + Finalize(); + + fixture.Server()->Run(true, 2000, false); + EXPECT_EQ(2000u, worldPoses.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return state; +} + +////////////////////////////////////////////////// +std::vector> + LighterThanAirDynamicsTest::TestUnstableYaw( + const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + worldPoses.clear(); + bodyAngularVels.clear(); + state.clear(); + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + // Small disturbance to enduce some lateral movement + // to allow munk moment to grow + math::Vector3d body_y_force(0, 10, 0); + math::Vector3d world_frame_force = DCM * body_y_force; + + body.AddWorldForce(_ecm, world_frame_force); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto worldPose = body.WorldPose(_ecm); + math::Vector3d bodyAngularVel = + (body.WorldPose(_ecm)).value().Rot().Inverse() * \ + body.WorldAngularVelocity(_ecm).value(); + ASSERT_TRUE(worldPose); + worldPoses.push_back(worldPose.value()); + bodyAngularVels.push_back(bodyAngularVel); + state.push_back(std::make_pair(worldPose.value(), bodyAngularVel)); + + }). + OnPreUpdate([&](const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) + { + auto WorldPose = body.WorldPose(_ecm); + + math::Matrix3d DCM(WorldPose.value().Rot()); + math::Vector3d body_x_force(100, 0, 0); + + math::Vector3d world_frame_force = DCM * body_x_force; + + body.AddWorldForce(_ecm, world_frame_force); + }). + Finalize(); + + fixture.Server()->Run(true, 2000, false); + EXPECT_EQ(2000u, worldPoses.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return state; +} + +////////////////////////////////////////////////// +// Test if the plugin can be loaded succesfully and +// runs +std::vector LighterThanAirDynamicsTest::TestWorld( + const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + std::vector bodyVels; + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto bodyVel = body.WorldLinearVelocity(_ecm); + ASSERT_TRUE(bodyVel); + bodyVels.push_back(bodyVel.value()); + }). + Finalize(); + + fixture.Server()->Run(true, 2000, false); + EXPECT_EQ(2000u, bodyVels.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return bodyVels; +} + +///////////////////////////////////////////////// +/// This test evaluates whether the lighter-than-air-dynamics plugin +/// is loaded successfully and is stable +TEST_F(LighterThanAirDynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SpawnModel)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "lighter_than_air_dynamics.sdf"); + + this->TestWorld(world, "hull"); + +} + +///////////////////////////////////////////////// +// Test whether the yaw of the hull will grow +// due to a a small disturbance in its lateral system +TEST_F(LighterThanAirDynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32( + UnstableMunkMomentInYaw)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "lighter_than_air_dynamics.sdf"); + + auto states = this->TestUnstableYaw(world, "hull"); + auto states2 = this->TestUnstableYaw(world, "hull2"); + + math::Vector3d ang_vel = states.back().second; + math::Vector3d end_pos = states.back().first.Pos(); + math::Quaterniond end_rot = states.back().first.Rot(); + + math::Vector3d ang_vel2 = states2.back().second; + math::Vector3d end_pos2 = states2.back().first.Pos(); + math::Quaterniond end_rot2 = states2.back().first.Rot(); + + // Drag from hull should result in translating less than an object with no + // aerodynamic drag + EXPECT_LE(end_pos.X(), end_pos2.X()); + + // Due to the munk moment, the yaw of the aircraft should grow + EXPECT_GT(abs(end_rot.Euler().Z()), abs(end_rot2.Euler().Z())); + + // Due to the munk moment, the yawrate needs to increase + EXPECT_GT(abs(ang_vel.Z()), abs(ang_vel2.Z())); + +} + +///////////////////////////////////////////////// +// Test whether the pitch of the hull will grow +// due to a a small disturbance in its longitudinal system +TEST_F(LighterThanAirDynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32( + UnstableMunkMomentInPitch)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "lighter_than_air_dynamics.sdf"); + + auto states = this->TestUnstablePitch(world, "hull"); + auto states2 = this->TestUnstablePitch(world, "hull2"); + + math::Vector3d ang_vel = states.back().second; + math::Vector3d end_pos = states.back().first.Pos(); + math::Quaterniond end_rot = states.back().first.Rot(); + + math::Vector3d ang_vel2 = states2.back().second; + math::Vector3d end_pos2 = states2.back().first.Pos(); + math::Quaterniond end_rot2 = states2.back().first.Rot(); + + // Drag from hull should result in translating less than an object with no + // aerodynamic drag + EXPECT_LE(end_pos.X(), end_pos2.X()); + + // Due to the munk moment, the yaw of the aircraft should grow + EXPECT_GT(abs(end_rot.Euler().Y()), abs(end_rot2.Euler().Y())); + + // Due to the munk moment, the yawrate needs to increase + EXPECT_GT(abs(ang_vel.Y()), abs(ang_vel2.Y())); + +} diff --git a/test/worlds/lighter_than_air_dynamics.sdf b/test/worlds/lighter_than_air_dynamics.sdf new file mode 100644 index 0000000000..223d8b7bcd --- /dev/null +++ b/test/worlds/lighter_than_air_dynamics.sdf @@ -0,0 +1,136 @@ + + + + + + 0.001 + + 0 + + + 0 0 0 + + + + + + 0 0 30 0 0 0 + + 0 0 0 0 0 0 + + 5.815677050417254 + + 0.0069861272 + 0 + 0 + 0.1292433532 + 0 + 0.1292433532 + + + + 0.0256621271421697 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + 0.538639694339695 + 0.0 + 0.0 + 0.0 + 0.189291512412098 + + 0.538639694339695 + 0.0 + -0.189291512412098 + 0.0 + + 0.000160094457776136 + 0.0 + 0.0 + + 2.02957381640185 + 0.0 + + 2.02957381640185 + + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + + + hull_link + 1.2754 + -0.06425096870274437 + 0.07237374316691345 + 0.08506449448628849 + -0.0941254292661463 + 0.1 + 2.4389047 + + + + + 0 5 30 0 0 0 + + 0 0 0 0 0 0 + + 5.815677050417254 + + 0.0069861272 + 0 + 0 + 0.1292433532 + 0 + 0.1292433532 + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + 0 0 0 0 1.5707963267948966 0 + + + 3 + 0.75 + + + + + + + +