From 9e6800d18c416fb18281ef8fa513f5cc76fdbeca Mon Sep 17 00:00:00 2001 From: henrykotze Date: Mon, 30 Oct 2023 14:23:57 +0200 Subject: [PATCH] LTA Dynamics System copy of Hydrodynamics folder, and rename to lta Renamed to LighterThanAirDynamics - Added some initial private data - Requires still alot of cleanup some initial dynamics added Building sucessfully with some warnings Running test to determine succesfull Spawning successfull ran tests, but still debugging alot Added additional debug messages Protection for zero division - Test running sucessfully with adding forces and torques, but requires more extensive testing and validation Improved Documentation,some more work on tests Improve documentation to include math Silver bullet model withs its coefficients small documentation update Code style updates. wip Style checks completed Small correction to dynamics Added Mass Force and Torques included - Link class now provides the FluidAddedMassMatrix Removed Munk from added mass & correct coeff names Signed-off-by: henrykotze --- include/gz/sim/Link.hh | 9 + src/Link.cc | 12 + src/systems/CMakeLists.txt | 1 + .../lighter_than_air_dynamics/CMakeLists.txt | 7 + .../LighterThanAirDynamics.cc | 482 ++++++++++++++++++ .../LighterThanAirDynamics.hh | 137 +++++ test/integration/CMakeLists.txt | 1 + test/integration/lighter_than_air_dynamics.cc | 116 +++++ test/worlds/CMakeLists.txt | 1 + test/worlds/lighter_than_air_dynamics.sdf.in | 108 ++++ 10 files changed, 874 insertions(+) create mode 100644 src/systems/lighter_than_air_dynamics/CMakeLists.txt create mode 100644 src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc create mode 100644 src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh create mode 100644 test/integration/lighter_than_air_dynamics.cc create mode 100644 test/worlds/lighter_than_air_dynamics.sdf.in diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index d67a17d4de1..688925cf964 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -269,6 +270,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 8378661dbe7..28dff1d31f7 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -337,6 +337,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 c51e78b6063..e09c0a60301 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -129,6 +129,7 @@ add_subdirectory(joint_traj_control) add_subdirectory(kinetic_energy_monitor) add_subdirectory(label) 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/lighter_than_air_dynamics/CMakeLists.txt b/src/systems/lighter_than_air_dynamics/CMakeLists.txt new file mode 100644 index 00000000000..b88a41d9998 --- /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 +) \ No newline at end of file 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 00000000000..f9963fb229f --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.cc @@ -0,0 +1,482 @@ +/* + * Copyright (C) 2021 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/transport/Node.hh" + +#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::skew_symmetric_matrix(math::Vector3d mat) +{ + math::Matrix3d skew_symmetric(0, -1.0*mat[2], mat[1], + mat[2], 0, -1.0*mat[0], + -1.0*mat[1], mat[0], 0); + + + return skew_symmetric; +} + + +math::Vector3d LighterThanAirDynamics::added_mass_force(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, + math::Matrix3d m12) +{ + auto skew_ang_vel = this->skew_symmetric_matrix(ang_vel); + math::Vector3d forces = skew_ang_vel * (m11*lin_vel + m12*ang_vel); + + return forces; + + +} + +math::Vector3d LighterThanAirDynamics::added_mass_torque(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12, + math::Matrix3d m21, math::Matrix3d m22) +{ + auto skew_ang_vel = this->skew_symmetric_matrix(ang_vel); + auto skew_lin_vel = this->skew_symmetric_matrix(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 = skew_lin_vel * (m11*lin_vel*0 + m12*ang_vel) + + skew_ang_vel*(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("airDensity")) + { + gzwarn << + " parameter is deprecated and will be removed Ignition G.\n" + << "\tPlease update your SDF to use instead."; + } + + this->dataPtr->airDensity = SdfParamDouble(_sdf, "airDensity", + SdfParamDouble(_sdf, "air_density", 998) + ); + + // 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 hydrodynamic" + << " 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; + + + // components::WorldLinearVelocity *windLinearVel = nullptr; + // if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ + // Entity windEntity = _ecm.EntityByComponents(components::Wind()); + // windLinearVel = + // _ecm.Component(windEntity); + // } + + // 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 linear_vel = pose->Rot().Inverse() * (linearVelocity->Data()); + auto angular_velocity = pose->Rot().Inverse() * *rotationalVelocity; + + // Calculate viscous forces + math::Vector3d vel_eps_v = LocalVelocity(linear_vel, angular_velocity, + math::Vector3d(-this->dataPtr->epsV, 0, 0)); + + double q0_eps_v = DynamicPressure(vel_eps_v, this->dataPtr->airDensity); + double gamma_eps_v = 0.0f; + + gamma_eps_v = std::atan2(std::sqrt(vel_eps_v[1]*vel_eps_v[1] + + vel_eps_v[2]*vel_eps_v[2]), vel_eps_v[0]); + + double forceInviscCoeff = this->dataPtr->forceInviscCoeff; + double forceViscCoeff = this->dataPtr->forceViscCoeff; + double momentInviscCoeff = this->dataPtr->momentInviscCoeff; + double momentViscCoeff = this->dataPtr->momentViscCoeff; + + double force_visc_mag_ = q0_eps_v*(-forceInviscCoeff*std::sin(2*gamma_eps_v) + + forceViscCoeff*std::sin(gamma_eps_v)*std::sin(gamma_eps_v)); + + double moment_visc_mag_ = q0_eps_v*(-momentInviscCoeff*std::sin(2*gamma_eps_v) + + momentViscCoeff*std::sin(gamma_eps_v)*std::sin(gamma_eps_v)); + + double visc_normal_mag_ = std::sqrt(vel_eps_v[1]*vel_eps_v[1] + + vel_eps_v[2]*vel_eps_v[2]); + + + double visc_normal_y_ = 0.0; + double visc_normal_z_ = 0.0; + + if(visc_normal_mag_ > __DBL_EPSILON__){ + + visc_normal_y_ = vel_eps_v[1]/visc_normal_mag_; + visc_normal_z_ = vel_eps_v[2]/visc_normal_mag_; + } + + + auto force_visc = force_visc_mag_ * math::Vector3d(0, + -visc_normal_y_, + -visc_normal_z_); + + auto moment_visc = moment_visc_mag_ * math::Vector3d(0, + visc_normal_z_, + -visc_normal_y_); + + // Added Mass forces & Torques + auto force_added_mass = -1.0 * this->added_mass_force(linear_vel, + angular_velocity, + this->dataPtr->m11, + this->dataPtr->m12); + + auto moment_added_mass = -1.0 * this->added_mass_torque(linear_vel, + angular_velocity, + this->dataPtr->m11, + this->dataPtr->m12, + this->dataPtr->m21, + this->dataPtr->m22); + + // Axial drag + double q0 = DynamicPressure(linear_vel, this->dataPtr->airDensity); + double angle_of_attack = std::atan2(linear_vel[2], linear_vel[0]); + double axial_drag_coeff = this->dataPtr->axialDragCoeff; + + auto force_axial_drag = math::Vector3d(-q0*axial_drag_coeff* + std::cos(angle_of_attack)*std::cos(angle_of_attack), 0, 0); + + + math::Vector3d totalForce = force_added_mass + force_visc + force_axial_drag; + math::Vector3d totalTorque = moment_added_mass + moment_visc; + + 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 00000000000..b3c34f2ef61 --- /dev/null +++ b/src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2021 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 the local velocity at an offset from a origin + /// \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); + + + public: math::Vector3d added_mass_torque(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12, + math::Matrix3d m21, math::Matrix3d m22); + + public: math::Vector3d added_mass_force(math::Vector3d lin_vel, + math::Vector3d ang_vel, + math::Matrix3d m11, math::Matrix3d m12); + + public: math::Matrix3d skew_symmetric_matrix(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 fedae18b249..07c14b889c6 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 00000000000..22e4d25c2ce --- /dev/null +++ b/test/integration/lighter_than_air_dynamics.cc @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2022 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 "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: math::Vector3d defaultForce{0, 0, 10.0}; +}; + +////////////////////////////////////////////////// +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_BINARY_PATH), + "test", "worlds", "lighter_than_air_dynamics.sdf"); + + this->TestWorld(world, "hull"); + +} diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt index a5225177954..97b2614bfa2 100644 --- a/test/worlds/CMakeLists.txt +++ b/test/worlds/CMakeLists.txt @@ -6,3 +6,4 @@ configure_file (environmental_data.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/envi configure_file (environmental_sensor.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_sensor.sdf) configure_file (hydrodynamics.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/hydrodynamics.sdf) +configure_file (lighter_than_air_dynamics.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/lighter_than_air_dynamics.sdf) diff --git a/test/worlds/lighter_than_air_dynamics.sdf.in b/test/worlds/lighter_than_air_dynamics.sdf.in new file mode 100644 index 00000000000..856e52910a7 --- /dev/null +++ b/test/worlds/lighter_than_air_dynamics.sdf.in @@ -0,0 +1,108 @@ + + + + + + 0.001 + + 0 + + + 0 0 -9.81 + + + + + + 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 + + + + + 0 0 0 0 0 0 + + 0.5 + + 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.2 + + + + + + + 0.1 + + + + + + + hull_link + 1.2754 + -0.06425096870274437 + 0.07237374316691345 + 0.08506449448628849 + -0.0941254292661463 + 0.1 + 2.4389047 + + + + +