From f6efeefb4ab99284d1a053061235f8c2fa02a5da Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 3 Oct 2023 08:54:08 +0800 Subject: [PATCH 1/7] Fix enviroment system loading mechanism (#1842) * Fix enviroment system loading mechanism Currently, there is an issue with the way the Environment loader plugin loads data. In particular it directly writes to the ECM. While this makes sense intuitively, it does not work in practice as the GUI runs on a client process while systems that use it run on the server. This PR fixes this issue by introducing a topic through which the GUI may load Environment Data on the server. Signed-off-by: Arjo Chakravarty * small changes Signed-off-by: Arjo Chakravarty * Working on porting the visuals Signed-off-by: Arjo Chakravarty * Actually send message for loading from ui to environment preload plugin. Visuallization still goes :boom: Signed-off-by: Arjo Chakravarty * Rewrite EnvironmentVisualization Widget to be simpler. Signed-off-by: Arjo Chakravarty * fix crashes. Vis still not working Signed-off-by: Arjo Chakravarty * Get a different :boom: Signed-off-by: Arjo Chakravarty * Works some times. Signed-off-by: Arjo Chakravarty * Fixed synchronization issues. Now left with one more crash that needs debugging when "play" is hit. Signed-off-by: Arjo Chakravarty * No more :boom:s :tada: Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * Sprinkled with healthy dose of Doxygen Also refactored the visualization tool out. Signed-off-by: Arjo Chakravarty * Style Signed-off-by: Arjo Chakravarty * More style fixes Signed-off-by: Arjo Chakravarty * Fix Typo with unit map Signed-off-by: Arjo Chakravarty * Address PR feedback Signed-off-by: Arjo Chakravarty * Style fixes Signed-off-by: Arjo Chakravarty * Fix incorrect use of path. Signed-off-by: Arjo Chakravarty * Fix example loading issues. Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * Update src/systems/environment_preload/VisualizationTool.cc Co-authored-by: Mabel Zhang Signed-off-by: Arjo Chakravarty * Adds a warning regarding loading plugins. Signed-off-by: Arjo Chakravarty * Automatically loads plugin if missing This commit automatically loads the environment preload plugin if it is missing. Signed-off-by: Arjo Chakravarty * Address some feedback I missed Signed-off-by: Arjo Chakravarty * Address some feedback Signed-off-by: Arjo Chakravarty * Fixes issue described by @iche033. However fix depends on https://github.com/gazebosim/gz-math/pull/551 Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * Fixed failing tests Signed-off-by: Arjo Chakravarty --------- Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty Co-authored-by: Michael Carroll Co-authored-by: Mabel Zhang Co-authored-by: Ian Chen --- examples/worlds/CMakeLists.txt | 4 + examples/worlds/environmental_sensor.sdf | 11 +- .../environment_loader/EnvironmentLoader.cc | 119 +++++--- .../EnvironmentVisualization.cc | 244 +++------------- .../EnvironmentVisualization.hh | 6 +- .../environment_preload/CMakeLists.txt | 1 + .../environment_preload/EnvironmentPreload.cc | 265 ++++++++++++++---- .../environment_preload/VisualizationTool.cc | 232 +++++++++++++++ .../environment_preload/VisualizationTool.hh | 143 ++++++++++ 9 files changed, 732 insertions(+), 293 deletions(-) create mode 100644 src/systems/environment_preload/VisualizationTool.cc create mode 100644 src/systems/environment_preload/VisualizationTool.hh diff --git a/examples/worlds/CMakeLists.txt b/examples/worlds/CMakeLists.txt index fee3a9e41c..258ee1529e 100644 --- a/examples/worlds/CMakeLists.txt +++ b/examples/worlds/CMakeLists.txt @@ -2,4 +2,8 @@ file(GLOB files "*.sdf") install(FILES ${files} DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds) +file(GLOB csv_files "*.csv") +install(FILES ${csv_files} + DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds) + add_subdirectory(thumbnails) diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf index e1947c8591..8ef8ed35f1 100644 --- a/examples/worlds/environmental_sensor.sdf +++ b/examples/worlds/environmental_sensor.sdf @@ -1,5 +1,12 @@ - + + @@ -104,6 +112,7 @@ 1 30 + sensors/humidity diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index 6ac8b464cf..cb688b3a8a 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -19,10 +19,11 @@ #include #include -#include #include +#include #include +#include #include #include @@ -33,6 +34,11 @@ #include #include +#include + +#include +#include + using namespace gz; using namespace sim; @@ -42,6 +48,11 @@ namespace sim { inline namespace GZ_SIM_VERSION_NAMESPACE { +const char* preload_plugin_name{ + "gz::sim::systems::EnvironmentPreload"}; +const char* preload_plugin_filename{ + "gz-sim-environment-preload-system"}; +using Units = msgs::DataLoadPathOptions_DataAngularUnits; /// \brief Private data class for EnvironmentLoader class EnvironmentLoaderPrivate { @@ -65,7 +76,7 @@ class EnvironmentLoaderPrivate public: int zIndex{-1}; /// \brief Index of data dimension to be used as units. - public: QString unit; + public: QString unit{"radians"}; public: using ReferenceT = math::SphericalCoordinates::CoordinateType; @@ -76,12 +87,12 @@ class EnvironmentLoaderPrivate {QString("ecef"), math::SphericalCoordinates::ECEF}}; /// \brief Map of supported spatial units. - public: const QMap + public: const QMap unitMap{ {QString("degree"), - components::EnvironmentalData::ReferenceUnits::DEGREES}, + Units::DataLoadPathOptions_DataAngularUnits_DEGREES}, {QString("radians"), - components::EnvironmentalData::ReferenceUnits::RADIANS} + Units::DataLoadPathOptions_DataAngularUnits_RADIANS} }; /// \brief Spatial reference. @@ -92,6 +103,12 @@ class EnvironmentLoaderPrivate /// \brief Whether to attempt an environmental data load. public: std::atomic needsLoad{false}; + + /// \brief Gz transport node + public: transport::Node node; + + /// \brief publisher + public: std::optional pub{std::nullopt}; }; } } @@ -123,38 +140,54 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *) void EnvironmentLoader::Update(const UpdateInfo &, EntityComponentManager &_ecm) { - if (this->dataPtr->needsLoad) + auto world = worldEntity(_ecm); + + if (!this->dataPtr->pub.has_value()) { - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->needsLoad = false; - - /// TODO(arjo): Handle the case where we are loading a file in windows. - /// Should SDFormat files support going from windows <=> unix paths? - std::ifstream dataFile(this->dataPtr->dataPath.toStdString()); - gzmsg << "Loading environmental data from " - << this->dataPtr->dataPath.toStdString() - << std::endl; - try + auto topic = transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/" + "environment"); + this->dataPtr->pub = + {this->dataPtr->node.Advertise(topic)}; + } + + static bool warned = false; + if (!this->dataPtr->pub->HasConnections() && !warned) + { + warned = true; + gzwarn << "Could not find a subscriber for the environment. " + << "Attempting to load environmental preload plugin." + << std::endl; + + auto nameComp = _ecm.Component(world); + if (nullptr == nameComp) { + gzerr << "Failed to get world name" << std::endl; + return; + } + auto worldName = nameComp->Data(); + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(world); + auto plugin = req.add_plugins(); + plugin->set_name(preload_plugin_name); + plugin->set_filename(preload_plugin_filename); + plugin->set_innerxml(""); + msgs::Boolean res; + bool result; + const unsigned int timeout = 5000; + const auto service = transport::TopicUtils::AsValidTopic( + "/world/" + worldName + "/entity/system/add"); + if (service.empty()) + { + gzerr << "Unable to request " << service << std::endl; + return; + } + + if (this->dataPtr->node.Request(service, req, timeout, res, result)) { - using ComponentDataT = components::EnvironmentalData; - auto data = ComponentDataT::MakeShared( - common::IO::ReadFrom( - common::CSVIStreamIterator(dataFile), - common::CSVIStreamIterator(), - this->dataPtr->timeIndex, { - static_cast(this->dataPtr->xIndex), - static_cast(this->dataPtr->yIndex), - static_cast(this->dataPtr->zIndex)}), - this->dataPtr->referenceMap[this->dataPtr->reference], - this->dataPtr->unitMap[this->dataPtr->unit]); - - using ComponentT = components::Environment; - _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)}); + gzdbg << "Added plugin successfully" << std::endl; } - catch (const std::invalid_argument &exc) + else { - gzerr << "Failed to load environmental data" << std::endl - << exc.what() << std::endl; + gzerr << "Failed to load plugin" << std::endl; } } } @@ -162,7 +195,25 @@ void EnvironmentLoader::Update(const UpdateInfo &, ///////////////////////////////////////////////// void EnvironmentLoader::ScheduleLoad() { - this->dataPtr->needsLoad = this->IsConfigured(); + if(this->IsConfigured() && this->dataPtr->pub.has_value()) + { + msgs::DataLoadPathOptions data; + data.set_path(this->dataPtr->dataPath.toStdString()); + data.set_time( + this->dataPtr->dimensionList[this->dataPtr->timeIndex].toStdString()); + data.set_x( + this->dataPtr->dimensionList[this->dataPtr->xIndex].toStdString()); + data.set_y( + this->dataPtr->dimensionList[this->dataPtr->yIndex].toStdString()); + data.set_z( + this->dataPtr->dimensionList[this->dataPtr->zIndex].toStdString()); + auto referenceFrame = this->dataPtr->referenceMap[this->dataPtr->reference]; + + data.set_coordinate_type(msgs::ConvertCoord(referenceFrame)); + data.set_units(this->dataPtr->unitMap[this->dataPtr->unit]); + + this->dataPtr->pub->Publish(data); + } } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc index 3ed5f6f9ce..c9c040f454 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc @@ -36,9 +36,7 @@ #include -#include -#include -#include +#include #include using namespace gz; @@ -51,201 +49,44 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Private data class for EnvironmentVisualization -class EnvironmentVisualizationPrivate +class EnvironmentVisualizationTool { - public: EnvironmentVisualizationPrivate() - { - this->pcPub = - this->node.Advertise("/point_cloud"); - } - /// \brief To synchronize member access. - public: std::mutex mutex; - - /// \brief first load we need to scan for existing data sensor - public: bool first {true}; - - public: std::atomic resample{true}; ///////////////////////////////////////////////// - public: void CreatePointCloudTopics( - std::shared_ptr data) { - this->pubs.clear(); - this->sessions.clear(); - for (auto key : data->frame.Keys()) - { - this->pubs.emplace(key, node.Advertise(key)); - gz::msgs::Float_V msg; - this->floatFields.emplace(key, msg); - this->sessions.emplace(key, data->frame[key].CreateSession()); - } - } - - ///////////////////////////////////////////////// - public: void Step( - const UpdateInfo &_info, - std::shared_ptr &data, - const EntityComponentManager& _ecm, - double xSamples, double ySamples, double zSamples) + public: void Initiallize( + const EntityComponentManager &_ecm) { - auto now = std::chrono::steady_clock::now(); - std::chrono::duration dt(now - this->lastTick); - - if (this->resample) - { - this->CreatePointCloudTopics(data); - this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples); - this->resample = false; - this->lastTick = now; - } - - for (auto &it : this->sessions) - { - auto res = - data->frame[it.first].StepTo(it.second, - std::chrono::duration(_info.simTime).count()); - if (res.has_value()) - { - it.second = res.value(); - } - } - - // Publish at 2 hz for now. In future make reconfigureable. - if (dt.count() > 0.5) - { - this->Visualize(data, xSamples, ySamples, zSamples); - this->Publish(); - lastTick = now; - } + auto world = worldEntity(_ecm); + auto topic = + common::joinPaths( + scopedName(world, _ecm), "environment", "visualize", "res"); + std::lock_guard lock(this->mutex); + this->pcPub = node.Advertise(topic); } ///////////////////////////////////////////////// public: void Visualize( - std::shared_ptr data, - double xSamples, double ySamples, double zSamples) { - - for (auto key : data->frame.Keys()) - { - const auto session = this->sessions[key]; - auto frame = data->frame[key]; - auto [lower_bound, upper_bound] = frame.Bounds(session); - auto step = upper_bound - lower_bound; - auto dx = step.X() / xSamples; - auto dy = step.Y() / ySamples; - auto dz = step.Z() / zSamples; - std::size_t idx = 0; - for (std::size_t x_steps = 0; x_steps < ceil(xSamples); x_steps++) - { - auto x = lower_bound.X() + x_steps * dx; - for (std::size_t y_steps = 0; y_steps < ceil(ySamples); y_steps++) - { - auto y = lower_bound.Y() + y_steps * dy; - for (std::size_t z_steps = 0; z_steps < ceil(zSamples); z_steps++) - { - auto z = lower_bound.Z() + z_steps * dz; - auto res = frame.LookUp(session, math::Vector3d(x, y, z)); - - if (res.has_value()) - { - this->floatFields[key].mutable_data()->Set(idx, - static_cast(res.value())); - } - else - { - this->floatFields[key].mutable_data()->Set(idx, std::nanf("")); - } - idx++; - } - } - } - } - } - - ///////////////////////////////////////////////// - public: void Publish() + double xSamples, double ySamples, double zSamples) { - pcPub.Publish(this->pcMsg); - for(auto &[key, pub] : this->pubs) - { - pub.Publish(this->floatFields[key]); - } + std::lock_guard lock(this->mutex); + this->vec = msgs::Convert(math::Vector3d(xSamples, ySamples, zSamples)); + this->pcPub.Publish(vec); } - ///////////////////////////////////////////////// - public: void ResizeCloud( - std::shared_ptr data, - const EntityComponentManager& _ecm, - unsigned int xSamples, unsigned int ySamples, unsigned int zSamples) - { - assert (pubs.size() > 0); - - // Assume all data have same point cloud. - gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, - {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); - auto numberOfPoints = xSamples * ySamples * zSamples; - std::size_t dataSize{ - static_cast(numberOfPoints * pcMsg.point_step())}; - pcMsg.mutable_data()->resize(dataSize); - pcMsg.set_height(1); - pcMsg.set_width(numberOfPoints); - - auto session = this->sessions[this->pubs.begin()->first]; - auto frame = data->frame[this->pubs.begin()->first]; - auto [lower_bound, upper_bound] = - frame.Bounds(session); + /// \brief The sample resolution + public: gz::msgs::Vector3d vec; - auto step = upper_bound - lower_bound; - auto dx = step.X() / xSamples; - auto dy = step.Y() / ySamples; - auto dz = step.Z() / zSamples; - - // Populate point cloud - gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); - gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); - gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); - - for (std::size_t x_steps = 0; x_steps < xSamples; x_steps++) - { - auto x = lower_bound.X() + x_steps * dx; - for (std::size_t y_steps = 0; y_steps < ySamples; y_steps++) - { - auto y = lower_bound.Y() + y_steps * dy; - for (std::size_t z_steps = 0; z_steps < zSamples; z_steps++) - { - auto z = lower_bound.Z() + z_steps * dz; - auto coords = getGridFieldCoordinates( - _ecm, math::Vector3d{x, y, z}, - data); + /// \brief Publisher to publish sample resolution + public: transport::Node::Publisher pcPub; - if (!coords.has_value()) - { - continue; - } + /// \brief Gz transport node + public: transport::Node node; - auto pos = coords.value(); - *xIter = pos.X(); - *yIter = pos.Y(); - *zIter = pos.Z(); - ++xIter; - ++yIter; - ++zIter; - } - } - } - for (auto key : data->frame.Keys()) - { - this->floatFields[key].mutable_data()->Resize( - numberOfPoints, std::nanf("")); - } - } + /// \brief To synchronize member access. + public: std::mutex mutex; - public: transport::Node::Publisher pcPub; - public: std::unordered_map pubs; - public: std::unordered_map floatFields; - public: transport::Node node; - public: gz::msgs::PointCloudPacked pcMsg; - public: std::unordered_map> sessions; - public: std::chrono::time_point lastTick; + /// \brief first load we need to scan for existing data sensor + public: bool first{true}; }; } } @@ -253,10 +94,14 @@ class EnvironmentVisualizationPrivate ///////////////////////////////////////////////// EnvironmentVisualization::EnvironmentVisualization() - : GuiSystem(), dataPtr(new EnvironmentVisualizationPrivate) + : GuiSystem(), dataPtr(new EnvironmentVisualizationTool) { gui::App()->Engine()->rootContext()->setContextProperty( "EnvironmentVisualization", this); + this->qtimer = new QTimer(this); + connect(qtimer, &QTimer::timeout, + this, &EnvironmentVisualization::ResamplePointcloud); + this->qtimer->start(1000); } ///////////////////////////////////////////////// @@ -274,38 +119,23 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *) } ///////////////////////////////////////////////// -void EnvironmentVisualization::Update(const UpdateInfo &_info, +void EnvironmentVisualization::Update(const UpdateInfo &, EntityComponentManager &_ecm) { - _ecm.EachNew( - [this]( - const Entity &/*_entity*/, - const components::Environment* /*environment*/ - ) { - this->dataPtr->resample = true; - return true; - } - ); - - auto environData = - _ecm.Component( - worldEntity(_ecm)); - - if (environData == nullptr) + if (this->dataPtr->first) { - return; + this->dataPtr->Initiallize(_ecm); + this->dataPtr->first = false; + this->ResamplePointcloud(); } - - this->dataPtr->Step( - _info, environData->Data(), _ecm, - this->xSamples, this->ySamples, this->zSamples - ); } ///////////////////////////////////////////////// void EnvironmentVisualization::ResamplePointcloud() { - this->dataPtr->resample = true; + this->dataPtr->Visualize( + this->xSamples, this->ySamples, this->zSamples + ); } diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh index 91cd420eb9..e5a3eb4397 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh @@ -30,7 +30,7 @@ namespace sim // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { - class EnvironmentVisualizationPrivate; + class EnvironmentVisualizationTool; /// \class EnvironmentVisualization EnvironmentVisualization.hh /// gz/sim/systems/EnvironmentVisualization.hh @@ -66,13 +66,15 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \internal /// \brief Pointer to private data - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; public: unsigned int xSamples{10}; public: unsigned int ySamples{10}; public: unsigned int zSamples{10}; + + private: QTimer* qtimer; }; } } diff --git a/src/systems/environment_preload/CMakeLists.txt b/src/systems/environment_preload/CMakeLists.txt index 178ec5ec32..ed6a4b8efe 100644 --- a/src/systems/environment_preload/CMakeLists.txt +++ b/src/systems/environment_preload/CMakeLists.txt @@ -1,6 +1,7 @@ gz_add_system(environment-preload SOURCES EnvironmentPreload.cc + VisualizationTool.cc PUBLIC_LINK_LIBS gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-common${GZ_COMMON_VER}::io diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 5c678aa2db..5192fa46df 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -15,8 +15,10 @@ * */ #include "EnvironmentPreload.hh" +#include "VisualizationTool.hh" #include +#include #include #include #include @@ -27,6 +29,11 @@ #include +#include + +#include +#include + #include "gz/sim/components/Environment.hh" #include "gz/sim/components/World.hh" #include "gz/sim/Util.hh" @@ -35,72 +42,114 @@ using namespace gz; using namespace sim; using namespace systems; +using Units = msgs::DataLoadPathOptions_DataAngularUnits; ////////////////////////////////////////////////// class gz::sim::systems::EnvironmentPreloadPrivate { + /// \brief Is the file loaded public: bool loaded{false}; + /// \brief SDF Description public: std::shared_ptr sdf; -}; -////////////////////////////////////////////////// -EnvironmentPreload::EnvironmentPreload() - : System(), dataPtr(new EnvironmentPreloadPrivate) -{ -} + /// \brief GzTransport node + public: transport::Node node; -////////////////////////////////////////////////// -EnvironmentPreload::~EnvironmentPreload() = default; + /// \brief Data descriptions + public: msgs::DataLoadPathOptions dataDescription; -////////////////////////////////////////////////// -void EnvironmentPreload::Configure( - const Entity &/*_entity*/, - const std::shared_ptr &_sdf, - EntityComponentManager &/*_ecm*/, - EventManager &/*_eventMgr*/) -{ - this->dataPtr->sdf = _sdf; -} + /// \brief mutex to protect the samples and data description + public: std::mutex mtx; -////////////////////////////////////////////////// -void EnvironmentPreload::PreUpdate( - const gz::sim::UpdateInfo &, - gz::sim::EntityComponentManager &_ecm) -{ - if (!std::exchange(this->dataPtr->loaded, true)) + /// \brief Do we need to reload the system. + public: std::atomic needsReload{false}; + + /// \brief Visualization Helper + public: std::unique_ptr visualizationPtr; + + /// \brief Are visualizations enabled + public: bool visualize{false}; + + /// \brief Sample resolutions + public: math::Vector3d samples; + + /// \brief Is the file loaded + public: bool fileLoaded{false}; + + /// \brief File loading error logger + public: bool logFileLoadError{true}; + + /// \brief Reference to data + public: std::shared_ptr envData; + + ////////////////////////////////////////////////// + public: EnvironmentPreloadPrivate() : + visualizationPtr(new EnvironmentVisualizationTool) {} + + ////////////////////////////////////////////////// + public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg) + { + std::lock_guard lock(this->mtx); + this->dataDescription = _msg; + this->needsReload = true; + this->logFileLoadError = true; + this->visualizationPtr->FileReloaded(); + gzdbg << "Loading file " << _msg.path() << "\n"; + } + + ////////////////////////////////////////////////// + public: void OnVisualResChanged(const msgs::Vector3d &_resChanged) + { + std::lock_guard lock(this->mtx); + if (!this->fileLoaded) + { + // Only visualize if a file exists + return; + } + auto converted = msgs::Convert(_resChanged); + if (this->samples == converted) + { + // If the sample has not changed return. + // This is because resampling is expensive. + return; + } + this->samples = converted; + this->visualize = true; + this->visualizationPtr->resample = true; + } + + ////////////////////////////////////////////////// + public: void ReadSdf(EntityComponentManager &_ecm) { - if (!this->dataPtr->sdf->HasElement("data")) + if (!this->sdf->HasElement("data")) { - gzerr << "No environmental data file was specified"; + gzerr << "No environmental data file was specified" << std::endl; return; } + std::lock_guard lock(mtx); std::string dataPath = - this->dataPtr->sdf->Get("data"); + this->sdf->Get("data"); + if (common::isRelativePath(dataPath)) { - auto * component = + auto *component = _ecm.Component(worldEntity(_ecm)); const std::string rootPath = common::parentPath(component->Data().Element()->FilePath()); dataPath = common::joinPaths(rootPath, dataPath); } + this->dataDescription.set_path(dataPath); - std::ifstream dataFile(dataPath); - if (!dataFile.is_open()) - { - gzerr << "No environmental data file was found at " << dataPath; - return; - } - - components::EnvironmentalData::ReferenceUnits unit{ - components::EnvironmentalData::ReferenceUnits::RADIANS}; + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_RADIANS); std::string timeColumnName{"t"}; bool ignoreTime = false; std::array spatialColumnNames{"x", "y", "z"}; - auto spatialReference = math::SphericalCoordinates::GLOBAL; sdf::ElementConstPtr elem = - this->dataPtr->sdf->FindElement("dimensions"); + this->sdf->FindElement("dimensions"); + msgs::SphericalCoordinatesType spatialReference = + msgs::SphericalCoordinatesType::GLOBAL; if (elem) { if (elem->HasElement("ignore_time")) @@ -120,27 +169,28 @@ void EnvironmentPreload::PreUpdate( elem->Get("reference"); if (referenceName == "global") { - spatialReference = math::SphericalCoordinates::GLOBAL; + spatialReference = msgs::SphericalCoordinatesType::GLOBAL; } else if (referenceName == "spherical") { - spatialReference = math::SphericalCoordinates::SPHERICAL; + spatialReference = msgs::SphericalCoordinatesType::SPHERICAL; if (elem->HasAttribute("units")) { std::string unitName = elem->Get("units"); if (unitName == "degrees") { - unit = components::EnvironmentalData::ReferenceUnits::DEGREES; + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_DEGREES); } else if (unitName != "radians") { - gzerr << "Unrecognized unit " << unitName << "\n"; + ignerr << "Unrecognized unit " << unitName << "\n"; } } } else if (referenceName == "ecef") { - spatialReference = math::SphericalCoordinates::ECEF; + spatialReference = msgs::SphericalCoordinatesType::ECEF; } else { @@ -160,26 +210,143 @@ void EnvironmentPreload::PreUpdate( } } + this->dataDescription.set_static_time(ignoreTime); + this->dataDescription.set_coordinate_type(spatialReference); + this->dataDescription.set_time(timeColumnName); + this->dataDescription.set_x(spatialColumnNames[0]); + this->dataDescription.set_y(spatialColumnNames[1]); + this->dataDescription.set_z(spatialColumnNames[2]); + + this->needsReload = true; + } + + ////////////////////////////////////////////////// + public: components::EnvironmentalData::ReferenceUnits ConvertUnits( + const Units &_unit) + { + switch (_unit) + { + case Units::DataLoadPathOptions_DataAngularUnits_DEGREES: + return components::EnvironmentalData::ReferenceUnits::DEGREES; + case Units::DataLoadPathOptions_DataAngularUnits_RADIANS: + return components::EnvironmentalData::ReferenceUnits::RADIANS; + default: + gzerr << "Invalid unit conversion. Defaulting to radians." << std::endl; + return components::EnvironmentalData::ReferenceUnits::RADIANS; + } + } + + ////////////////////////////////////////////////// + public: void LoadEnvironment(EntityComponentManager &_ecm) + { try { - gzmsg << "Loading Environment Data\n"; + std::lock_guard lock(this->mtx); + std::array spatialColumnNames{ + this->dataDescription.x(), + this->dataDescription.y(), + this->dataDescription.z()}; + + math::SphericalCoordinates::CoordinateType spatialReference = + msgs::Convert(this->dataDescription.coordinate_type()); + auto units = this->ConvertUnits(this->dataDescription.units()); + + std::ifstream dataFile(this->dataDescription.path()); + if (!dataFile.is_open()) + { + if (this->logFileLoadError) + { + gzerr << "No environmental data file was found at " << + this->dataDescription.path() << std::endl; + logFileLoadError = false; + } + return; + } + + gzmsg << "Loading Environment Data " << this->dataDescription.path() << + std::endl; + using ComponentDataT = components::EnvironmentalData; auto data = ComponentDataT::MakeShared( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), - timeColumnName, spatialColumnNames), - spatialReference, unit, ignoreTime); - + this->dataDescription.time(), spatialColumnNames), + spatialReference, units, this->dataDescription.static_time()); + this->envData = data; using ComponentT = components::Environment; auto component = ComponentT{std::move(data)}; _ecm.CreateComponent(worldEntity(_ecm), std::move(component)); + this->visualizationPtr->resample = true; + this->fileLoaded = true; } catch (const std::invalid_argument &exc) { - gzerr << "Failed to load environment data" << std::endl - << exc.what() << std::endl; + if (this->logFileLoadError) + { + gzerr << "Failed to load environment data" << std::endl + << exc.what() << std::endl; + this->logFileLoadError = false; + } } + + this->needsReload = false; + } +}; + +////////////////////////////////////////////////// +EnvironmentPreload::EnvironmentPreload() + : System(), dataPtr(new EnvironmentPreloadPrivate) +{ +} + +////////////////////////////////////////////////// +EnvironmentPreload::~EnvironmentPreload() = default; + +////////////////////////////////////////////////// +void EnvironmentPreload::Configure( + const Entity &/*_entity*/, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->sdf = _sdf; +} + +////////////////////////////////////////////////// +void EnvironmentPreload::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if (!std::exchange(this->dataPtr->loaded, true)) + { + auto world = worldEntity(_ecm); + + // See https://github.com/gazebosim/gz-sim/issues/1786 + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment"), + &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get()); + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment/visualize/res"), + &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get()); + + this->dataPtr->visualizationPtr->resample = true; + this->dataPtr->ReadSdf(_ecm); + } + + if (this->dataPtr->needsReload) + { + this->dataPtr->LoadEnvironment(_ecm); + } + + if (this->dataPtr->visualize) + { + std::lock_guard lock(this->dataPtr->mtx); + auto samples = this->dataPtr->samples; + this->dataPtr->visualizationPtr->Step(_info, _ecm, this->dataPtr->envData, + samples.X(), samples.Y(), samples.Z()); } } diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc new file mode 100644 index 0000000000..75b37b6557 --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -0,0 +1,232 @@ +/* + * 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 "VisualizationTool.hh" + +///////////////////////////////////////////////// +EnvironmentVisualizationTool::EnvironmentVisualizationTool() +{ + this->pcPub = + this->node.Advertise("/point_cloud"); +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info) +{ + this->pubs.clear(); + this->sessions.clear(); + + for (auto key : _data->frame.Keys()) + { + this->pubs.emplace(key, node.Advertise(key)); + gz::msgs::Float_V msg; + this->floatFields.emplace(key, msg); + const double time = std::chrono::duration(_info.simTime).count(); + auto sess = _data->frame[key].CreateSession(time); + if (!_data->frame[key].IsValid(sess)) + { + gzerr << key << "data is out of time bounds. Nothing will be published" + <finishedTime = true; + continue; + } + this->sessions.emplace(key, sess); + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::FileReloaded() +{ + this->finishedTime = false; +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples) +{ + if (this->finishedTime) + { + return; + } + auto now = std::chrono::steady_clock::now(); + std::chrono::duration dt(now - this->lastTick); + + if (this->resample) + { + this->CreatePointCloudTopics(_data, _info); + if (this->finishedTime) { + this->resample = false; + return; + } + this->ResizeCloud(_data, _ecm, _xSamples, _ySamples, _zSamples); + this->resample = false; + this->lastTick = now; + } + + // Progress session pointers to next time stamp + for (auto &it : this->sessions) + { + auto res = + _data->frame[it.first].StepTo(it.second, + std::chrono::duration(_info.simTime).count()); + if (res.has_value()) + { + it.second = res.value(); + } + else + { + gzerr << "Data does not exist beyond this time." + << " Not publishing new environment visuallization data." + << std::endl; + this->finishedTime = true; + return; + } + } + + // Publish at 2 hz for now. In future make reconfigureable. + if (dt.count() > 0.5 && !this->finishedTime) + { + this->Visualize(_data, _xSamples, _ySamples, _zSamples); + this->Publish(); + lastTick = now; + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Visualize( + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples) +{ + for (auto key : _data->frame.Keys()) + { + const auto session = this->sessions[key]; + auto frame = _data->frame[key]; + auto [lower_bound, upper_bound] = frame.Bounds(session); + auto step = upper_bound - lower_bound; + auto dx = step.X() / _xSamples; + auto dy = step.Y() / _ySamples; + auto dz = step.Z() / _zSamples; + std::size_t idx = 0; + for (std::size_t x_steps = 0; x_steps < ceil(_xSamples); x_steps++) + { + auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < ceil(_ySamples); y_steps++) + { + auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < ceil(_zSamples); z_steps++) + { + auto z = lower_bound.Z() + z_steps * dz; + auto res = frame.LookUp(session, math::Vector3d(x, y, z)); + + if (res.has_value()) + { + this->floatFields[key].mutable_data()->Set(idx, + static_cast(res.value())); + } + else + { + this->floatFields[key].mutable_data()->Set(idx, std::nanf("")); + } + idx++; + } + } + } + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Publish() +{ + pcPub.Publish(this->pcMsg); + for (auto &[key, pub] : this->pubs) + { + pub.Publish(this->floatFields[key]); + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::ResizeCloud( + const std::shared_ptr &_data, + const EntityComponentManager &_ecm, + unsigned int _numXSamples, + unsigned int _numYSamples, + unsigned int _numZSamples) +{ + assert(pubs.size() > 0); + + // Assume all data have same point cloud. + gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, + {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); + auto numberOfPoints = _numXSamples * _numYSamples * _numZSamples; + std::size_t dataSize{ + static_cast(numberOfPoints * pcMsg.point_step())}; + pcMsg.mutable_data()->resize(dataSize); + pcMsg.set_height(1); + pcMsg.set_width(numberOfPoints); + + auto session = this->sessions[this->pubs.begin()->first]; + auto frame = _data->frame[this->pubs.begin()->first]; + auto [lower_bound, upper_bound] = frame.Bounds(session); + + auto step = upper_bound - lower_bound; + auto dx = step.X() / _numXSamples; + auto dy = step.Y() / _numYSamples; + auto dz = step.Z() / _numZSamples; + + // Populate point cloud + gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); + gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); + gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); + + for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++) + { + auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < _numYSamples; y_steps++) + { + auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < _numZSamples; z_steps++) + { + auto z = lower_bound.Z() + z_steps * dz; + auto coords = getGridFieldCoordinates(_ecm, math::Vector3d{x, y, z}, + _data); + + if (!coords.has_value()) + { + continue; + } + + auto pos = coords.value(); + *xIter = pos.X(); + *yIter = pos.Y(); + *zIter = pos.Z(); + ++xIter; + ++yIter; + ++zIter; + } + } + } + for (auto key : _data->frame.Keys()) + { + this->floatFields[key].mutable_data()->Resize( + numberOfPoints, std::nanf("")); + } +} diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh new file mode 100644 index 0000000000..bb2e2fce1b --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -0,0 +1,143 @@ +/* + * 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. + * + */ +#ifndef GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ +#define GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/components/Environment.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + +/// \brief This class helps handle point cloud visuallizations +/// of environment data. +class EnvironmentVisualizationTool +{ + /// \brief Environment constructor + public: EnvironmentVisualizationTool(); + + /// \brief To synchronize member access. + private: std::mutex mutex; + + /// \brief First load we need to scan for existing data sensor + private: bool first{true}; + + /// \brief Enable resampling + public: std::atomic resample{true}; + + /// \brief Time has come to an end. + private: bool finishedTime{false}; + + /// \brief Create publisher structures whenever a new environment is made + /// available. + /// \param[in] _data Data to be visuallized + /// \param[in] _info simulation info for current time step + private: void CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info); + + /// \brief Invoke when new file is made available. + public: void FileReloaded(); + + /// \brief Step the visualizations + /// \param[in] _info The simulation info including timestep + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + public: void Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples); + + /// \brief Publishes a sample of the data + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void Visualize( + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples); + + /// \brief Get the point cloud data. + private: void Publish(); + + /// \brief Resize the point cloud structure (used to reallocate + /// memory when resolution changes) + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void ResizeCloud( + const std::shared_ptr &_data, + const EntityComponentManager &_ecm, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); + + /// \brief Publisher for point clouds + private: transport::Node::Publisher pcPub; + + /// \brief Publishers for data + private: std::unordered_map pubs; + + /// \brief Floating point message buffers + private: std::unordered_map floatFields; + + /// \brief GZ buffers + private: transport::Node node; + + /// \brief Point cloud buffer + private: gz::msgs::PointCloudPacked pcMsg; + + /// \brief Session cursors + private: std::unordered_map> sessions; + + /// \brief Duration from last update + private: std::chrono::time_point lastTick; +}; +} +} +} +#endif From 2881041c53d4d682a50574aa56114af944b911ec Mon Sep 17 00:00:00 2001 From: Frederik Markus <80588263+frede791@users.noreply.github.com> Date: Mon, 9 Oct 2023 10:00:25 +0200 Subject: [PATCH 2/7] Lift Drag Bug Fix (#2189) Signed-off-by: frederik --- src/systems/lift_drag/LiftDrag.cc | 42 ++++++++++++++++++----- src/systems/lift_drag/LiftDrag.hh | 57 ++++++++++++++++--------------- 2 files changed, 63 insertions(+), 36 deletions(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index f465e04789..cd58c9f22c 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -40,6 +41,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ExternalWorldWrenchCmd.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Wind.hh" using namespace gz; using namespace sim; @@ -87,6 +89,10 @@ class gz::sim::systems::LiftDragPrivate /// \brief Cm-alpha rate after stall public: double cmaStall = 0.0; + /// \brief How much Cm changes with a change in control + /// surface deflection angle + public: double cm_delta = 0.0; + /// \brief air density /// at 25 deg C it's about 1.1839 kg/m^3 /// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3. @@ -155,6 +161,7 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, this->area = _sdf->Get("area", this->area).first; this->alpha0 = _sdf->Get("a0", this->alpha0).first; this->cp = _sdf->Get("cp", this->cp).first; + this->cm_delta = _sdf->Get("cm_delta", this->cm_delta).first; // blade forward (-drag) direction in link frame this->forward = @@ -206,7 +213,6 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, return; } - if (_sdf->HasElement("control_joint_name")) { auto controlJointName = _sdf->Get("control_joint_name"); @@ -259,6 +265,13 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto worldPose = _ecm.Component(this->linkEntity); + // get wind as a component from the _ecm + components::WorldLinearVelocity *windLinearVel = nullptr; + if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + windLinearVel = + _ecm.Component(windEntity); + } components::JointPosition *controlJointPosition = nullptr; if (this->controlJointEntity != kNullEntity) { @@ -271,7 +284,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto &pose = worldPose->Data(); const auto cpWorld = pose.Rot().RotateVector(this->cp); - const auto vel = worldLinVel->Data() + worldAngVel->Data().Cross(cpWorld); + auto vel = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld); + if (windLinearVel != nullptr){ + vel = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld) - windLinearVel->Data(); + } if (vel.Length() <= 0.01) return; @@ -281,6 +299,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // rotate forward and upward vectors into world frame const auto forwardI = pose.Rot().RotateVector(this->forward); + if (forwardI.Dot(vel) <= 0.0){ + // Only calculate lift or drag if the wind relative velocity + // is in the same direction + return; + } + math::Vector3d upwardI; if (this->radialSymmetry) { @@ -304,7 +328,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) spanwiseI.Dot(velI), minRatio, maxRatio); // get cos from trig identity - const double cosSweepAngle = 1.0 - sinSweepAngle * sinSweepAngle; + double cosSweepAngle = sqrt(1.0 - sinSweepAngle * sinSweepAngle); double sweep = std::asin(sinSweepAngle); // truncate sweep to within +/-90 deg @@ -336,7 +360,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // compute angle between upwardI and liftI // in general, given vectors a and b: - // cos(theta) = a.Dot(b)/(a.Length()*b.Lenghth()) + // cos(theta) = a.Dot(b)/(a.Length()*b.Length()) // given upwardI and liftI are both unit vectors, we can drop the denominator // cos(theta) = a.Dot(b) const double cosAlpha = @@ -435,15 +459,16 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) else cm = this->cma * alpha * cosSweepAngle; - /// \todo(anyone): implement cm - /// for now, reset cm to zero, as cm needs testing - cm = 0.0; + // Take into account the effect of control surface deflection angle to cm + if (controlJointPosition && !controlJointPosition->Data().empty()) + { + cm += this->cm_delta * controlJointPosition->Data()[0]; + } // compute moment (torque) at cp // spanwiseI used to be momentDirection math::Vector3d moment = cm * q * this->area * spanwiseI; - // force and torque about cg in world frame math::Vector3d force = lift + drag; math::Vector3d torque = moment; @@ -530,7 +555,6 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); this->dataPtr->initialized = true; - if (this->dataPtr->validConfig) { Link link(this->dataPtr->linkEntity); diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index 8c7cba996d..a84475854d 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -35,34 +35,37 @@ namespace systems /// \brief The LiftDrag system computes lift and drag forces enabling /// simulation of aerodynamic robots. /// - /// The following parameters are used by the system: + /// ## System Parameters /// - /// link_name : Name of the link affected by the group of lift/drag - /// properties. This can be a scoped name to reference links in - /// nested models. \sa entitiesFromScopedName to learn more - /// about scoped names. - /// air_density : Density of the fluid this model is suspended in. - /// area : Surface area of the link. - /// a0 : The initial "alpha" or initial angle of attack. a0 is also - /// the y-intercept of the alpha-lift coefficient curve. - /// cla : The ratio of the coefficient of lift and alpha slope before - /// stall. Slope of the first portion of the alpha-lift - /// coefficient curve. - /// cda : The ratio of the coefficient of drag and alpha slope before - /// stall. - /// cp : Center of pressure. The forces due to lift and drag will be - /// applied here. - /// forward : 3-vector representing the forward direction of motion in the - /// link frame. - /// upward : 3-vector representing the direction of lift or drag. - /// alpha_stall : Angle of attack at stall point; the peak angle of attack. - /// cla_stall : The ratio of coefficient of lift and alpha slope after - /// stall. Slope of the second portion of the alpha-lift - /// coefficient curve. - /// cda_stall : The ratio of coefficient of drag and alpha slope after - /// stall. - /// control_joint_name: Name of joint that actuates a control surface for this - /// lifting body (Optional) + /// - ``: Name of the link affected by the group of lift/drag + /// properties. This can be a scoped name to reference links in + /// nested models. \sa entitiesFromScopedName to learn more + /// about scoped names. + /// - ``: Density of the fluid this model is suspended in. + /// - ``: Surface area of the link. + /// - ``: The initial "alpha" or initial angle of attack. a0 is also + /// the y-intercept of the alpha-lift coefficient curve. + /// - ``: The ratio of the coefficient of lift and alpha slope before + /// stall. Slope of the first portion of the alpha-lift + /// coefficient curve. + /// - ``: The ratio of the coefficient of drag and alpha slope before + /// stall. + /// - ``: Center of pressure. The forces due to lift and drag will be + /// applied here. + /// - ``: 3-vector representing the forward direction of motion + /// in the link frame. + /// - ``: 3-vector representing the direction of lift or drag. + /// - ``: Angle of attack at stall point; the peak angle + /// of attack. + /// - ``: The ratio of coefficient of lift and alpha slope after + /// stall. Slope of the second portion of the alpha-lift + /// coefficient curve. + /// - ``: The ratio of coefficient of drag and alpha slope after + /// stall. + /// - ``: Name of joint that actuates a control surface + /// for this lifting body (Optional) + /// - ``: How much Cm changes with a change in control + /// surface deflection angle class LiftDrag : public System, public ISystemConfigure, From ba406277f350c4e631578e2cbc74d1424af17118 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 9 Oct 2023 09:35:37 -0700 Subject: [PATCH 3/7] fix INTEGRATION_save_world's SdfGeneratorFixture.ModelWithNestedIncludes test (#2197) Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- test/integration/save_world.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index d2f9fa013d..09fbebffbd 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -422,7 +422,7 @@ TEST_F(SdfGeneratorFixture, ModelWithNestedIncludes) ASSERT_NE(nullptr, uri); ASSERT_NE(nullptr, uri->GetText()); EXPECT_EQ( - "https://fuel.gazebosim.org/1.0/openrobotics/models/coke can/2", + "https://fuel.gazebosim.org/1.0/openrobotics/models/coke can/3", std::string(uri->GetText())); name = include->FirstChildElement("name"); From 8a808009f6672db964acd27e62b710f13535b063 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 9 Oct 2023 18:16:32 -0700 Subject: [PATCH 4/7] Relax pose check in actor no mesh test (#2196) Signed-off-by: Ian Chen --- test/integration/actor_trajectory.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/test/integration/actor_trajectory.cc b/test/integration/actor_trajectory.cc index 8116499686..7a6819ba28 100644 --- a/test/integration/actor_trajectory.cc +++ b/test/integration/actor_trajectory.cc @@ -156,9 +156,12 @@ TEST_F(ActorFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(ActorTrajectoryNoMesh)) std::lock_guard lock(g_mutex); auto it = g_modelPoses.find(boxName); auto &poses = it->second; - for (unsigned int i = 0; i < poses.size()-1; ++i) + for (unsigned int i = 0; i < poses.size()-2; i+=2) { - EXPECT_NE(poses[i], poses[i+1]); + // There could be times when the rendering thread has not updated + // between PostUpdates so two consecutive poses may still be the same. + // So check for diff between every other pose + EXPECT_NE(poses[i], poses[i+2]); } } From ab0669c87b14faedec69e9f768ad5aea32e8b744 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 11 Oct 2023 10:18:05 -0700 Subject: [PATCH 5/7] Fix another deadlock in sensors system (#2141) (#2200) Signed-off-by: Ian Chen --- src/systems/sensors/Sensors.cc | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index e9e43b862f..a774bd6ad6 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -389,9 +389,12 @@ void SensorsPrivate::RunOnce() this->activeSensors.clear(); } - this->updateAvailable = false; this->forceUpdate = false; - this->renderCv.notify_one(); + { + std::unique_lock cvLock(this->renderMutex); + this->updateAvailable = false; + this->renderCv.notify_one(); + } } ////////////////////////////////////////////////// @@ -726,9 +729,12 @@ void Sensors::PostUpdate(const UpdateInfo &_info, this->dataPtr->forceUpdate = (this->dataPtr->renderUtil.PendingSensors() > 0) || hasRenderConnections; + } + { + std::unique_lock cvLock(this->dataPtr->renderMutex); this->dataPtr->updateAvailable = true; + this->dataPtr->renderCv.notify_one(); } - this->dataPtr->renderCv.notify_one(); } } } From 5d1c5058fba0dc6b19eaf29d2782b49abb36e630 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 16 Oct 2023 14:08:35 -0700 Subject: [PATCH 6/7] Fix sensors system parallel updates (#2201) --------- Signed-off-by: Ian Chen --- src/systems/sensors/Sensors.cc | 67 +++++++++++++++++++++++-------- test/integration/reset_sensors.cc | 6 +++ 2 files changed, 57 insertions(+), 16 deletions(-) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a774bd6ad6..9a1b69a24f 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -125,6 +125,9 @@ class gz::sim::systems::SensorsPrivate /// \brief Mutex to protect rendering data public: std::mutex renderMutex; + /// \brief Mutex to protect renderUtil changes + public: std::mutex renderUtilMutex; + /// \brief Condition variable to signal rendering thread /// /// This variable is used to block/unblock operations in the rendering @@ -132,6 +135,13 @@ class gz::sim::systems::SensorsPrivate /// documentation on RenderThread. public: std::condition_variable renderCv; + /// \brief Condition variable to signal update time applied + /// + /// This variable is used to block/unblock operations in PostUpdate thread + /// to make sure renderUtil's ECM updates are applied to the scene first + /// before they are overriden by PostUpdate + public: std::condition_variable updateTimeCv; + /// \brief Connection to events::Stop event, used to stop thread public: common::ConnectionPtr stopConn; @@ -141,6 +151,12 @@ class gz::sim::systems::SensorsPrivate /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; + /// \brief Update time applied in the rendering thread + public: std::chrono::steady_clock::duration updateTimeApplied; + + /// \brief Update time to be appplied in the rendering thread + public: std::chrono::steady_clock::duration updateTimeToApply; + /// \brief Next sensors update time public: std::chrono::steady_clock::duration nextUpdateTime; @@ -297,13 +313,13 @@ void SensorsPrivate::RunOnce() if (!this->scene) return; - std::chrono::steady_clock::duration updateTimeApplied; GZ_PROFILE("SensorsPrivate::RunOnce"); { GZ_PROFILE("Update"); - std::unique_lock timeLock(this->renderMutex); + std::unique_lock timeLock(this->renderUtilMutex); this->renderUtil.Update(); - updateTimeApplied = this->updateTime; + this->updateTimeApplied = this->updateTime; + this->updateTimeCv.notify_one(); } bool activeSensorsEmpty = true; @@ -334,7 +350,7 @@ void SensorsPrivate::RunOnce() { GZ_PROFILE("PreRender"); this->eventManager->Emit(); - this->scene->SetTime(updateTimeApplied); + this->scene->SetTime(this->updateTimeApplied); // Update the scene graph manually to improve performance // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true @@ -361,11 +377,11 @@ void SensorsPrivate::RunOnce() // safety check to see if reset occurred while we're rendering // avoid publishing outdated data if reset occurred std::unique_lock timeLock(this->renderMutex); - if (updateTimeApplied <= this->updateTime) + if (this->updateTimeApplied <= this->updateTime) { // publish data GZ_PROFILE("RunOnce"); - this->sensorManager.RunOnce(updateTimeApplied); + this->sensorManager.RunOnce(this->updateTimeApplied); this->eventManager->Emit(); } @@ -605,8 +621,10 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) s->SetNextDataUpdateTime(_info.simTime); } this->dataPtr->nextUpdateTime = _info.simTime; - std::unique_lock lock(this->dataPtr->renderMutex); + std::unique_lock lock2(this->dataPtr->renderUtilMutex); this->dataPtr->updateTime = _info.simTime; + this->dataPtr->updateTimeToApply = _info.simTime; + this->dataPtr->updateTimeApplied = _info.simTime; } } @@ -668,7 +686,19 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->running && this->dataPtr->initialized) { { - std::unique_lock lock(this->dataPtr->renderMutex); + GZ_PROFILE("UpdateFromECM"); + // Make sure we do not override the state in renderUtil if there are + // still ECM changes that still need to be propagated to the scene, + // i.e. wait until renderUtil.Update(), has taken place in the + // rendering thread + std::unique_lock lock(this->dataPtr->renderUtilMutex); + this->dataPtr->updateTimeCv.wait(lock, [this]() + { + return !this->dataPtr->updateAvailable || + (this->dataPtr->updateTimeToApply == + this->dataPtr->updateTimeApplied); + }); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); this->dataPtr->updateTime = _info.simTime; } @@ -719,16 +749,21 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::unique_lock lockSensors(this->dataPtr->sensorsMutex); this->dataPtr->activeSensors = std::move(this->dataPtr->sensorsToUpdate); + } - this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( - this->dataPtr->sensorsToUpdate, _info.simTime); + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); - // Force scene tree update if there are sensors to be created or - // subscribes to the render events. This does not necessary force - // sensors to update. Only active sensors will be updated - this->dataPtr->forceUpdate = - (this->dataPtr->renderUtil.PendingSensors() > 0) || - hasRenderConnections; + // Force scene tree update if there are sensors to be created or + // subscribes to the render events. This does not necessary force + // sensors to update. Only active sensors will be updated + this->dataPtr->forceUpdate = + (this->dataPtr->renderUtil.PendingSensors() > 0) || + hasRenderConnections; + + { + std::unique_lock timeLock(this->dataPtr->renderUtilMutex); + this->dataPtr->updateTimeToApply = this->dataPtr->updateTime; } { std::unique_lock cvLock(this->dataPtr->renderMutex); diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 592f918380..be904211f9 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -79,6 +79,7 @@ struct MsgReceiver transport::Node node; std::atomic msgReceived = {false}; + std::atomic msgCount = 0; void Start(const std::string &_topic) { this->msgReceived = false; @@ -94,6 +95,7 @@ struct MsgReceiver std::lock_guard lk(this->msgMutex); this->lastMsg = _msg; this->msgReceived = true; + this->msgCount++; } T Last() { @@ -252,6 +254,10 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) EXPECT_FLOAT_EQ(centerPix.G(), centerPix.B()); } + // wait until expected no. of messages are received. + // sim runs for 2000 iterations with camera at 10 Hz + 1 msg at t=0 + while (imageReceiver.msgCount < 21) + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Send command to reset to initial state worldReset(); From b8d16797ef134db95c34343f445d8e56e9a9159f Mon Sep 17 00:00:00 2001 From: Frederik Markus <80588263+frede791@users.noreply.github.com> Date: Thu, 2 Nov 2023 05:35:26 +0100 Subject: [PATCH 7/7] Porting Advanced Lift Drag Plugin to Gazebo (#2185) # Summary This is a plugin that ports the behaviour of the advanced lift drag plugin that was present in Gazebo Classic to Gazebo. The physics implementation have not changed, but the plugin has been adapted to work with the entity component system. Primary modeling differences in the advanced_lift_drag plugin from the original liftdrag_plugin include: - quadratic formulation for drag - side force - flat-plate post-stall model - aerodynamic moments about all three axes - body rate stability derivatives - actuator control derivatives The objective is to provide a more accurate model of a wing than what is provided in the basic lift drag plugin. Signed-off-by: frederik Signed-off-by: Frederik Markus <80588263+frede791@users.noreply.github.com> Signed-off-by: Arjo Chakravarty Co-authored-by: frederik Co-authored-by: Arjo Chakravarty --- examples/worlds/advanced_lift_drag_system.sdf | 686 ++++++++++++++ src/systems/CMakeLists.txt | 1 + .../advanced_lift_drag/AdvancedLiftDrag.cc | 840 ++++++++++++++++++ .../advanced_lift_drag/AdvancedLiftDrag.hh | 152 ++++ src/systems/advanced_lift_drag/CMakeLists.txt | 4 + 5 files changed, 1683 insertions(+) create mode 100644 examples/worlds/advanced_lift_drag_system.sdf create mode 100644 src/systems/advanced_lift_drag/AdvancedLiftDrag.cc create mode 100644 src/systems/advanced_lift_drag/AdvancedLiftDrag.hh create mode 100644 src/systems/advanced_lift_drag/CMakeLists.txt diff --git a/examples/worlds/advanced_lift_drag_system.sdf b/examples/worlds/advanced_lift_drag_system.sdf new file mode 100644 index 0000000000..8b21318720 --- /dev/null +++ b/examples/worlds/advanced_lift_drag_system.sdf @@ -0,0 +1,686 @@ + + + + + + + + + + + + + + + + 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 + + + + + + + 0 0 0.246 0 0 0 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 0.197563 + 0 + 0 + 0.1458929 + 0 + 0.1477 + + + + 0 0 -0.07 0 0 0 + + + 0.47 0.47 0.11 + + + + + + 10 + 0.01 + + + + + + + + + 0.07 0 -0.08 0 0 0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/body.dae + + + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + + + 1 + + 0 + + + 1 + 250 + + + + 1 + 50 + + + + 0 + 0.01 + + + + + + + + 0.3 0 0.0 0 1.57 0 + + 0 0 0 0 0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + 0.0 0 0.0 0 0 0 + + + 0.005 + 0.1 + + + + + + + + + + + + + 0 0 -0.09 0 0 0 + + + 1 1 1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/iris_prop_ccw.dae + + + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + + + 1 + + 0 + + + + rotor_puller + base_link + + 1 0 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 0.3 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/left_aileron.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 -0.3 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/right_aileron.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 0.15 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/left_flap.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 -0.15 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/right_flap.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + -0.5 0 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/elevators.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + -0.5 0 0.05 0 0 0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/rudder.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + base_link + left_elevon + -0.07 0.4 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_0 + servo_0 + 10 + 0 + 0 + + + + base_link + right_elevon + -0.07 -0.4 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_1 + servo_1 + 10 + 0 + 0 + + + + base_link + left_flap + -0.07 0.2 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_4 + servo_4 + 10 + 0 + 0 + + + + base_link + right_flap + -0.07 -0.2 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_5 + servo_5 + 10 + 0 + 0 + + + + base_link + elevator + -0.5 0 0 0 0 0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_2 + servo_2 + 10 + 0 + 0 + + + + base_link + rudder + -0.5 0 0.05 0.00 0 0.0 + + 0 0 1 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_3 + servo_3 + 10 + 0 + 0 + + + + + + + 0.0 + 0.15188 + 6.5 + 0.97 + 5.015 + 0.029 + 0.075 + -0.463966 + -0.258244 + -0.039250 + 0.100826 + 0.0 + 0.065861 + 0.0 + -0.487407 + 0.0 + -0.040416 + 0.055166 + 0.0 + 7.971792 + 0.0 + -12.140140 + 0.0 + 0.0 + 0.230299 + 0.0 + 0.078165 + 0.0 + -0.089947 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + -0.12 0.0 0.0 + 0.34 + 0.22 + 1.2041 + 1 0 0 + 0 0 1 + base_link + 4 + + servo_0 + 0 + 1 + -0.000059 + 0.000171 + -0.011940 + -0.003331 + 0.001498 + -0.000057 + + + servo_1 + 1 + 1 + -0.000059 + -0.000171 + -0.011940 + 0.003331 + 0.001498 + 0.000057 + + + servo_2 + -1 + 2 + 0.000274 + 0 + 0.010696 + 0.0 + -0.025798 + 0.0 + + + servo_3 + 1 + 3 + 0.0 + -0.003913 + 0.0 + -0.000257 + 0.0 + 0.001613 + + + + + rotor_puller_joint + rotor_puller + cw + 0.0125 + 0.025 + 3500 + 8.54858e-06 + 0.01 + command/motor_speed + 0 + 8.06428e-05 + 1e-06 + 10 + velocity + + + servo_0 + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 2f66c9ee22..c51e78b606 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -96,6 +96,7 @@ endfunction() add_subdirectory(ackermann_steering) add_subdirectory(acoustic_comms) +add_subdirectory(advanced_lift_drag) add_subdirectory(air_pressure) add_subdirectory(air_speed) add_subdirectory(altimeter) diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc new file mode 100644 index 0000000000..42d6efc189 --- /dev/null +++ b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc @@ -0,0 +1,840 @@ +/* + * Copyright (C) 2019 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. + * + * @author: Karthik Srivatsan, Frederik Markus + * @version: 1.1 + * + * @brief: this plugin models the lift and drag of an aircraft + * as a single body, using stability, aerodynamic, and control derivatives. + * It takes in a specified number of control surfaces and control + * derivatives are defined/specified with respect to the deflection + * of individual control surfaces. Coefficients for this plugin can be + * obtained using Athena Vortex Lattice (AVL) by Mark Drela + * https://nps.edu/web/adsc/aircraft-aerodynamics2 + * The sign conventions used in this plugin are therefore written + * in a way to be compatible with AVL. + * Force equations are computed in the body, while + * moment equations are computed in the stability frame. + * Has been adapted for Gazebo (Ignition) using the ECS. + * + * + */ + +#include "AdvancedLiftDrag.hh" + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" + +#include "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/Inertial.hh" +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/JointPosition.hh" +#include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ExternalWorldWrenchCmd.hh" +#include "gz/sim/components/Pose.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +class gz::sim::systems::AdvancedLiftDragPrivate +{ + // Initialize the system + public: void Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf); + + /// \brief Initializes lift and drag forces in order to later + /// update the corresponding components + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Update(EntityComponentManager &_ecm); + + /// \brief Compute Control Surface effects + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Comp_Ctrl_surf_eff(EntityComponentManager &_ecm); + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Lift Coefficient at zero angle of attack + public: double CL0 = 0.0; + + /// \brief Drag coefficient at zero angle of attack + public: double CD0 = 0.0; + + /// \brief Pitching moment coefficient at zero angle of attack + public: double Cem0 = 0.0; + + // Get angle-of-attack (alpha) derivatives + /// \brief dCL/da (slope of CL-alpha curve) + public: double CLa = 0.0; + + /// \brief dCy/da (sideforce slope wrt alpha) + public: double CYa = 0.0; + + /// \brief dCl/da (roll moment slope wrt alpha) + public: double Cella = 0.0; + + /// \brief dCm/da (pitching moment slope wrt alpha - before stall) + public: double Cema = 0.0; + + /// \brief dCn/da (yaw moment slope wrt alpha) + public: double Cena = 0.0; + + // Get sideslip angle (beta) derivatives + /// \brief dCL/dbeta (lift coefficient slope wrt beta) + public: double CLb = 0.0; + + /// \brief dCY/dbeta (side force slope wrt beta) + public: double CYb = 0.0; + + /// \brief dCl/dbeta (roll moment slope wrt beta) + public: double Cellb = 0.0; + + /// \brief dCm/dbeta (pitching moment slope wrt beta) + public: double Cemb = 0.0; + + /// \brief dCn/dbeta (yaw moment slope wrt beta) + public: double Cenb = 0.0; + + /// \brief angle of attack when airfoil stalls + public: double alphaStall = GZ_PI_2; + + /// \brief The angle of attack + public: double alpha = 0.0; + + /// \brief The sideslip angle + public: double beta = 0.0; + + /// \brief Slope of the Cm-alpha curve after stall + public: double CemaStall = 0.0; + + /// \brief AVL reference point (it replaces the center of pressure + /// in the original LiftDragPlugin) + public: gz::math::Vector3d cp = math::Vector3d::Zero; + + // Get the derivatives with respect to non-dimensional rates. + // In the next few lines, if you see "roll/pitch/yaw rate", remember it is in + // a non-dimensional form- it is not the actual body rate. + // Also, keep in mind that this CDp is not parasitic drag: that is CD0. + + /// \brief dCD/dp (drag coefficient slope wrt roll rate) + public: double CDp = 0.0; + + /// \brief dCY/dp (sideforce slope wrt roll rate) + public: double CYp = 0.0; + + /// \brief dCL/dp (lift coefficient slope wrt roll rate) + public: double CLp = 0.0; + + /// \brief dCl/dp (roll moment slope wrt roll rate) + public: double Cellp = 0.0; + + /// \brief dCm/dp (pitching moment slope wrt roll rate) + public: double Cemp = 0.0; + + /// \brief dCn/dp (yaw moment slope wrt roll rate) + public: double Cenp = 0.0; + + /// \brief dCD/dq (drag coefficient slope wrt pitching rate) + public: double CDq = 0.0; + + /// \brief dCY/dq (side force slope wrt pitching rate) + public: double CYq = 0.0; + + /// \brief dCL/dq (lift coefficient slope wrt pitching rate) + public: double CLq = 0.0; + + /// \brief dCl/dq (roll moment slope wrt pitching rate) + public: double Cellq = 0.0; + + /// \brief dCm/dq (pitching moment slope wrt pitching rate) + public: double Cemq = 0.0; + + /// \brief dCn/dq (yaw moment slope wrt pitching rate) + public: double Cenq = 0.0; + + /// \brief dCD/dr (drag coefficient slope wrt yaw rate) + public: double CDr = 0.0; + + /// \brief dCY/dr (side force slope wrt yaw rate) + public: double CYr = 0.0; + + /// \brief dCL/dr (lift coefficient slope wrt yaw rate) + public: double CLr = 0.0; + + /// \brief dCl/dr (roll moment slope wrt yaw rate) + public: double Cellr = 0.0; + + /// \brief dCm/dr (pitching moment slope wrt yaw rate) + public: double Cemr = 0.0; + + /// \brief dCn/dr (yaw moment slope wrt yaw rate) + public: double Cenr = 0.0; + + /// \brief Number of present control surfaces + public: int num_ctrl_surfaces = 0; + + /// Initialize storage of control surface properties + /// \brief Joint that the control surface connects to + public: std::vector controlJoints; + + /// \brief Direction the control surface deflects to + public: std::vector ctrl_surface_direction; + + /// \brief Effect of the control surface's deflection on drag + public: std::vector CD_ctrl; + + /// \brief Effect of the control surface's deflection on side force + public: std::vector CY_ctrl; + + /// \brief Effect of the control surface's deflection on lift + public: std::vector CL_ctrl; + + /// \brief Effect of the control surface's deflection on roll moment + public: std::vector Cell_ctrl; + + /// \brief Effect of the control surface's deflection on pitching moment + public: std::vector Cem_ctrl; + + /// \brief Effect of the control surface's deflection on yaw moment + public: std::vector Cen_ctrl; + + /// \brief Add aspect ratio (should that be computed?) + public: double AR = 0.0; + + /// \brief Add mean-aerodynamic chord + public: double mac = 0.0; + + /// \brief Add wing efficiency (Oswald efficiency factor for a 3D wing) + public: double eff = 0.0; + + /// \brief The sigmoid blending parameter + public: double M = 15.0; + + /// \brief coefficients for the flat plate drag model + public: double CD_fp_k1 = -0.224; + public: double CD_fp_k2 = -0.115; + + + /// \brief air density + /// at 25 deg C it's about 1.1839 kg/m^3 + /// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3. + public: double rho = 1.2041; + + /// \brief if the shape is aerodynamically radially symmetric about + /// the forward direction. Defaults to false for wing shapes. + /// If set to true, the upward direction is determined by the + /// angle of attack. + public: bool radialSymmetry = false; + + /// \brief effective planeform surface area + public: double area = 1.0; + + /// \brief Normally, this is taken as a direction parallel to the chord + /// of the airfoil in zero angle of attack forward flight. + public: math::Vector3d forward = math::Vector3d::UnitX; + + /// \brief A vector in the lift/drag plane, perpendicular to the forward + /// vector. Inflow velocity orthogonal to forward and upward vectors + /// is considered flow in the wing sweep direction. + public: math::Vector3d upward = math::Vector3d::UnitZ; + + /// \brief how much to change CL per radian of control surface joint + /// value. + public: double controlJointRadToCL = 4.0; + + /// \brief Link entity targeted this plugin. + public: Entity linkEntity; + + /// \brief Joint entity that actuates a control surface for this lifting body + public: Entity controlJointEntity; + + /// \brief Set during Load to true if the configuration for the system is + /// valid and the post-update can run + public: bool validConfig{false}; + + /// \brief Copy of the sdf configuration used for this plugin + public: sdf::ElementPtr sdfConfig; + + /// \brief Initialization flag + public: bool initialized{false}; +}; + +////////////////////////////////////////////////// +void AdvancedLiftDragPrivate::Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf) +{ + this->CL0 = _sdf->Get("CL0", this->CL0).first; + this->CD0 = _sdf->Get("CD0", this->CD0).first; + this->Cem0 = _sdf->Get("Cem0", this->Cem0).first; + this->CLa = _sdf->Get("CLa", this->CLa).first; + this->CYa = _sdf->Get("CYa", this->CYa).first; + this->Cella = _sdf->Get("Cella", this->Cella).first; + this->Cema = _sdf->Get("Cema", this->Cema).first; + this->Cena = _sdf->Get("Cena", this->Cena).first; + this->CLb = _sdf->Get("CLb", this->CLb).first; + this->CYb = _sdf->Get("CYb", this->CYb).first; + this->Cellb = _sdf->Get("Cellb", this->Cellb).first; + this->Cemb = _sdf->Get("Cemb", this->Cemb).first; + this->Cenb = _sdf->Get("Cenb", this->Cenb).first; + this->alphaStall = _sdf->Get("alpha_stall", this->alphaStall).first; + this->CemaStall = _sdf->Get("Cema_stall", this->CemaStall).first; + this->cp = _sdf->Get("cp", this->cp).first; + this->CDp = _sdf->Get("CDp", this->CDp).first; + this->CYp = _sdf->Get("CYp", this->CYp).first; + this->CLp = _sdf->Get("CLp", this->CLp).first; + this->Cellp = _sdf->Get("Cellp", this->Cellp).first; + this->Cemp = _sdf->Get("Cemp", this->Cemp).first; + this->Cenp = _sdf->Get("Cenp", this->Cenp).first; + + this->CDq = _sdf->Get("CDq", this->CDq).first; + this->CYq = _sdf->Get("CYq", this->CYq).first; + this->CLq = _sdf->Get("CLq", this->CLq).first; + this->Cellq = _sdf->Get("Cellq", this->Cellq).first; + this->Cemq = _sdf->Get("Cemq", this->Cemq).first; + this->Cenq = _sdf->Get("Cenq", this->Cenq).first; + this->CDr = _sdf->Get("CDr", this->CDr).first; + this->CYr = _sdf->Get("CYr", this->CYr).first; + this->CLr = _sdf->Get("CLr", this->CLr).first; + this->Cellr = _sdf->Get("Cellr", this->Cellr).first; + this->Cemr = _sdf->Get("Cemr", this->Cemr).first; + this->Cenr = _sdf->Get("Cenr", this->Cenr).first; + this->num_ctrl_surfaces = _sdf->Get( + "num_ctrl_surfaces", this->num_ctrl_surfaces).first; + + /* + Next, get the properties of each control surface: which joint it connects to, + which direction it deflects in, and the effect of its deflection on the + coefficient of drag, side force, lift, roll moment, pitching moment, and yaw moment. + */ + + while( _sdf->HasElement("control_surface") ) + { + auto curr_ctrl_surface = _sdf->GetElement("control_surface"); + auto ctrl_surface_name = curr_ctrl_surface->GetElement( + "name")->Get(); + auto entities = + entitiesFromScopedName(ctrl_surface_name, _ecm, this->model.Entity()); + + if (entities.empty()) + { + gzerr << "Joint with name[" << ctrl_surface_name << "] not found. " + << "The LiftDrag will not generate forces.\n"; + this->validConfig = false; + return; + } + else if (entities.size() > 1) + { + gzwarn << "Multiple joint entities with name[" << ctrl_surface_name + << "] found. Using the first one.\n"; + } + + controlJointEntity = *entities.begin(); + if (!_ecm.EntityHasComponentType(this->controlJointEntity, + components::Joint::typeId)) + { + this->controlJointEntity = kNullEntity; + gzerr << "Entity with name[" << ctrl_surface_name + << "] is not a joint.\n"; + this->validConfig = false; + return; + } + + this->controlJoints.push_back(controlJointEntity); + this->ctrl_surface_direction.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "direction"))->GetValue())->GetAsString())); + this->CD_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "CD_ctrl"))->GetValue())->GetAsString())); + this->CY_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "CY_ctrl"))->GetValue())->GetAsString())); + this->CL_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "CL_ctrl"))->GetValue())->GetAsString())); + this->Cell_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "Cell_ctrl"))->GetValue())->GetAsString())); + this->Cem_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "Cem_ctrl"))->GetValue())->GetAsString())); + this->Cen_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "Cen_ctrl"))->GetValue())->GetAsString())); + _sdf->RemoveChild(curr_ctrl_surface); + } + + this->AR = _sdf->Get("AR", this->AR).first; + this->mac = _sdf->Get("mac", this->mac).first; + this->eff = _sdf->Get("eff", this->eff).first; + this->rho = _sdf->Get("air_density", this->rho).first; + this->radialSymmetry = _sdf->Get("radial_symmetry", + this->radialSymmetry).first; + this->area = _sdf->Get("area", this->area).first; + + // blade forward (-drag) direction in link frame + this->forward = + _sdf->Get("forward", this->forward).first; + if(std::fabs(this->forward.Length()) >= 1e-9){ + this->forward.Normalize(); + } + + else + { + gzerr << "Forward vector length is zero. This is not valid.\n"; + this->validConfig = false; + return; + } + + // blade upward (+lift) direction in link frame + this->upward = _sdf->Get( + "upward", this->upward) .first; + this->upward.Normalize(); + + this->controlJointRadToCL = _sdf->Get( + "control_joint_rad_to_cl", this->controlJointRadToCL).first; + + if (_sdf->HasElement("link_name")) + { + sdf::ElementPtr elem = _sdf->GetElement("link_name"); + auto linkName = elem->Get(); + auto entities = + entitiesFromScopedName(linkName, _ecm, this->model.Entity()); + + if (entities.empty()) + { + gzerr << "Link with name[" << linkName << "] not found. " + << "The AdvancedLiftDrag will not generate forces.\n"; + this->validConfig = false; + return; + } + else if (entities.size() > 1) + { + gzwarn << "Multiple link entities with name[" << linkName << "] found. " + << "Using the first one.\n"; + } + + this->linkEntity = *entities.begin(); + if (!_ecm.EntityHasComponentType(this->linkEntity, + components::Link::typeId)) + { + this->linkEntity = kNullEntity; + gzerr << "Entity with name[" << linkName << "] is not a link.\n"; + this->validConfig = false; + return; + } + } + else + { + gzerr << "AdvancedLiftDrag system requires the 'link_name' parameter.\n"; + this->validConfig = false; + return; + } + + // If we reached here, we have a valid configuration + this->validConfig = true; +} + +////////////////////////////////////////////////// +AdvancedLiftDrag::AdvancedLiftDrag() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void AdvancedLiftDragPrivate::Update(EntityComponentManager &_ecm) +{ + GZ_PROFILE("AdvancedLiftDragPrivate::Update"); + // get linear velocity at cp in world frame + const auto worldLinVel = + _ecm.Component(this->linkEntity); + const auto worldAngVel = + _ecm.Component(this->linkEntity); + const auto worldPose = + _ecm.Component(this->linkEntity); + + + std::vector controlJointPosition_vec( + this->num_ctrl_surfaces); + // Generate a new vector that contains the current positions for all joints + for(int i = 0; i < this->num_ctrl_surfaces; i++){ + components::JointPosition *controlJointPosition = nullptr; + if(this->controlJoints[i] != kNullEntity){ + controlJointPosition = _ecm.Component( + this->controlJoints[i]); + controlJointPosition_vec[i] = controlJointPosition; + } + } + + if (!worldLinVel || !worldAngVel || !worldPose) + return; + + const auto &pose = worldPose->Data(); + const auto cpWorld = pose.Rot().RotateVector(this->cp); + const auto air_velocity = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld); + + // Define body frame: X forward, Z downward, Y out the right wing + gz::math::Vector3d body_x_axis = pose.Rot().RotateVector(this->forward); + gz::math::Vector3d body_z_axis = -1*(pose.Rot().RotateVector(this->upward)); + gz::math::Vector3d body_y_axis = body_z_axis.Cross(body_x_axis); + + // Get the in-plane velocity (remove spanwise velocity from the velocity + // vector air_velocity) + gz::math::Vector3d velInLDPlane = air_velocity - air_velocity.Dot( + body_y_axis)*body_y_axis; + + // Compute dynamic pressure + const double speedInLDPlane = velInLDPlane.Length(); + + // Define stability frame: X is in-plane velocity, Y is the same as body Y, + // and Z perpendicular to both + if(speedInLDPlane <= 1e-9){ + return; + } + gz::math::Vector3d stability_x_axis = velInLDPlane/speedInLDPlane; + gz::math::Vector3d stability_y_axis = body_y_axis; + gz::math::Vector3d stability_z_axis = stability_x_axis.Cross( + stability_y_axis); + + double span = std::sqrt(this->area*this->AR); + double epsilon = 1e-9; + if (fabs(this->mac - 0.0) <= epsilon){ + // Computing the mean aerodynamic chord involves integrating the square of + // the chord along the span. If this parameter has not been input, this + // plugin will approximate MAC as mean chord. This works for rectangular + // and trapezoidal wings, but for more complex wing shapes, doing the + // integral is preferred. + this->mac = this->area/span; + } + + // Get non-dimensional body rates. Gazebo uses ENU, so some have to be flipped + // gz::math::Vector3d body_rates = this->link->GetRelativeAngularVel(); + components::AngularVelocity *AngVel = nullptr; + if (this->linkEntity != kNullEntity) + { + AngVel = _ecm.Component(this->linkEntity); + } + if(AngVel == nullptr){ + gzerr << "Angular Velocity cannot be null.\n"; + this->validConfig = false; + return; + } + + double rr = AngVel->Data()[0]; // Roll rate + double pr = -1*AngVel->Data()[1]; // Pitch rate + double yr = -1*AngVel->Data()[2]; // Yaw rate + + // Compute angle of attack, alpha, using the stability and body axes + // Project stability x onto body x and z, then take arctan to find alpha + double stabx_proj_bodyx = stability_x_axis.Dot(body_x_axis); + double stabx_proj_bodyz = stability_x_axis.Dot(body_z_axis); + this->alpha = atan2(stabx_proj_bodyz, stabx_proj_bodyx); + + double sinAlpha = sin(this->alpha); + double cosAlpha = cos(this->alpha); + + // Compute sideslip angle, beta + double velSW = air_velocity.Dot(body_y_axis); + double velFW = air_velocity.Dot(body_x_axis); + this->beta = (atan2(velSW, velFW)); + + // Compute dynamic pressure + double dyn_pres = 0.5 * this->rho * speedInLDPlane * speedInLDPlane; + double half_rho_vel = 0.5 * this->rho * speedInLDPlane; + + // Compute CL at cp, check for stall + double CL{0.0}; + + // Use a sigmoid function to blend pre- and post-stall models + double sigma = (1+exp(-1*this->M*(this->alpha-this->alphaStall))+exp( + this->M*(this->alpha+this->alphaStall)))/((1+exp( + -1*this->M*(this->alpha-this->alphaStall)))*(1+exp( + this->M*(this->alpha+this->alphaStall)))); // blending function + + // The lift coefficient (as well as all the other coefficients) are + // defined with the coefficient build-up method and using a quasi-steady + // approach. The first thing that is calculated is the CL of the wing in + // steady conditions (normal CL-alpha curve). Secondly, the effect of the + // roll, pitch, and yaw is added through the AVL stability coefficients. + // Finally, the effect of the control surfaces is added. + + // The lift coefficient of the wing is defined as a combination of a linear + // function for the pre-stall regime and a combination of exponents a + // trigonometric functions for the post-stall regime. Both functions are + // blended in with the sigmoid function. + // CL_prestall = this->CL0 + this->CLa*ths->alpha + // CL_poststall = 2*(this->alpha/abs(this->alpha))*pow(sinAlpha,2.0)*cosAlpha + + CL = (1-sigma)*(this->CL0 + this->CLa*this->alpha) + sigma*( + 2*(this->alpha/abs(this->alpha))*sinAlpha*sinAlpha*cosAlpha); + + // Add sideslip effect, if any + CL = CL + this->CLb*this->beta; + + // Compute control surface effects + double CL_ctrl_tot = 0; + double CD_ctrl_tot = 0; + double CY_ctrl_tot = 0; + double Cell_ctrl_tot = 0; + double Cem_ctrl_tot = 0; + double Cen_ctrl_tot = 0; + + for(int i = 0; i < this->num_ctrl_surfaces; i++){ + double controlAngle = 0.0; + if (controlJointPosition_vec[i] && !controlJointPosition_vec[ + i]->Data().empty()) + { + components::JointPosition *tmp_controlJointPosition = + controlJointPosition_vec[i]; + controlAngle = tmp_controlJointPosition->Data()[0] * 180/M_PI; + } + + // AVL's and Gazebo's direction of "positive" deflection may be different. + // Future users, keep an eye on this. + CL_ctrl_tot += controlAngle*this->CL_ctrl[i]* + this->ctrl_surface_direction[i]; + CD_ctrl_tot += controlAngle*this->CD_ctrl[i]* + this->ctrl_surface_direction[i]; + CY_ctrl_tot += controlAngle*this->CY_ctrl[i]* + this->ctrl_surface_direction[i]; + Cell_ctrl_tot += controlAngle*this->Cell_ctrl[i]* + this->ctrl_surface_direction[i]; + Cem_ctrl_tot += controlAngle*this->Cem_ctrl[i]* + this->ctrl_surface_direction[i]; + Cen_ctrl_tot += controlAngle*this->Cen_ctrl[i]* + this->ctrl_surface_direction[i]; + } + + // AVL outputs a "CZ_elev", but the Z axis is down. This plugin + // uses CL_elev, which is the negative of CZ_elev + CL = CL+CL_ctrl_tot; + + // Compute lift force at cp + gz::math::Vector3d lift = (CL * dyn_pres + (this->CLp * ( + rr*span/2) * half_rho_vel) + (this->CLq * (pr*this->mac/2) * + half_rho_vel) + (this->CLr * (yr*span/2) * half_rho_vel)) * + (this->area * (-1 * stability_z_axis)); + + // Compute CD at cp, check for stall + double CD{0.0}; + + // Add in quadratic model and a high-angle-of-attack (Newtonian) model + // The post stall model used below has two parts. The first part, the + // (estimated) flat plate drag, comes from the data in Ostowari and Naik, + // Post-Stall Wind Tunnel Data for NACA 44XX Series Airfoil Sections. + // https://www.nrel.gov/docs/legosti/old/2559.pdf + // The second part (0.5-0.5cos(2*alpha)) is fitted using data from + // Stringer et al, + // A new 360° airfoil model for predicting airfoil thrust potential in + // vertical-axis wind turbine designs. + // https://aip.scitation.org/doi/pdf/10.1063/1.5011207 + // I halved the drag numbers to make sure it would work with my + // flat plate drag model. + + + // To estimate the flat plate coefficient of drag, I fit a sigmoid function + // to the data in Ostowari and Naik. The form I used was: + // CD_FP = 2/(1+exp(k1+k2*AR)). + // The coefficients k1 and k2 might need some tuning. + // I chose a sigmoid because the CD would initially increase quickly + // with aspect ratio, but that rate of increase would slow down as AR + // goes to infinity. + + double CD_fp = 2 / (1 + exp(this->CD_fp_k1 + this->CD_fp_k2 * ( + std::max(this->AR, 1 / this->AR)))); + CD = (1 - sigma) * (this->CD0 + (CL*CL) / (M_PI * this->AR * + this->eff)) + sigma * abs( + CD_fp * (0.5 - 0.5 * cos(2 * this->alpha))); + + // Add in control surface terms + CD = CD + CD_ctrl_tot; + + // Place drag at cp + gz::math::Vector3d drag = (CD * dyn_pres + (this->CDp * (rr*span/2) * + half_rho_vel) + ( + this->CDq * (pr*this->mac/2) * half_rho_vel) + + (this->CDr * (yr*span/2) * half_rho_vel)) * (this->area * + (-1*stability_x_axis)); + + // Compute sideforce coefficient, CY + // Start with angle of attack, sideslip, and control terms + double CY = this->CYa * this->alpha + this->CYb * this->beta + CY_ctrl_tot; + + gz::math::Vector3d sideforce = (CY * dyn_pres + (this->CYp * ( + rr*span/2) * half_rho_vel) + (this->CYq * (pr*this->mac/2) * + half_rho_vel) + (this->CYr * (yr*span/2) * half_rho_vel)) * + (this->area * stability_y_axis); + + // The Cm is divided in three sections: alpha>stall angle, alpha<-stall + // angle-stall anglestall angle region the Cm is assumed to always be positive or + // zero, in the alpha<-stall angle the Cm is assumed to always be + // negative or zero. + + double Cem{0.0}; + + if (alpha > this->alphaStall) + { + Cem = this->Cem0 + (this->Cema * this->alphaStall + + this->CemaStall * (this->alpha - this->alphaStall)); + } + else if (alpha < -this->alphaStall) + { + Cem = this->Cem0 + (-this->Cema * this->alphaStall + + this->CemaStall * (this->alpha + this->alphaStall)); + } + else + { + Cem = this->Cem0 + this->Cema * this->alpha; + } + // Add sideslip effect, if any + Cem = this->Cemb * this->beta; + + Cem += Cem_ctrl_tot; + + gz::math::Vector3d pm = ((Cem * dyn_pres) + (this->Cemp * ( + rr*span/2) * half_rho_vel) + (this->Cemq * (pr*this->mac/2) * + half_rho_vel) + (this->Cemr * (yr*span/2) * half_rho_vel)) * + (this->area * this->mac * body_y_axis); + + // Compute roll moment coefficient, Cell + // Start with angle of attack, sideslip, and control terms + double Cell = this-> Cella * this->alpha + this->Cellb * + this-> beta + Cell_ctrl_tot; + gz::math::Vector3d rm = ((Cell * dyn_pres) + (this->Cellp * ( + rr*span/2) * half_rho_vel) + (this->Cellq * (pr*this->mac/2) * + half_rho_vel) + (this->Cellr * (yr*span/2) * half_rho_vel)) * + (this->area * span * body_x_axis); + + // Compute yaw moment coefficient, Cen + // Start with angle of attack, sideslip, and control terms + double Cen = this->Cena * this->alpha + this->Cenb * this->beta + + Cen_ctrl_tot; + gz::math::Vector3d ym = ((Cen * dyn_pres) + (this->Cenp * ( + rr*span/2) * half_rho_vel) + (this->Cenq * (pr*this->mac/2) * + half_rho_vel) + (this->Cenr * (yr*span/2) * half_rho_vel)) * + (this->area * span * body_z_axis); + + // Compute moment (torque) + gz::math::Vector3d moment = pm+rm+ym; + + // Compute force about cg in inertial frame + gz::math::Vector3d force = lift + drag + sideforce; + gz::math::Vector3d torque = moment; + + // Correct for nan or inf + force.Correct(); + this->cp.Correct(); + torque.Correct(); + + // positions + const auto totalTorque = torque + cpWorld.Cross(force); + Link link(this->linkEntity); + link.AddWorldWrench(_ecm, force, totalTorque); +} + +////////////////////////////////////////////////// +void AdvancedLiftDrag::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, EventManager &) +{ + this->dataPtr->model = Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) + { + gzerr << "Advanced LiftDrag system should be attached to a model entity." + << "Failed to initialize." << std::endl; + return; + } + this->dataPtr->sdfConfig = _sdf->Clone(); +} + +////////////////////////////////////////////////// +void AdvancedLiftDrag::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("AdvancedLiftDrag::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + gzwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + if (!this->dataPtr->initialized) + { + // We call Load here instead of Configure because we can't be guaranteed + // that all entities have been created when Configure is called + this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); + this->dataPtr->initialized = true; + + if (this->dataPtr->validConfig) + { + Link link(this->dataPtr->linkEntity); + link.EnableVelocityChecks(_ecm, true); + + for(int i = 0; i < this->dataPtr->num_ctrl_surfaces; i++){ + + if ((this->dataPtr->controlJoints[i]!= kNullEntity) && + !_ecm.Component(this->dataPtr-> + controlJoints[i])) + { + _ecm.CreateComponent(this->dataPtr->controlJoints[i], + components::JointPosition()); + } + } + } + } + + if (_info.paused) + return; + + // This is not an "else" because "initialized" can be set in the if block + // above + if (this->dataPtr->initialized && this->dataPtr->validConfig) + { + + this->dataPtr->Update(_ecm); + } +} + +GZ_ADD_PLUGIN(AdvancedLiftDrag, + System, + AdvancedLiftDrag::ISystemConfigure, + AdvancedLiftDrag::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(AdvancedLiftDrag, + "gz::sim::systems::AdvancedLiftDrag") diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.hh b/src/systems/advanced_lift_drag/AdvancedLiftDrag.hh new file mode 100644 index 0000000000..0a5be89e8f --- /dev/null +++ b/src/systems/advanced_lift_drag/AdvancedLiftDrag.hh @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2019 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_ADVANCED_LIFT_DRAG_HH_ +#define GZ_SIM_SYSTEMS_ADVANCED_LIFT_DRAG_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class AdvancedLiftDragPrivate; + + /// \brief The LiftDrag system computes lift and drag forces enabling + /// simulation of aerodynamic robots. + /// + /// + /// A tool can be found at the link below that automatically generates + /// the Advanced Lift Drag plugin for you. All that is needed is to + /// provide some physical parameters of the model. The README contains + /// all necessary setup and usage steps. + /// + /// https://github.com/PX4/PX4-Autopilot/blob/main/Tools/simulation/ + /// gz/tools/avl_automation/ + /// + /// + /// ## System Parameters + /// + /// - ``: Name of the link affected by the group of lift/drag + /// properties. This can be a scoped name to reference links in + /// nested models. \sa entitiesFromScopedName to learn more + /// about scoped names. + /// - ``: Density of the fluid this model is suspended in. + /// - ``: Surface area of the link. + /// - ``: Lift Coefficient at zero angle of attack + /// - ``: Pitching moment coefficient at zero angle of attack + /// - ``: Drag Coefficient at zero angle of attack + /// - ``: dCL/da (slope of CL-alpha curve) Slope of the first + /// portion of the alpha-lift coefficient curve. + /// - ``: dCy/da (sideforce slope wrt alpha) + /// - ``: dCl/da (roll moment slope wrt alpha) + /// - ``: dCm/da (pitching moment slope wrt alpha - before stall) + /// - ``: dCn/da (yaw moment slope wrt alpha) + /// - ``: dCL/dbeta (lift coefficient slope wrt beta) + /// - ``: dCY/dbeta (side force slope wrt beta) + /// - ``: dCl/dbeta (roll moment slope wrt beta) + /// - ``: dCm/dbeta (pitching moment slope wrt beta) + /// - ``: dCn/dbeta (yaw moment slope wrt beta) + /// - ``: angle of attack when airfoil stalls + /// - ``: Slope of the Cm-alpha curve after stall + /// - ``: 3-vector replacing the center of pressure in the original + /// LiftDragPlugin. + /// - ``: dCD/dp (drag coefficient slope wrt roll rate) + /// - ``: dCY/dp (sideforce slope wrt roll rate) + /// - ``: dCL/dp (lift coefficient slope wrt roll rate) + /// - ``: dCl/dp (roll moment slope wrt roll rate) + /// - ``: dCn/dp (yaw moment slope wrt roll rate) + /// - ``: dCD/dq (drag coefficient slope wrt pitching rate) + /// - ``: dCY/dq (side force slope wrt pitching rate) + /// - ``: dCL/dq (lift coefficient slope wrt pitching rate) + /// - ``: dCl/dq (roll moment slope wrt pitching rate) + /// - ``: dCm/dq (pitching moment slope wrt pitching rate) + /// - ``: dCn/dq (yaw moment slope wrt pitching rate) + /// - ``: dCD/dr (drag coefficient slope wrt yaw rate) + /// - ``: dCY/dr (side force slope wrt yaw rate) + /// - ``: dCL/dr (lift coefficient slope wrt yaw rate) + /// - ``: dCl/dr (roll moment slope wrt yaw rate) + /// - ``: dCm/dr (pitching moment slope wrt yaw rate) + /// - ``: dCn/dr (yaw moment slope wrt yaw rate) + /// - ``: Number of control surfaces + /// - ``: Vector that points to the joints that connect to the + /// control surface + /// - ``: Vectors of control surface deflections + /// - ``: Vector of the effect of the deflection on the coefficient + /// of drag + /// - ``: Vector of the effect of the deflection on the coefficient + /// of side force + /// - ``: Vector of the effect of the deflection on the coefficient + /// of lift + /// - ``: Vector of the effect of the deflection on the coefficient + /// of roll moment + /// - ``: Vector of the effect of the deflection on the coefficient + /// of pitching moment + /// - ``: Vector of the effect of the deflection on the coefficient + /// of yaw moment + /// - ``: Aspect ratio + /// - ``: The mean-aerodynamic chord + /// - ``: Wing efficiency (This is the Oswald efficiency factor for a + /// 3D wing) + /// - ``: The ratio of the coefficient of drag and alpha slope before + /// stall. + /// - ``: 3-vector representing the forward direction of motion in + /// the link frame. + /// - ``: 3-vector representing the direction of lift or drag. + /// - ``: Angle of attack at stall point; peak angle of attack. + /// - ``: The angle of attack + /// - ``: The sideslip angle + /// - ``: The sigmoid blending parameter + /// - ``: The first of the flat plate drag model coefficients + /// - ``: The second of the flat plate drag model coefficients + /// - ``: Copy of the sdf configuration used for this plugin + + class AdvancedLiftDrag + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: AdvancedLiftDrag(); + + /// \brief Destructor + public: ~AdvancedLiftDrag() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/src/systems/advanced_lift_drag/CMakeLists.txt b/src/systems/advanced_lift_drag/CMakeLists.txt new file mode 100644 index 0000000000..271f9d06a1 --- /dev/null +++ b/src/systems/advanced_lift_drag/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(advanced-lift-drag + SOURCES + AdvancedLiftDrag.cc +)