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 + + + + +