diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 0000000000..4bd4a9aa0b --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index 6682fed462..d7e8c722a0 100644 --- a/examples/plugin/custom_sensor_system/CMakeLists.txt +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -27,10 +27,11 @@ add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_cl add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc) target_link_libraries(${PROJECT_NAME} - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} - PRIVATE gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} - PRIVATE odometer + PRIVATE + gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + gz-sensors${GZ_SENSORS_VER}::gz-sensors${GZ_SENSORS_VER} + odometer ) target_include_directories(${PROJECT_NAME} PUBLIC ${sensors_clone_SOURCE_DIR}/examples/custom_sensor) diff --git a/examples/standalone/gtest_setup/CMakeLists.txt b/examples/standalone/gtest_setup/CMakeLists.txt index 444b069b09..eb08a179a2 100644 --- a/examples/standalone/gtest_setup/CMakeLists.txt +++ b/examples/standalone/gtest_setup/CMakeLists.txt @@ -10,13 +10,14 @@ set(GZ_SIM_VER ${gz-sim9_VERSION_MAJOR}) include(FetchContent) FetchContent_Declare( googletest - URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + # Version 1.14. Use commit hash to prevent tag relocation + URL https://github.com/google/googletest/archive/f8d7d77c06936315286eb55f8de22cd23c188571.zip ) set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) enable_testing() -include(Dart) # Generate tests foreach(TEST_TARGET 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/include/gz/sim/components/Factory.hh b/include/gz/sim/components/Factory.hh index 393c770a9d..5f27a130bd 100644 --- a/include/gz/sim/components/Factory.hh +++ b/include/gz/sim/components/Factory.hh @@ -17,6 +17,7 @@ #ifndef GZ_SIM_COMPONENTS_FACTORY_HH_ #define GZ_SIM_COMPONENTS_FACTORY_HH_ +#include #include #include #include diff --git a/package.xml b/package.xml new file mode 100644 index 0000000000..913279ea55 --- /dev/null +++ b/package.xml @@ -0,0 +1,55 @@ + + + + gz-sim9 + 9.0.0 + Gazebo Sim : A Robotic Simulator + Michael Carroll + Apache License 2.0 + https://github.com/gazebosim/gz-sim + + cmake + + benchmark + glut + gz-cmake4 + gz-common6 + gz-fuel_tools10 + gz-gui9 + gz-math8 + gz-msgs11 + gz-physics8 + gz-plugin3 + gz-rendering9 + gz-sensors9 + gz-tools2 + gz-transport14 + gz-utils3 + libfreeimage-dev + libglew-dev + libxi-dev + libxmu-dev + protobuf-dev + pybind11-dev + python3-distutils + qml-module-qt-labs-folderlistmodel + qml-module-qt-labs-settings + qml-module-qtgraphicaleffects + qml-module-qtquick-controls2 + qml-module-qtquick-controls + qml-module-qtquick-dialogs + qml-module-qtquick-layouts + qml-module-qtquick2 + qtbase5-dev + qtdeclarative5-dev + sdformat15 + tinyxml2 + uuid + + xvfb + python3-pytest + + + cmake + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e3730218e7..7f0aaf946a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -124,7 +124,6 @@ set (gtest_sources SimulationRunner_TEST.cc SystemLoader_TEST.cc SystemManager_TEST.cc - System_TEST.cc TestFixture_TEST.cc Util_TEST.cc World_TEST.cc 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/System_TEST.cc b/src/System_TEST.cc deleted file mode 100644 index e238456085..0000000000 --- a/src/System_TEST.cc +++ /dev/null @@ -1,27 +0,0 @@ -/* - * Copyright (C) 2018 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 "gz/sim/System.hh" - -using namespace gz; - -///////////////////////////////////////////////// -TEST(System, Constructor) -{ -} diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index 812a787540..6af4435972 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -19,10 +19,12 @@ #include "EntityContextMenu.hh" #include -#include +#include #include +#include #include +#include #include #include @@ -35,14 +37,21 @@ namespace gz::sim /// \brief Private data class for EntityContextMenu class EntityContextMenuPrivate { + + /// \brief Protects variable changed through services. + public: std::mutex mutex; + /// \brief Gazebo communication node. public: transport::Node node; /// \brief Move to service name public: std::string moveToService; - /// \brief Follow service name - public: std::string followService; + /// \brief Track topic name + public: std::string trackTopic; + + /// \brief Currently tracked topic name + public: std::string currentTrackTopic; /// \brief Remove service name public: std::string removeService; @@ -76,12 +85,37 @@ namespace gz::sim /// \brief Name of world. public: std::string worldName; + + /// \brief Storing last follow target for look at. + public: std::string followTargetLookAt; + + /// \brief Flag used to disable look at when not following target. + public: bool followingTarget{false}; + + /// \brief /gui/track publisher + public: transport::Node::Publisher trackPub; }; } using namespace gz; using namespace sim; +///////////////////////////////////////////////// +void EntityContextMenu::OnCurrentlyTrackedSub(const msgs::CameraTrack &_msg) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->followingTarget = false; + if (_msg.track_mode() == gz::msgs::CameraTrack::FOLLOW || + _msg.track_mode() == gz::msgs::CameraTrack::FOLLOW_LOOK_AT || + _msg.track_mode() == gz::msgs::CameraTrack::FOLLOW_FREE_LOOK) + { + this->dataPtr->followingTarget = true; + } + this->FollowingTargetChanged(); + + return; +} + ///////////////////////////////////////////////// void GzSimPlugin::registerTypes(const char *_uri) { @@ -94,11 +128,17 @@ void GzSimPlugin::registerTypes(const char *_uri) EntityContextMenu::EntityContextMenu() : dataPtr(std::make_unique()) { + this->dataPtr->currentTrackTopic = "/gui/currently_tracked"; + this->dataPtr->node.Subscribe(this->dataPtr->currentTrackTopic, + &EntityContextMenu::OnCurrentlyTrackedSub, this); + gzmsg << "Currently tracking topic on [" + << this->dataPtr->currentTrackTopic << "]" << std::endl; + // For move to service requests this->dataPtr->moveToService = "/gui/move_to"; - // For follow service requests - this->dataPtr->followService = "/gui/follow"; + // For track topic message + this->dataPtr->trackTopic = "/gui/track"; // For remove service requests this->dataPtr->removeService = "/world/default/remove"; @@ -129,11 +169,27 @@ EntityContextMenu::EntityContextMenu() // For paste service requests this->dataPtr->pasteService = "/gui/paste"; + + this->dataPtr->trackPub = + this->dataPtr->node.Advertise(this->dataPtr->trackTopic); } ///////////////////////////////////////////////// EntityContextMenu::~EntityContextMenu() = default; +///////////////////////////////////////////////// +void EntityContextMenu::SetFollowingTarget(bool &_followingTarget) +{ + this->dataPtr->followingTarget = _followingTarget; + this->FollowingTargetChanged(); +} + +///////////////////////////////////////////////// +bool EntityContextMenu::FollowingTarget() const +{ + return this->dataPtr->followingTarget; +} + ///////////////////////////////////////////////// void EntityContextMenu::OnRemove( const QString &_data, const QString &_type) @@ -196,9 +252,40 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) } else if (request == "follow") { - msgs::StringMsg req; - req.set_data(_data.toStdString()); - this->dataPtr->node.Request(this->dataPtr->followService, req, cb); + msgs::CameraTrack followMsg; + followMsg.mutable_follow_target()->set_name(_data.toStdString()); + followMsg.set_track_mode(msgs::CameraTrack::FOLLOW); + this->dataPtr->followTargetLookAt = followMsg.follow_target().name(); + gzmsg << "Follow target: " << followMsg.follow_target().name() << std::endl; + this->dataPtr->trackPub.Publish(followMsg); + } + else if (request == "free_look") + { + msgs::CameraTrack followMsg; + followMsg.mutable_follow_target()->set_name(_data.toStdString()); + followMsg.set_track_mode(msgs::CameraTrack::FOLLOW_FREE_LOOK); + this->dataPtr->followTargetLookAt = followMsg.follow_target().name(); + gzmsg << "Follow target: " << followMsg.follow_target().name() << std::endl; + this->dataPtr->trackPub.Publish(followMsg); + } + else if (request == "look_at") + { + msgs::CameraTrack followMsg; + followMsg.mutable_track_target()->set_name(_data.toStdString()); + followMsg.set_track_mode(msgs::CameraTrack::FOLLOW_LOOK_AT); + followMsg.mutable_follow_target()->set_name( + this->dataPtr->followTargetLookAt); + gzmsg << "Follow target: " << followMsg.follow_target().name() << std::endl; + gzmsg << "Look at target: " << followMsg.track_target().name() << std::endl; + this->dataPtr->trackPub.Publish(followMsg); + } + else if (request == "track") + { + msgs::CameraTrack trackMsg; + trackMsg.mutable_track_target()->set_name(_data.toStdString()); + trackMsg.set_track_mode(msgs::CameraTrack::TRACK); + gzmsg << "Track target: " << trackMsg.track_target().name() << std::endl; + this->dataPtr->trackPub.Publish(trackMsg); } else if (request == "view_transparent") { diff --git a/src/gui/plugins/modules/EntityContextMenu.hh b/src/gui/plugins/modules/EntityContextMenu.hh index 381417f61e..5f09419f32 100644 --- a/src/gui/plugins/modules/EntityContextMenu.hh +++ b/src/gui/plugins/modules/EntityContextMenu.hh @@ -19,6 +19,7 @@ #define GZ_SIM_GUI_ENTITYCONTEXTMENU_HH_ #include +#include #include #include @@ -46,6 +47,13 @@ namespace sim class EntityContextMenu : public QQuickItem { Q_OBJECT + /// \brief followingTarget + Q_PROPERTY( + bool followingTarget + READ FollowingTarget + WRITE SetFollowingTarget + NOTIFY FollowingTargetChanged + ) /// \brief Constructor public: EntityContextMenu(); @@ -53,6 +61,21 @@ namespace sim /// \brief Destructor public: ~EntityContextMenu() override; + /// \brief Get whether it is following target + /// \return True if followingTarget + public: Q_INVOKABLE bool FollowingTarget() const; + + /// \brief Set whether followingTarget + /// \param[in] _followingTarget True if followingTarget + public: Q_INVOKABLE void SetFollowingTarget(bool &_followingTarget); + + /// \brief Notify that followingTarget has changed + signals: void FollowingTargetChanged(); + + /// \brief Callback function to get data from the message + /// \param[in] _msg CameraTrack message + public: void OnCurrentlyTrackedSub(const msgs::CameraTrack &_msg); + /// \brief Callback when a context menu item is invoked /// \param[in] _data Request data /// \param[in] _type Entity type diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index 1d3cd66120..72626d3f95 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -28,9 +28,19 @@ Item { onTriggered: context.OnRequest("move_to", context.entity) } MenuItem { - id: followMenu - text: "Follow" - onTriggered: context.OnRequest("follow", context.entity) + id: followOptionsSubmenu + text: "Follow Options >" + MouseArea { + id: followOptionsSubMouseArea + anchors.fill: parent + hoverEnabled: true + onEntered: secondMenu.open() + } + } + MenuItem { + id: trackMenu + text: "Track" + onTriggered: context.OnRequest("track", context.entity) } MenuItem { id: removeMenu @@ -67,13 +77,42 @@ Item { id: viewSubMouseArea anchors.fill: parent hoverEnabled: true - onEntered: secondMenu.open() + onEntered: thirdMenu.open() } } } Menu { id: secondMenu x: menu.x + menu.width + y: menu.y + followOptionsSubmenu.y + MenuItem { + id: followMenu + text: "Follow" + onTriggered: { + menu.close() + context.OnRequest("follow", context.entity) + } + } + MenuItem { + id: followFreeLookMenu + text: "Free Look" + onTriggered: { + menu.close() + context.OnRequest("free_look", context.entity) + } + } + MenuItem { + id: followLookAtMenu + text: "Look At" + onTriggered: { + menu.close() + context.OnRequest("look_at", context.entity) + } + } + } + Menu { + id: thirdMenu + x: menu.x + menu.width y: menu.y + viewSubmenu.y MenuItem { id: viewCOMMenu @@ -140,6 +179,9 @@ Item { context.type = _type moveToMenu.enabled = false followMenu.enabled = false + followFreeLookMenu.enabled = false + followLookAtMenu.enabled = false + trackMenu.enabled = false removeMenu.enabled = false viewTransparentMenu.enabled = false; viewCOMMenu.enabled = false; @@ -156,6 +198,12 @@ Item { { moveToMenu.enabled = true followMenu.enabled = true + followFreeLookMenu.enabled = true + if (context.followingTarget) + { + followLookAtMenu.enabled = true + } + trackMenu.enabled = true } if (context.type == "model" || context.type == "light") 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/elevator/vender/afsm/include/afsm/detail/transitions.hpp b/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp index e4fe1fa821..7b5843c009 100644 --- a/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp +++ b/src/systems/elevator/vender/afsm/include/afsm/detail/transitions.hpp @@ -525,7 +525,8 @@ class state_transition_table { void check_default_transition() { - auto const& ttable = transition_table( state_indexes{} ); + auto st = state_indexes{}; + auto const& ttable = transition_table(st); ttable[current_state()](*this, none{}); } @@ -544,7 +545,8 @@ class state_transition_table { void exit(Event&& event) { - auto const& table = exit_table( state_indexes{} ); + auto st = state_indexes{}; + auto const& table = exit_table(st); table[current_state()](states_, ::std::forward(event), *fsm_); } @@ -552,7 +554,8 @@ class state_transition_table { actions::event_process_result process_transition_event(Event&& event) { - auto const& inv_table = transition_table( state_indexes{} ); + auto st = state_indexes{}; + auto const& inv_table = transition_table(st); return inv_table[current_state()](*this, ::std::forward(event)); } @@ -655,10 +658,11 @@ class state_transition_table { event_set current_handled_events() const { - auto const& table = get_current_events_table(state_indexes{}); + auto st = state_indexes{}; + auto const& table = get_current_events_table(st); auto res = table[current_state_](states_); auto const& available_transitions - = get_available_transitions_table(state_indexes{}); + = get_available_transitions_table(st); auto const& trans = available_transitions[current_state_]; res.insert( trans.begin(), trans.end()); return res; @@ -667,7 +671,8 @@ class state_transition_table { event_set current_deferrable_events() const { - auto const& table = get_current_deferred_events_table(state_indexes{}); + auto st = state_indexes{}; + auto const& table = get_current_deferred_events_table(st); return table[current_state_](states_); } 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/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 3ce3f4b619..84769ca3f2 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -41,6 +40,7 @@ #include #include #include +#include #include #include @@ -228,6 +228,9 @@ class gz::sim::systems::SensorsPrivate /// \brief Check if any of the sensors have connections public: bool SensorsHaveConnections(); + /// \brief Returns all sensors that have a pending trigger + public: std::unordered_set SensorsWithPendingTrigger(); + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -745,11 +748,15 @@ void Sensors::PostUpdate(const UpdateInfo &_info, this->dataPtr->sensorsToUpdate, _info.simTime); } + std::unordered_set sensorsWithPendingTriggers = + this->dataPtr->SensorsWithPendingTrigger(); + // notify the render thread if updates are available if (hasRenderConnections || this->dataPtr->nextUpdateTime <= _info.simTime || this->dataPtr->renderUtil.PendingSensors() > 0 || - this->dataPtr->forceUpdate) + this->dataPtr->forceUpdate || + !sensorsWithPendingTriggers.empty()) { if (this->dataPtr->disableOnDrainedBattery) this->dataPtr->UpdateBatteryState(_ecm); @@ -769,6 +776,9 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::unique_lock lockSensors(this->dataPtr->sensorsMutex); this->dataPtr->activeSensors = std::move(this->dataPtr->sensorsToUpdate); + // Add all sensors that have pending triggers. + this->dataPtr->activeSensors.insert(sensorsWithPendingTriggers.begin(), + sensorsWithPendingTriggers.end()); } this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( @@ -1075,6 +1085,27 @@ bool SensorsPrivate::SensorsHaveConnections() return false; } +////////////////////////////////////////////////// +std::unordered_set +SensorsPrivate::SensorsWithPendingTrigger() +{ + std::unordered_set sensorsWithPendingTrigger; + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + if (nullptr == s) + { + continue; + } + + if (s->HasPendingTrigger()) + { + sensorsWithPendingTrigger.insert(id); + } + } + return sensorsWithPendingTrigger; +} + GZ_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemReset, 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 + + + + + + + +