From f6efeefb4ab99284d1a053061235f8c2fa02a5da Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 3 Oct 2023 08:54:08 +0800 Subject: [PATCH 01/21] 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 02/21] 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 03/21] 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 04/21] 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 05/21] 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 71d965a6f7c5cbac4341002999c09eeaa90e45e2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 16 Oct 2023 13:58:12 -0500 Subject: [PATCH 06/21] Fix custom_sensor_system example (#2208) Signed-off-by: Addisu Z. Taddese --- examples/plugin/custom_sensor_system/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt index 66ceb25192..32bb92dac3 100644 --- a/examples/plugin/custom_sensor_system/CMakeLists.txt +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -20,7 +20,7 @@ include(FetchContent) FetchContent_Declare( sensors_clone GIT_REPOSITORY https://github.com/gazebosim/gz-sensors - GIT_TAG main + GIT_TAG gz-sensors8 ) FetchContent_Populate(sensors_clone) add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_clone_BINARY_DIR}) From 5d1c5058fba0dc6b19eaf29d2782b49abb36e630 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 16 Oct 2023 14:08:35 -0700 Subject: [PATCH 07/21] 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 4374ae7bd003202152ec9f74101ffbf0780528c5 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Wed, 25 Oct 2023 15:27:21 -0500 Subject: [PATCH 08/21] Implements a method to get the link inertia Signed-off-by: Voldivh --- include/gz/sim/Link.hh | 8 ++++++++ python/src/gz/sim/Link.cc | 3 +++ python/test/link_TEST.py | 6 +++++- src/Link.cc | 17 ++++++++++++++++- test/integration/link.cc | 29 +++++++++++++++++++++++++++++ 5 files changed, 61 insertions(+), 2 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index d67a17d4de..842e287025 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -173,6 +174,13 @@ namespace gz public: std::optional WorldPose( const EntityComponentManager &_ecm) const; + /// \brief Get the world inertia of the link. + /// \param[in] _ecm Entity-component manager. + /// \return Inertia of the link pose in world frame or nullopt + /// if the link does not have the component components::Inertial. + public: std::optional WorldInertial( + const EntityComponentManager &_ecm) const; + /// \brief Get the world pose of the link inertia. /// \param[in] _ecm Entity-component manager. /// \return Inertial pose in world frame or nullopt if the diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index 51a6b15807..1c8bb8381d 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -79,6 +79,9 @@ void defineSimLink(py::object module) .def("world_pose", &gz::sim::Link::WorldPose, py::arg("ecm"), "Get the pose of the link frame in the world coordinate frame.") + .def("world_inertial", &gz::sim::Link::WorldInertial, + py::arg("ecm"), + "Get the inertia of the link in the world frame.") .def("world_inertial_pose", &gz::sim::Link::WorldInertialPose, py::arg("ecm"), "Get the world pose of the link inertia.") diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 35071869e1..94cc03491b 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -18,7 +18,7 @@ from gz_test_deps.common import set_verbosity from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity -from gz_test_deps.math import Matrix3d, Vector3d, Pose3d +from gz_test_deps.math import Inertiald, Matrix3d, Vector3d, Pose3d class TestModel(unittest.TestCase): post_iterations = 0 @@ -59,6 +59,10 @@ def on_pre_udpate_cb(_info, _ecm): # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) + # World Inertial Test + self.assertEqual(Pose3d(), link.world_inertial(_ecm).pose()) + self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertial(_ecm).moi()) + self.assertEqual(10.0, link.world_inertial(_ecm).mass_matrix().mass()) # World Inertial Pose Test. self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) # World Velocity Test diff --git a/src/Link.cc b/src/Link.cc index 426c57e780..fe5d844d7b 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -15,7 +15,6 @@ * */ -#include #include #include #include @@ -190,6 +189,22 @@ std::optional Link::WorldPose( .value_or(sim::worldPose(this->dataPtr->id, _ecm)); } +////////////////////////////////////////////////// +std::optional Link::WorldInertial( + const EntityComponentManager &_ecm) const +{ + auto inertial = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); + + if (!inertial) + return std::nullopt; + + const math::Pose3d &comWorldPose = + worldPose * inertial->Data().Pose(); + return std::make_optional(math::Inertiald(inertial->Data().MassMatrix(), comWorldPose)); +} + ////////////////////////////////////////////////// std::optional Link::WorldInertialPose( const EntityComponentManager &_ecm) const diff --git a/test/integration/link.cc b/test/integration/link.cc index 6603633ae0..245716dace 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -351,6 +351,35 @@ TEST_F(LinkIntegrationTest, LinkAccelerations) ecm.Component(eLink)); } +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, LinkInertia) +{ + EntityComponentManager ecm; + EventManager eventMgr; + SdfEntityCreator creator(ecm, eventMgr); + + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + + ASSERT_TRUE(link.Valid(ecm)); + + // Before we add components, pose functions should return nullopt + EXPECT_EQ(std::nullopt, link.WorldInertial(ecm)); + + math::MassMatrix3d linkMassMatrix(10.0, {0.4, 0.4, 0.4}, {0.02, 0.02, 0.02}); + math::Pose3d linkWorldPose; + linkWorldPose.Set(0.0, 0.1, 0.2, 0.0, GZ_PI_4, GZ_PI_2); + math::Inertiald linkInertial{linkMassMatrix, linkWorldPose}; + ecm.CreateComponent(eLink, components::Inertial(linkInertial)); + + ASSERT_TRUE(link.WorldInertial(ecm)); + EXPECT_EQ(10.0, link.WorldInertial(ecm).value().MassMatrix().Mass()); + EXPECT_EQ(linkWorldPose, link.WorldInertial(ecm).value().Pose()); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, LinkInertiaMatrix) { From 44a8869362e86464a6857aaaa2331e4c63e4c274 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Wed, 25 Oct 2023 15:36:57 -0500 Subject: [PATCH 09/21] Corrects line length according to cpplint Signed-off-by: Voldivh --- src/Link.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Link.cc b/src/Link.cc index fe5d844d7b..4fef211baf 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -202,7 +202,8 @@ std::optional Link::WorldInertial( const math::Pose3d &comWorldPose = worldPose * inertial->Data().Pose(); - return std::make_optional(math::Inertiald(inertial->Data().MassMatrix(), comWorldPose)); + return std::make_optional( + math::Inertiald(inertial->Data().MassMatrix(), comWorldPose)); } ////////////////////////////////////////////////// From 7151c65e957c318ff9205f99c7b5db62e0768899 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 12:33:42 -0500 Subject: [PATCH 10/21] Adds whitespaces between series of numbers Signed-off-by: Voldivh --- python/test/link_TEST.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 94cc03491b..c5994b5e1e 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -61,7 +61,7 @@ def on_pre_udpate_cb(_info, _ecm): self.assertEqual(1, link.visual_count(_ecm)) # World Inertial Test self.assertEqual(Pose3d(), link.world_inertial(_ecm).pose()) - self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertial(_ecm).moi()) + self.assertEqual(Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1), link.world_inertial(_ecm).moi()) self.assertEqual(10.0, link.world_inertial(_ecm).mass_matrix().mass()) # World Inertial Pose Test. self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) From 5f107009bfa2d8fd468f5a994271386404c77032 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 15:19:44 -0500 Subject: [PATCH 11/21] Modifies test for new method Signed-off-by: Voldivh --- python/test/link_TEST.py | 2 +- test/integration/link.cc | 12 +++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index c5994b5e1e..4d6be57631 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -80,7 +80,7 @@ def on_pre_udpate_cb(_info, _ecm): self.assertEqual(Vector3d(), link.world_linear_acceleration(_ecm)) self.assertEqual(Vector3d(), link.world_angular_acceleration(_ecm)) # World Inertia Matrix Test - self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertia_matrix(_ecm)) + self.assertEqual(Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1), link.world_inertia_matrix(_ecm)) # World Kinetic Energy Test self.assertEqual(0, link.world_kinetic_energy(_ecm)) link.enable_velocity_checks(_ecm, False) diff --git a/test/integration/link.cc b/test/integration/link.cc index 245716dace..6f9f3081ba 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -371,13 +371,19 @@ TEST_F(LinkIntegrationTest, LinkInertia) math::MassMatrix3d linkMassMatrix(10.0, {0.4, 0.4, 0.4}, {0.02, 0.02, 0.02}); math::Pose3d linkWorldPose; - linkWorldPose.Set(0.0, 0.1, 0.2, 0.0, GZ_PI_4, GZ_PI_2); - math::Inertiald linkInertial{linkMassMatrix, linkWorldPose}; + linkWorldPose.Set(1.0, 0.0, 0.0, 0, 0, GZ_PI_4); + // This is the pose of the inertia frame relative to its parent link frame + math::Pose3d inertiaPose; + inertiaPose.Set(1.0, 2.0, 3.0, 0, GZ_PI_2, 0); + + math::Inertiald linkInertial{linkMassMatrix, inertiaPose}; + + ecm.CreateComponent(eLink, components::WorldPose(linkWorldPose)); ecm.CreateComponent(eLink, components::Inertial(linkInertial)); ASSERT_TRUE(link.WorldInertial(ecm)); EXPECT_EQ(10.0, link.WorldInertial(ecm).value().MassMatrix().Mass()); - EXPECT_EQ(linkWorldPose, link.WorldInertial(ecm).value().Pose()); + EXPECT_EQ(linkWorldPose * inertiaPose, link.WorldInertial(ecm).value().Pose()); } ////////////////////////////////////////////////// From 06287649383f57d1dd3d8bf05e37b51d00cd0bf2 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 15:22:07 -0500 Subject: [PATCH 12/21] Removes whiteline Signed-off-by: Voldivh --- test/integration/link.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/test/integration/link.cc b/test/integration/link.cc index 6f9f3081ba..5536658b18 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -375,7 +375,6 @@ TEST_F(LinkIntegrationTest, LinkInertia) // This is the pose of the inertia frame relative to its parent link frame math::Pose3d inertiaPose; inertiaPose.Set(1.0, 2.0, 3.0, 0, GZ_PI_2, 0); - math::Inertiald linkInertial{linkMassMatrix, inertiaPose}; ecm.CreateComponent(eLink, components::WorldPose(linkWorldPose)); From 03be14fdd25da01c914b87d6445657c1204f4435 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 15:40:04 -0500 Subject: [PATCH 13/21] Reduce line length for cpplint Signed-off-by: Voldivh --- test/integration/link.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/link.cc b/test/integration/link.cc index 5536658b18..9e140a9499 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -382,7 +382,8 @@ TEST_F(LinkIntegrationTest, LinkInertia) ASSERT_TRUE(link.WorldInertial(ecm)); EXPECT_EQ(10.0, link.WorldInertial(ecm).value().MassMatrix().Mass()); - EXPECT_EQ(linkWorldPose * inertiaPose, link.WorldInertial(ecm).value().Pose()); + EXPECT_EQ(linkWorldPose * inertiaPose, + link.WorldInertial(ecm).value().Pose()); } ////////////////////////////////////////////////// From d693df6dc60fe00b5f64140873d18283ee8d334f Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 27 Oct 2023 23:36:10 -0400 Subject: [PATCH 14/21] Standardize Doxygen parameter formatting for systems A-N (#2183) Signed-off-by: Mabel Zhang --- .../ackermann_steering/AckermannSteering.hh | 72 ++++++++--------- src/systems/acoustic_comms/AcousticComms.hh | 2 + .../battery_plugin/LinearBatteryPlugin.hh | 27 ++++--- src/systems/breadcrumbs/Breadcrumbs.hh | 4 +- src/systems/buoyancy/Buoyancy.hh | 5 +- src/systems/buoyancy_engine/BuoyancyEngine.hh | 51 ++++++------ .../CameraVideoRecorder.hh | 28 ++++--- src/systems/comms_endpoint/CommsEndpoint.hh | 28 ++++--- .../detachable_joint/DetachableJoint.hh | 2 +- src/systems/diff_drive/DiffDrive.hh | 70 ++++++++-------- src/systems/elevator/Elevator.hh | 16 ++-- .../environment_preload/EnvironmentPreload.hh | 5 +- src/systems/follow_actor/FollowActor.hh | 23 +++--- src/systems/hydrodynamics/Hydrodynamics.hh | 2 +- .../joint_controller/JointController.hh | 54 +++++++------ .../JointPositionController.hh | 54 ++++++------- .../JointStatePublisher.hh | 7 +- .../JointTrajectoryController.hh | 24 +++--- .../KineticEnergyMonitor.hh | 10 +-- src/systems/lens_flare/LensFlare.hh | 27 ++++--- src/systems/lift_drag/LiftDrag.hh | 55 ++++++------- src/systems/log/LogPlayback.hh | 5 +- .../log_video_recorder/LogVideoRecorder.hh | 19 +++-- .../LogicalAudioSensorPlugin.hh | 2 + src/systems/mecanum_drive/MecanumDrive.hh | 33 ++++---- .../MulticopterVelocityControl.hh | 79 ++++++++++--------- src/systems/pose_publisher/PosePublisher.hh | 46 +++++------ src/systems/user_commands/UserCommands.hh | 26 +++--- 28 files changed, 399 insertions(+), 377 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index f82e0cec20..3d829d8902 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -36,106 +36,106 @@ namespace systems /// \brief Ackermann steering controller which can be attached to a model /// with any number of left and right wheels. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Boolean used to only control the steering angle + /// - ``: Boolean used to only control the steering angle /// only. Calculates the angles of wheels from steering_limit, wheel_base, /// and wheel_separation. Uses gz::msg::Double on default topic name /// `/model/{name_of_model}/steer_angle`. Automatically set True when /// `` is True, uses defaults topic name "/actuators". /// - /// `` True to enable the use of actutor message + /// - `` True to enable the use of actutor message /// for steering angle command. Relies on `` for the /// index of the position actuator and defaults to topic "/actuators". /// - /// `` used with `` to set + /// - `` used with `` to set /// the index of the steering angle position actuator. /// - /// ``: Float used to control the steering angle P gain. + /// - ``: Float used to control the steering angle P gain. /// Only used for when in steering_only mode. /// - /// ``: Name of a joint that controls a left wheel. This + /// - ``: Name of a joint that controls a left wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a right wheel. This + /// - ``: Name of a joint that controls a right wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls steering for - /// left wheel. This element must appear once. Vehicles that steer - /// rear wheels are not currently correctly supported. + /// - ``: Name of a joint that controls steering for + /// left wheel. This element must appear once. Vehicles that steer + /// rear wheels are not currently correctly supported. /// - /// ``: Name of a joint that controls steering for - /// right wheel. This element must appear once. + /// - ``: Name of a joint that controls steering for + /// right wheel. This element must appear once. /// - /// ``: Distance between wheels, in meters. This element + /// - ``: Distance between wheels, in meters. This element /// is optional, although it is recommended to be included with an /// appropriate value. The default value is 1.0m. /// - /// ``: Distance between wheel kingpins, in meters. This + /// - ``: Distance between wheel kingpins, in meters. This /// element is optional, although it is recommended to be included with an /// appropriate value. The default value is 0.8m. Generally a bit smaller /// than the wheel_separation. /// - /// ``: Distance between front and rear wheels, in meters. This + /// - ``: Distance between front and rear wheels, in meters. This /// element is optional, although it is recommended to be included with an /// appropriate value. The default value is 1.0m. /// - /// ``: Value to limit steering to. Can be calculated by + /// - ``: Value to limit steering to. Can be calculated by /// measuring the turning radius and wheel_base of the real vehicle. /// steering_limit = asin(wheel_base / turning_radius) /// The default value is 0.5 radians. /// - /// ``: Wheel radius in meters. This element is optional, + /// - ``: Wheel radius in meters. This element is optional, /// although it is recommended to be included with an appropriate value. The /// default value is 0.2m. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Minimum velocity [m/s], usually <= 0. - /// ``: Maximum velocity [m/s], usually >= 0. - /// ``: Minimum acceleration [m/s^2], usually <= 0. - /// ``: Maximum acceleration [m/s^2], usually >= 0. - /// ``: jerk [m/s^3], usually <= 0. - /// ``: jerk [m/s^3], usually >= 0. + /// - ``: Minimum velocity [m/s], usually <= 0. + /// - ``: Maximum velocity [m/s], usually >= 0. + /// - ``: Minimum acceleration [m/s^2], usually <= 0. + /// - ``: Maximum acceleration [m/s^2], usually >= 0. + /// - ``: jerk [m/s^3], usually <= 0. + /// - ``: jerk [m/s^3], usually >= 0. /// - /// ``: Custom topic that this system will subscribe to in order to + /// - ``: Custom topic that this system will subscribe to in order to /// receive command messages. This element is optional, and the /// default value is `/model/{name_of_model}/cmd_vel` or when steering_only /// is true `/model/{name_of_model}/steer_angle`. /// - /// ``: Custom sub_topic that this system will subscribe to in + /// - ``: Custom sub_topic that this system will subscribe to in /// order to receive command messages. This element is optional, and /// creates a topic `/model/{name_of_model}/{sub_topic}` /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `frame_id` to `child_frame_id`. This element is optional, /// and the default value is `/model/{name_of_model}/tf`. /// - /// ``: Custom `frame_id` field that this system will use as the + /// - ``: Custom `frame_id` field that this system will use as the /// origin of the odometry transform in both the `` /// `gz.msgs.Pose_V` message and the `` /// `gz.msgs.Odometry` message. This element if optional, and the /// default value is `{name_of_model}/odom`. /// - /// ``: Custom `child_frame_id` that this system will use as + /// - ``: Custom `child_frame_id` that this system will use as /// the target of the odometry transform in both the `` - /// `gz.msgs.Pose_V` message and the `` - /// `gz.msgs.Odometry` message. This element if optional, + /// gz.msgs.Pose_V message and the `` + /// gz.msgs.Odometry message. This element is optional, /// and the default value is `{name_of_model}/{name_of_link}`. /// /// A robot with rear drive and front steering would have one each /// of left_joint, right_joint, left_steering_joint and - /// right_steering_joint + /// right_steering_joint. /// /// References: - /// https://github.com/gazebosim/gz-sim/tree/main/src/systems/ackermann_steering - /// https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf - /// https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ + /// - https://github.com/gazebosim/gz-sim/tree/main/src/systems/ackermann_steering + /// - https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf + /// - https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ class AckermannSteering diff --git a/src/systems/acoustic_comms/AcousticComms.hh b/src/systems/acoustic_comms/AcousticComms.hh index 2845436cef..45e7db8845 100644 --- a/src/systems/acoustic_comms/AcousticComms.hh +++ b/src/systems/acoustic_comms/AcousticComms.hh @@ -76,6 +76,7 @@ namespace systems /// * : Seed value to be used for random sampling. /// /// Here's an example: + /// ``` /// @@ -93,6 +94,7 @@ namespace systems /// /// /// + /// ``` class AcousticComms: public gz::sim::comms::ICommsModel diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.hh b/src/systems/battery_plugin/LinearBatteryPlugin.hh index f82afa7515..2f142d37e5 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.hh +++ b/src/systems/battery_plugin/LinearBatteryPlugin.hh @@ -39,12 +39,13 @@ namespace systems /// \brief A plugin for simulating battery usage /// - /// This system processes the following sdf parameters: + /// ## System parameters + /// /// - `` name of the battery (required) /// - `` Initial voltage of the battery (required) /// - `` Voltage at full charge /// - `` Amount of voltage decrease when no - /// charge + /// charge /// - `` Initial charge of the battery (Ah) /// - `` Total charge that the battery can hold (Ah) /// - `` Internal resistance (Ohm) @@ -52,22 +53,22 @@ namespace systems /// - `` power load on battery (required) (Watts) /// - `` If true, the battery can be recharged /// - `` If true, the start/stop signals for recharging the - /// battery will also be available via topics. The - /// regular Gazebo services will still be available. + /// battery will also be available via topics. The + /// regular Gazebo services will still be available. /// - `` Hours taken to fully charge the battery. - /// (Required if `` is set to true) + /// (Required if `` is set to true) /// - `` True to change the battery behavior to fix some issues - /// described in https://github.com/gazebosim/gz-sim/issues/225. + /// described in https://github.com/gazebosim/gz-sim/issues/225. /// - `` Whether to start draining the battery right away. - /// False by default. + /// False by default. /// - `` A topic that is used to start battery - /// discharge. Any message on the specified topic will cause the battery to - /// start draining. This element can be specified multiple times if - /// multiple topics should be monitored. Note that this mechanism will - /// start the battery draining, and once started will keep drainig. + /// discharge. Any message on the specified topic will cause the battery to + /// start draining. This element can be specified multiple times if + /// multiple topics should be monitored. Note that this mechanism will + /// start the battery draining, and once started will keep drainig. /// - `` A topic that is used to stop battery - /// discharge. Any message on the specified topic will cause the battery to - /// stop draining. + /// discharge. Any message on the specified topic will cause the battery to + /// stop draining. class LinearBatteryPlugin : public System, diff --git a/src/systems/breadcrumbs/Breadcrumbs.hh b/src/systems/breadcrumbs/Breadcrumbs.hh index 5e5b3f64e4..0e09180263 100644 --- a/src/systems/breadcrumbs/Breadcrumbs.hh +++ b/src/systems/breadcrumbs/Breadcrumbs.hh @@ -56,7 +56,7 @@ namespace systems /// model using the `` tag. /// See the example in examples/worlds/breadcrumbs.sdf. /// - /// System Paramters + /// ## System Parameters /// /// - ``: Custom topic to be used to deploy breadcrumbs. If topic is /// not set, the default topic with the following pattern would be used @@ -82,7 +82,7 @@ namespace systems /// be deployed. Defaults to false. /// - ``: This is the model used as a template for deploying /// breadcrumbs. - /// ``: If true, then topic statistics are enabled on + /// - ``: If true, then topic statistics are enabled on /// `` and error messages will be generated when messages are /// dropped. Default to false. class Breadcrumbs diff --git a/src/systems/buoyancy/Buoyancy.hh b/src/systems/buoyancy/Buoyancy.hh index 5eae0f0864..bf89e01efb 100644 --- a/src/systems/buoyancy/Buoyancy.hh +++ b/src/systems/buoyancy/Buoyancy.hh @@ -67,7 +67,8 @@ namespace systems /// /// ## Examples /// - /// ### `uniform_fluid_density` world. + /// ### uniform_fluid_density world + /// /// The `buoyancy.sdf` SDF file contains three submarines. The first /// submarine is neutrally buoyant, the second sinks, and the third /// floats. To run: @@ -76,7 +77,7 @@ namespace systems /// gz sim -v 4 buoyancy.sdf /// ``` /// - /// ### `graded_buoyancy` world + /// ### graded_buoyancy world /// /// Often when simulating a maritime environment one may need to simulate both /// surface and underwater vessels. This means the buoyancy plugin needs to diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.hh b/src/systems/buoyancy_engine/BuoyancyEngine.hh index ab0ad73324..422f5a703a 100644 --- a/src/systems/buoyancy_engine/BuoyancyEngine.hh +++ b/src/systems/buoyancy_engine/BuoyancyEngine.hh @@ -38,32 +38,35 @@ namespace systems /// `/model/{namespace}/buoyancy_engine` topic for the volume of the bladder /// in *cubicmeters*. /// - /// ## Parameters - /// - The link which the plugin is attached to [required, string] - /// - The namespace for the topic. If empty the plugin will listen - /// on `buoyancy_engine` otherwise it listens on + /// ## System Parameters + /// - ``: The link which the plugin is attached to [required, + /// string] + /// - ``: The namespace for the topic. If empty the plugin will + /// listen on `buoyancy_engine` otherwise it listens on /// `/model/{namespace}/buoyancy_engine` [optional, string] - /// - Minimum volume of the engine [optional, float, + /// - ``: Minimum volume of the engine [optional, float, /// default=0.00003m^3] - /// - At this volume the engine has neutral buoyancy. Used to - /// estimate the weight of the engine [optional, float, default=0.0003m^3] - /// - The volume which the engine starts at [optional, float, + /// - ``: At this volume the engine has neutral buoyancy. Used + /// to estimate the weight of the engine [optional, float, /// default=0.0003m^3] - /// - Maximum volume of the engine [optional, float, + /// - ``: The volume which the engine starts at [optional, + /// float, default=0.0003m^3] + /// - ``: Maximum volume of the engine [optional, float, /// default=0.00099m^3] - /// - Maximum inflation rate for bladder [optional, + /// - ``: Maximum inflation rate for bladder [optional, /// float, default=0.000003m^3/s] - /// - The fluid density of the liquid its suspended in kgm^-3. - /// [optional, float, default=1000kgm^-3] - /// - The Z height in metres at which the surface of the water is. - /// If not defined then there is no surface [optional, float] + /// - ``: The fluid density of the liquid its suspended in + /// kgm^-3. [optional, float, default=1000kgm^-3] + /// - ``: The Z height in metres at which the surface of the water + /// is. If not defined then there is no surface [optional, float] /// /// ## Topics - /// * Subscribes to a gz::msgs::Double on `buoyancy_engine` or - /// `/model/{namespace}/buoyancy_engine`. This is the set point for the - /// engine. - /// * Publishes a gz::msgs::Double on `buoyancy_engine` or - /// `/model/{namespace}/buoyancy_engine/current_volume` on the current volume + /// - Subscribes to a gz::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine`. This is the set point for the + /// engine. + /// - Publishes a gz::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine/current_volume` on the current + /// volume /// /// ## Examples /// To get started run: @@ -72,18 +75,18 @@ namespace systems /// ``` /// Enter the following in a separate terminal: /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double + /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ /// -p "data: 0.003" /// ``` - /// To see the box float up. + /// to see the box float up. /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double + /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ /// -p "data: 0.001" /// ``` - /// To see the box go down. + /// to see the box go down. /// To see the current volume enter: /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/current_volume -e + /// gz topic -t /model/buoyant_box/buoyancy_engine/current_volume -e /// ``` class BuoyancyEnginePlugin: public gz::sim::System, diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.hh b/src/systems/camera_video_recorder/CameraVideoRecorder.hh index f19369cd13..2d3115e2a0 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.hh +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.hh @@ -34,23 +34,25 @@ namespace systems /** \class CameraVideoRecorder CameraVideoRecorder.hh \ * gz/sim/systems/CameraVideoRecorder.hh **/ + /// /// \brief Record video from a camera sensor - /// The system takes in the following parameter: - /// Name of topic for the video recorder service. If this is - /// not specified, the topic defaults to: - /// /world//link// - /// sensor//record_video /// - /// True/false value that specifies if the video should - /// be recorded using simulation time or real time. The - /// default is false, which indicates the use of real - /// time. + /// ## System Parameters + /// + /// - ``: Name of topic for the video recorder service. If this is + /// not specified, the topic defaults to: + /// /world//link// + /// sensor//record_video + /// + /// - ``: True/false value that specifies if the video should + /// be recorded using simulation time or real time. The default is false, + /// which indicates the use of real time. /// - /// Video recorder frames per second. The default value is 25, and - /// the support type is unsigned int. + /// - ``: Video recorder frames per second. The default value is 25, and + /// the support type is unsigned int. /// - /// Video recorder bitrate (bps). The default value is - /// 2070000 bps, and the supported type is unsigned int. + /// - ``: Video recorder bitrate (bps). The default value is + /// 2070000 bps, and the supported type is unsigned int. class CameraVideoRecorder final: public System, public ISystemConfigure, diff --git a/src/systems/comms_endpoint/CommsEndpoint.hh b/src/systems/comms_endpoint/CommsEndpoint.hh index 30dacdb560..4e43b7bbba 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.hh +++ b/src/systems/comms_endpoint/CommsEndpoint.hh @@ -36,22 +36,23 @@ namespace systems /// running. The system will bind this address in the broker automatically /// for you and unbind it when the model is destroyed. /// - /// The endpoint can be configured with the following SDF parameters: + /// ## System Parameters /// - /// * Required parameters: - ///
An identifier used to receive messages (string). - /// The topic name where you want to receive the messages targeted to - /// this address. + /// Required parameters: + /// - `
`: An identifier used to receive messages (string). + /// - ``: The topic name where you want to receive the messages + /// targeted to this address. /// - /// * Optional parameters: - /// Element used to capture where are the broker services. - /// This block can contain any of the next optional parameters: - /// : Service name used to bind an address. - /// The default value is "/broker/bind" - /// : Service name used to unbind from an address. - /// The default value is "/broker/unbind" + /// Optional parameters: + /// - ``: Element used to capture where are the broker services. + /// This block can contain any of the next optional parameters: + /// - ``: Service name used to bind an address. + /// The default value is "/broker/bind" + /// - ``: Service name used to unbind from an address. + /// The default value is "/broker/unbind" /// - /// Here's an example: + /// ## Example + /// ``` /// @@ -62,6 +63,7 @@ namespace systems /// /broker/unbind /// /// + /// ``` class CommsEndpoint : public System, public ISystemConfigure, diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index e6cb491285..c207b2d2ac 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -40,7 +40,7 @@ namespace systems /// model can be re-attached during simulation via a topic. The status of the /// detached state can be monitored via a topic as well. /// - /// Parameters: + /// ## System Parameters /// /// - ``: Name of the link in the parent model to be used in /// creating a fixed joint with a link in the child model. diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index 150621bb7c..0d16498e92 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -35,99 +35,99 @@ namespace systems /// \brief Differential drive controller which can be attached to a model /// with any number of left and right wheels. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of a joint that controls a left wheel. This + /// - ``: Name of a joint that controls a left wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a right wheel. This + /// - ``: Name of a joint that controls a right wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Distance between wheels, in meters. This element + /// - ``: Distance between wheels, in meters. This element /// is optional, although it is recommended to be included with an /// appropriate value. The default value is 1.0m. /// - /// ``: Wheel radius in meters. This element is optional, + /// - ``: Wheel radius in meters. This element is optional, /// although it is recommended to be included with an appropriate value. The /// default value is 0.2m. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Custom topic that this system will subscribe to in order to + /// - ``: Custom topic that this system will subscribe to in order to /// receive command velocity messages. This element if optional, and the /// default value is `/model/{name_of_model}/cmd_vel`. /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `frame_id` to `child_frame_id`. This element is optional, /// and the default value is `/model/{name_of_model}/tf`. /// - /// ``: Custom `frame_id` field that this system will use as the + /// - ``: Custom `frame_id` field that this system will use as the /// origin of the odometry transform in both the `` - /// `gz.msgs.Pose_V` message and the `` - /// `gz.msgs.Odometry` message. This element if optional, and the + /// gz.msgs.Pose_V message and the `` + /// gz.msgs.Odometry message. This element if optional, and the /// default value is `{name_of_model}/odom`. /// - /// ``: Custom `child_frame_id` that this system will use as + /// - ``: Custom `child_frame_id` that this system will use as /// the target of the odometry trasnform in both the `` - /// `gz.msgs.Pose_V` message and the `` - /// `gz.msgs.Odometry` message. This element if optional, + /// gz.msgs.Pose_V message and the `` + /// gz.msgs.Odometry message. This element if optional, /// and the default value is `{name_of_model}/{name_of_link}`. /// - /// ``: Sets both the minimum linear and minimum angular + /// - ``: Sets both the minimum linear and minimum angular /// velocity. /// - /// ``: Sets both the maximum linear and maximum angular + /// - ``: Sets both the maximum linear and maximum angular /// velocity. /// - /// ``: Sets both the minimum linear and minimum angular + /// - ``: Sets both the minimum linear and minimum angular /// acceleration. /// - /// ``: Sets both the maximum linear and maximum angular + /// - ``: Sets both the maximum linear and maximum angular /// acceleration. /// - /// ``: Sets both the minimum linear and minimum angular jerk. + /// - ``: Sets both the minimum linear and minimum angular jerk. /// - /// ``: Sets both the maximum linear and maximum angular jerk. + /// - ``: Sets both the maximum linear and maximum angular jerk. /// - /// ``: Sets the minimum linear velocity. Overrides + /// - ``: Sets the minimum linear velocity. Overrides /// `` if set. /// - /// ``: Sets the maximum linear velocity. Overrides + /// - ``: Sets the maximum linear velocity. Overrides /// `` if set. /// - /// ``: Sets the minimum angular velocity. Overrides + /// - ``: Sets the minimum angular velocity. Overrides /// `` if set. /// - /// ``: Sets the maximum angular velocity. Overrides + /// - ``: Sets the maximum angular velocity. Overrides /// `` if set. /// - /// ``: Sets the minimum linear acceleration. + /// - ``: Sets the minimum linear acceleration. /// Overrides `` if set. /// - /// ``: Sets the maximum linear acceleration. + /// - ``: Sets the maximum linear acceleration. /// Overrides `` if set. /// - /// ``: Sets the minimum angular acceleration. + /// - ``: Sets the minimum angular acceleration. /// Overrides `` if set. /// - /// ``: Sets the maximum angular acceleration. + /// - ``: Sets the maximum angular acceleration. /// Overrides `` if set. /// - /// ``: Sets the minimum linear jerk. Overrides `` - /// if set. + /// - ``: Sets the minimum linear jerk. Overrides + /// `` if set. /// - /// ``: Sets the maximum linear jerk. Overrides `` - /// if set. + /// - ``: Sets the maximum linear jerk. Overrides + /// `` if set. /// - /// ``: Sets the minimum angular jerk. Overrides + /// - ``: Sets the minimum angular jerk. Overrides /// `` if set. /// - /// ``: Sets the maximum angular jerk. Overrides + /// - ``: Sets the maximum angular jerk. Overrides /// `` if set. class DiffDrive : public System, diff --git a/src/systems/elevator/Elevator.hh b/src/systems/elevator/Elevator.hh index 0fa6f0ea52..dc188795ee 100644 --- a/src/systems/elevator/Elevator.hh +++ b/src/systems/elevator/Elevator.hh @@ -69,37 +69,37 @@ class ElevatorPrivate; /// /// ## System Parameters /// -/// ``: System update rate. This element is optional and the +/// - ``: System update rate. This element is optional and the /// default value is 10Hz. A value of zero gets translated to the simulation /// rate (no throttling for the system). /// -/// ``: Prefix in the names of the links that function as +/// - ``: Prefix in the names of the links that function as /// a reference for each floor level. When the elevator is requested to move /// to a given floor level, the cabin is commanded to move to the height of /// the corresponding floor link. The names of the links will be expected to /// be `{prerix}i`, where \f$i=[0,N)\f$ and N is the number of floors. This /// element is optional and the default value is `floor_`. /// -/// ``: Prefix in the names of the joints that control the +/// - ``: Prefix in the names of the joints that control the /// doors of the elevator. The names of the joints will be expected to be /// `{prerix}i`, where \f$i=[0,N)\f$ and N is the number of floors. This /// element is optional and the default value is `door_`. /// -/// ``: Name of the joint that controls the position of the +/// - ``: Name of the joint that controls the position of the /// cabin. This element is optional and the default value is `lift`. /// -/// ``: Topic to which this system will subscribe in order to +/// - ``: Topic to which this system will subscribe in order to /// receive command messages. This element is optional and the default value /// is `/model/{model_name}/cmd`. /// -/// ``: Topic on which this system will publish state (current +/// - ``: Topic on which this system will publish state (current /// floor) messages. This element is optional and the default value is /// `/model/{model_name}/state`. /// -/// ``: State publication rate. This rate is bounded by +/// - ``: State publication rate. This rate is bounded by /// ``. This element is optional and the default value is 5Hz. /// -/// ``: Time to wait with a door open before the door +/// - ``: Time to wait with a door open before the door /// closes. This element is optional and the default value is 5 sec. class GZ_SIM_VISIBLE Elevator : public System, public ISystemConfigure, diff --git a/src/systems/environment_preload/EnvironmentPreload.hh b/src/systems/environment_preload/EnvironmentPreload.hh index 16a9793cc0..9b462281c1 100644 --- a/src/systems/environment_preload/EnvironmentPreload.hh +++ b/src/systems/environment_preload/EnvironmentPreload.hh @@ -32,8 +32,9 @@ namespace systems { class EnvironmentPreloadPrivate; - /// \class EnvironmentPreload EnvironmentPreload.hh - /// gz/sim/systems/EnvironmentPreload.hh + /** \class EnvironmentPreload EnvironmentPreload.hh \ + * gz/sim/systems/EnvironmentPreload.hh + **/ /// \brief A plugin to preload an Environment component /// into the ECM upon simulation start-up. class EnvironmentPreload : diff --git a/src/systems/follow_actor/FollowActor.hh b/src/systems/follow_actor/FollowActor.hh index 7a01a920cc..27076786c5 100644 --- a/src/systems/follow_actor/FollowActor.hh +++ b/src/systems/follow_actor/FollowActor.hh @@ -35,24 +35,23 @@ namespace systems /// \class FollowActor FollowActor.hh gz/sim/systems/FollowActor.hh /// \brief Make an actor follow a target entity in the world. /// - /// ## SDF parameters + /// ## System Parameters /// - /// : Name of entity to follow. + /// - ``: Name of entity to follow. /// - /// : Distance in meters to keep from target's origin. + /// - ``: Distance in meters to keep from target's origin. /// - /// : Distance in meters from target's origin when to stop - /// following. When the actor is back within range it starts - /// following again. + /// - ``: Distance in meters from target's origin when to stop + /// following. When the actor is back within range it starts following + /// again. /// - /// : Actor's velocity in m/s + /// - ``: Actor's velocity in m/s /// - /// : Actor's animation to play. If empty, the first animation in - /// the model will be used. + /// - ``: Actor's animation to play. If empty, the first animation + /// in the model will be used. /// - /// : Velocity of the animation on the X axis. Used to - /// coordinate translational motion with the actor's - /// animation. + /// - ``: Velocity of the animation on the X axis. Used to + /// coordinate translational motion with the actor's animation. class FollowActor: public System, public ISystemConfigure, diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index aab1a7cdb0..1ada82b467 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -40,7 +40,7 @@ namespace systems /// forces like linear and quadratic drag, buoyancy (not provided by this /// plugin), etc. /// - /// # System Parameters + /// ## System Parameters /// The exact description of these parameters can be found on p. 37 and /// p. 43 of Fossen's book. They are used to calculate added mass, linear and /// quadratic drag and coriolis force. diff --git a/src/systems/joint_controller/JointController.hh b/src/systems/joint_controller/JointController.hh index ae65473423..ec1b3bd381 100644 --- a/src/systems/joint_controller/JointController.hh +++ b/src/systems/joint_controller/JointController.hh @@ -37,56 +37,60 @@ namespace systems /// /// ## System Parameters /// - /// `` The name of the joint to control. Required parameter. + /// - `` The name of the joint to control. Required parameter. /// Can also include multiple `` for identical joints. /// - /// `` True to enable the controller implementation + /// - `` True to enable the controller implementation /// using force commands. If this parameter is not set or is false, the /// controller will use velocity commands internally. /// - /// `` True to enable the use of actuator message + /// - `` True to enable the use of actuator message /// for velocity command. The actuator msg is an array of floats for /// position, velocity and normalized commands. Relies on /// `` for the index of the velocity actuator and /// defaults to topic "/actuators" when `topic` or `subtopic not set. /// - /// `` used with `` to set + /// - `` used with `` to set /// the index of the velocity actuator. /// - /// `` Topic to receive commands in. Defaults to + /// - `` Topic to receive commands in. Defaults to /// `/model//joint//cmd_vel`. /// - /// `` Sub topic to receive commands in. - /// Defaults to "/model//". + /// - `` Sub topic to receive commands in. + /// Defaults to "/model//". /// - /// `` Velocity to start with. + /// - `` Velocity to start with. /// - /// ### Velocity mode: No additional parameters are required. + /// ### Velocity mode /// - /// ### Force mode: The controller accepts the next optional parameters: + /// No additional parameters are required. /// - /// `` The proportional gain of the PID. - /// The default value is 1. + /// ### Force mode /// - /// `` The integral gain of the PID. - /// The default value is 0. + /// The controller accepts the next optional parameters: /// - /// `` The derivative gain of the PID. - /// The default value is 0. + /// - `` The proportional gain of the PID. + /// The default value is 1. /// - /// `` The integral upper limit of the PID. - /// The default value is 1. + /// - `` The integral gain of the PID. + /// The default value is 0. + /// + /// - `` The derivative gain of the PID. + /// The default value is 0. + /// + /// - `` The integral upper limit of the PID. + /// The default value is 1. /// - /// `` The integral lower limit of the PID. - /// The default value is -1. + /// - `` The integral lower limit of the PID. + /// The default value is -1. /// - /// `` Output max value of the PID. - /// The default value is 1000. + /// - `` Output max value of the PID. + /// The default value is 1000. /// - /// `` Output min value of the PID. - /// The default value is -1000. + /// - `` Output min value of the PID. + /// The default value is -1000. /// - /// `` Command offset (feed-forward) of the PID. + /// - `` Command offset (feed-forward) of the PID. /// The default value is 0. class JointController : public System, diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index af139641d7..f7290fea29 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -46,56 +46,56 @@ namespace systems /// /// ## System Parameters /// - /// `` The name of the joint to control. Required parameter. - /// Can also include multiple `` for identical joints. + /// - `` The name of the joint to control. Required parameter. + /// Can also include multiple `` for identical joints. /// - /// `` Axis of the joint to control. Optional parameter. - /// The default value is 0. + /// - `` Axis of the joint to control. Optional parameter. + /// The default value is 0. /// - /// `` True to enable the use of actutor message + /// - `` True to enable the use of actutor message /// for position command. Relies on `` for the /// index of the position actuator and defaults to topic "/actuators". /// - /// `` used with `` to set + /// - `` used with `` to set /// the index of the position actuator. /// - /// `` The proportional gain of the PID. Optional parameter. - /// The default value is 1. + /// - `` The proportional gain of the PID. Optional parameter. + /// The default value is 1. /// - /// `` The integral gain of the PID. Optional parameter. - /// The default value is 0.1. + /// - `` The integral gain of the PID. Optional parameter. + /// The default value is 0.1. /// - /// `` The derivative gain of the PID. Optional parameter. - /// The default value is 0.01 + /// - `` The derivative gain of the PID. Optional parameter. + /// The default value is 0.01 /// - /// `` The integral upper limit of the PID. Optional parameter. - /// The default value is 1. + /// - `` The integral upper limit of the PID. Optional parameter. + /// The default value is 1. /// - /// `` The integral lower limit of the PID. Optional parameter. - /// The default value is -1. + /// - `` The integral lower limit of the PID. Optional parameter. + /// The default value is -1. /// - /// `` Output max value of the PID. Optional parameter. - /// The default value is 1000. + /// - `` Output max value of the PID. Optional parameter. + /// The default value is 1000. /// - /// `` Output min value of the PID. Optional parameter. - /// The default value is -1000. + /// - `` Output min value of the PID. Optional parameter. + /// The default value is -1000. /// - /// `` Command offset (feed-forward) of the PID. Optional + /// - `` Command offset (feed-forward) of the PID. Optional /// parameter. The default value is 0. /// - /// `` Bypasses the PID and creates a perfect + /// - `` Bypasses the PID and creates a perfect /// position. The maximum speed on the joint can be set using the `` /// tag. /// - /// `` If you wish to listen on a non-default topic you may specify it - /// here, otherwise the controller defaults to listening on + /// - `` If you wish to listen on a non-default topic you may specify + /// it here, otherwise the controller defaults to listening on /// "/model//joint///cmd_pos". /// - /// `` If you wish to listen on a sub_topic you may specify it + /// - `` If you wish to listen on a sub_topic you may specify it /// here "/model//". /// - /// `` Initial position of a joint. Optional parameter. - /// The default value is 0. + /// - `` Initial position of a joint. Optional parameter. + /// The default value is 0. class JointPositionController : public System, public ISystemConfigure, diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index 28d2408091..b92da24414 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -41,12 +41,13 @@ namespace systems /// a model. Use the `` system parameter, described below, to /// control which joints are published. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of the topic to publish to. This parameter is optional, + /// - ``: Name of the topic to publish to. This parameter is optional, /// and if not provided, the joint state will be published to /// "/world//model//joint_state". - /// ``: Name of a joint to publish. This parameter can be + /// + /// - ``: Name of a joint to publish. This parameter can be /// specified multiple times, and is optional. All joints in a model will /// be published if joint names are not specified. class JointStatePublisher diff --git a/src/systems/joint_traj_control/JointTrajectoryController.hh b/src/systems/joint_traj_control/JointTrajectoryController.hh index 09dc7d9e48..5c654e04e0 100644 --- a/src/systems/joint_traj_control/JointTrajectoryController.hh +++ b/src/systems/joint_traj_control/JointTrajectoryController.hh @@ -65,68 +65,68 @@ namespace systems /// /// ## System Parameters /// - /// `` The name of the topic that this plugin subscribes to. + /// - `` The name of the topic that this plugin subscribes to. /// Optional parameter. /// Defaults to "/model/${MODEL_NAME}/joint_trajectory". /// - /// `` If enabled, trajectory execution begins at the + /// - `` If enabled, trajectory execution begins at the /// timestamp contained in the header of received trajectory messages. /// Optional parameter. /// Defaults to false. /// - /// `` Name of a joint to control. + /// - `` Name of a joint to control. /// This parameter can be specified multiple times, i.e. once for each joint. /// Optional parameter. /// Defaults to all 1-axis joints contained in the model SDF (order is kept). /// - /// `` Initial position of a joint (for position control). + /// - `` Initial position of a joint (for position control). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// Defaults to 0 for all joints. /// - /// `<%s_p_gain>` The proportional gain of the PID. + /// - `<%s_p_gain>` The proportional gain of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (disabled). /// - /// `<%s_i_gain>` The integral gain of the PID. Optional parameter. + /// - `<%s_i_gain>` The integral gain of the PID. Optional parameter. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (disabled). /// - /// `<%s_d_gain>` The derivative gain of the PID. + /// - `<%s_d_gain>` The derivative gain of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (disabled). /// - /// `<%s_i_min>` The integral lower limit of the PID. + /// - `<%s_i_min>` The integral lower limit of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (no limit if higher than `%s_i_max`). /// - /// `<%s_i_max>` The integral upper limit of the PID. + /// - `<%s_i_max>` The integral upper limit of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is -1 (no limit if lower than `%s_i_min`). /// - /// `<%s_cmd_min>` Output min value of the PID. + /// - `<%s_cmd_min>` Output min value of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (no limit if higher than `%s_i_max`). /// - /// `<%s_cmd_max>` Output max value of the PID. + /// - `<%s_cmd_max>` Output max value of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is -1 (no limit if lower than `%s_i_min`). /// - /// `<%s_cmd_offset>` Command offset (feed-forward) of the PID. + /// - `<%s_cmd_offset>` Command offset (feed-forward) of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh index c8d0020535..aae88a94eb 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -39,20 +39,20 @@ namespace systems /// that surpasses a specific threshold. /// This system can be used to detect when a model could be damaged. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of the link to monitor. This name must match + /// - ``: Name of the link to monitor. This name must match /// a name of link within the model. /// - /// ``: Threshold, in Joule (J), after which + /// - ``: Threshold, in Joule (J), after which /// a message is generated on `` with the kinetic energy value that /// surpassed the threshold. /// - /// ``: Custom topic that this system will publish to when kinetic + /// - ``: Custom topic that this system will publish to when kinetic /// energy surpasses the threshold. This element if optional, and the /// default value is `/model/{name_of_model}/kinetic_energy`. /// - /// # Example Usage + /// ## Example Usage /// /** \verbatim diff --git a/src/systems/lens_flare/LensFlare.hh b/src/systems/lens_flare/LensFlare.hh index 95ed7a07d4..9ea069d009 100644 --- a/src/systems/lens_flare/LensFlare.hh +++ b/src/systems/lens_flare/LensFlare.hh @@ -32,23 +32,26 @@ namespace systems /// \brief Private data class for LensFlare Plugin class LensFlarePrivate; - /** \class LensFlare LensFlare.hh \ + /** \class LensFlare LensFlare.hh \ * gz/sim/systems/LensFlare.hh **/ /// \brief Add lens flare effects to the camera output as a render pass - /// The system takes in the following parameter: - /// Sets the scale of the lens flare. If this is - /// not specified, the value defaults to 1.0 /// - /// Sets the color of the lens flare. The default - /// is {1.4, 1.2, 1.0} + /// ## System Parameters /// - /// Sets the number of steps to take in - /// each direction to check for occlusions. - /// The default value is set to 10. Use 0 to disable - /// Sets the light associated with the lens flares. - /// If not specified. The first light in the scene will - /// be used. + /// - ``: Sets the scale of the lens flare. If this is + /// not specified, the value defaults to 1.0 + /// + /// - ``: Sets the color of the lens flare. The default + /// is {1.4, 1.2, 1.0} + /// + /// - ``: Sets the number of steps to take in + /// each direction to check for occlusions. + /// The default value is set to 10. Use 0 to disable + /// + /// - ``: Sets the light associated with the lens flares. + /// If not specified. The first light in the scene will + /// be used. class LensFlare: public System, diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index 8c7cba996d..f9f33b960f 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -35,34 +35,35 @@ 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) class LiftDrag : public System, public ISystemConfigure, diff --git a/src/systems/log/LogPlayback.hh b/src/systems/log/LogPlayback.hh index bc8fd28110..66712a9c89 100644 --- a/src/systems/log/LogPlayback.hh +++ b/src/systems/log/LogPlayback.hh @@ -33,8 +33,9 @@ namespace systems // Forward declarations. class LogPlaybackPrivate; - /// \class LogPlayback LogPlayback.hh - /// gz/sim/systems/log/LogPlayback.hh + /** \class LogPlayback LogPlayback.hh \ + * gz/sim/systems/log/LogPlayback.hh + **/ /// \brief Log state playback class LogPlayback: public System, diff --git a/src/systems/log_video_recorder/LogVideoRecorder.hh b/src/systems/log_video_recorder/LogVideoRecorder.hh index 958f0152ce..0de7eb46ac 100644 --- a/src/systems/log_video_recorder/LogVideoRecorder.hh +++ b/src/systems/log_video_recorder/LogVideoRecorder.hh @@ -38,18 +38,21 @@ namespace systems /// \brief System which recordings videos from log playback /// There are two ways to specify what entities in the log playback to follow /// and record videos for: 1) by entity name and 2) by region. See the - /// following parameters: - /// - `` Name of entity to record. - /// - `` Axis-aligned box where entities are at start of log - /// + `` Min corner position of box region. - /// + `` Max corner position of box region. - /// - `` Sim time when recording should start - /// - `` Sim time when recording should end - /// - `` Exit gz-sim when log playback recording ends + /// system parameters. /// /// When recording is finished. An `end` string will be published to the /// `/log_video_recorder/status` topic and the videos are saved to a /// timestamped directory + /// + /// ## System Parameters + /// + /// - `` Name of entity to record. + /// - `` Axis-aligned box where entities are at start of log + /// + `` Min corner position of box region. + /// + `` Max corner position of box region. + /// - `` Sim time when recording should start + /// - `` Sim time when recording should end + /// - `` Exit gz-sim when log playback recording ends class LogVideoRecorder final: public System, public ISystemConfigure, diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh index 04c93edd45..0a4873d474 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh @@ -42,6 +42,8 @@ namespace systems /// such as speakers. This plugin is meant to check if audio /// could theoretically be heard at a certain location or not. /// + /// ## System Parameters + /// /// Secifying an audio source via SDF is done as follows: /// /// - `` A new audio source in the environment, which has the diff --git a/src/systems/mecanum_drive/MecanumDrive.hh b/src/systems/mecanum_drive/MecanumDrive.hh index 4cc910eee1..1442967687 100644 --- a/src/systems/mecanum_drive/MecanumDrive.hh +++ b/src/systems/mecanum_drive/MecanumDrive.hh @@ -35,58 +35,59 @@ namespace systems /// \brief Mecanum drive controller which can be attached to a model /// with any number of front/back left/right wheels. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of a joint that controls a front left wheel. + /// - ``: Name of a joint that controls a front left wheel. /// This element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a front right wheel. - /// This element can appear multiple times, and must appear at least once. + /// - ``: Name of a joint that controls a front right + /// wheel. This element can appear multiple times, and must appear at least + /// once. /// - /// ``: Name of a joint that controls a back left wheel. + /// - ``: Name of a joint that controls a back left wheel. /// This element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a back right wheel. + /// - ``: Name of a joint that controls a back right wheel. /// This element can appear multiple times, and must appear at least once. /// - /// ``: Longitudinal distance between front and back wheels, + /// - ``: Longitudinal distance between front and back wheels, /// in meters. This element is optional, although it is recommended to be /// included with an appropriate value. The default value is 1.0m. /// - /// ``: Lateral distance between left and right wheels, + /// - ``: Lateral distance between left and right wheels, /// in meters. This element is optional, although it is recommended to be /// included with an appropriate value. The default value is 1.0m. /// - /// ``: Wheel radius in meters. This element is optional, + /// - ``: Wheel radius in meters. This element is optional, /// although it is recommended to be included with an appropriate value. The /// default value is 0.2m. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Custom topic that this system will subscribe to in order to + /// - ``: Custom topic that this system will subscribe to in order to /// receive command velocity messages. This element if optional, and the /// default value is `/model/{name_of_model}/cmd_vel`. /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `frame_id` to `child_frame_id`. This element if optional, /// and the default value is `/model/{name_of_model}/tf`. /// - /// ``: Custom `frame_id` field that this system will use as the + /// - ``: Custom `frame_id` field that this system will use as the /// origin of the odometry transform in both the `` /// `gz.msgs.Pose_V` message and the `` /// `gz.msgs.Odometry` message. This element if optional, and the /// default value is `{name_of_model}/odom`. /// - /// ``: Custom `child_frame_id` that this system will use as + /// - ``: Custom `child_frame_id` that this system will use as /// the target of the odometry trasnform in both the `` /// `gz.msgs.Pose_V` message and the `` /// `gz.msgs.Odometry` message. This element if optional, - /// and the default value is `{name_of_model}/{name_of_link}`. + /// and the default value is `{name_of_model}/{name_of_link}`. class MecanumDrive : public System, public ISystemConfigure, diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.hh b/src/systems/multicopter_control/MulticopterVelocityControl.hh index c414d1ec53..eb2cd1caf8 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.hh +++ b/src/systems/multicopter_control/MulticopterVelocityControl.hh @@ -69,84 +69,85 @@ namespace systems /// * Maximum acceleration limit /// * Can be enabled/disabled at runtime. /// - /// # Parameters - /// The following parameters are used by the system + /// ## System Parameters /// - /// robotNamespace: All gz-transport topics subscribed to and published by - /// the system will be prefixed by this string. This is a required parameter. + /// - `robotNamespace`: All gz-transport topics subscribed to and published by + /// the system will be prefixed by this string. This is a required parameter. /// - /// commandSubTopic: The system subscribes to this topic to receive twist + /// - `commandSubTopic`: The system subscribes to this topic to receive twist /// commands. The default value is "cmd_vel". /// - /// enableSubTopic: Topic to enable or disable the system. If false, the + /// - `enableSubTopic`: Topic to enable or disable the system. If false, the /// controller sends a zero rotor velocity command once and gets disabled. If /// the vehicle is in the air, disabling the controller will cause it to fall. /// If true, the controller becomes enabled and waits for a twist message. The /// default value is "enable". /// - /// comLinkName: The link associated with the center of mass of the vehicle. - /// That is, the origin of the center of mass may not be on this link, but - /// this link and the center of mass frame have a fixed transform. Almost - /// always this should be the base_link of the vehicle. This is a required - /// parameter. + /// - `comLinkName`: The link associated with the center of mass of the + /// vehicle. That is, the origin of the center of mass may not be on this + /// link, but this link and the center of mass frame have a fixed transform. + /// Almost always this should be the base_link of the vehicle. This is a + /// required parameter. /// - /// velocityGain (x, y, z): Proportional gain on linear velocity. + /// - `velocityGain` (x, y, z): Proportional gain on linear velocity. /// attitudeGain (roll, pitch, yaw): Proportional gain on attitude. This /// parameter is scaled by the inverse of the inertia matrix so two vehicles /// with different inertial characteristics may have the same gain if other /// parameters, such as the forceConstant, are kept the same. This is a /// required parameter. /// - /// angularRateGain (roll, pitch, yaw): Proportional gain on angular velocity. - /// Even though only the yaw angle velocity is controlled, proper gain values - /// for roll and pitch velocities must be specified. This parameter is scaled - /// by the inverse of the inertia matrix so two vehicles with different - /// inertial characteristics may have the same gain if other parameters, such - /// as the forceConstant, are kept the same. This is a required parameter. + /// - `angularRateGain` (roll, pitch, yaw): Proportional gain on angular + /// velocity. Even though only the yaw angle velocity is controlled, proper + /// gain values for roll and pitch velocities must be specified. This + /// parameter is scaled by the inverse of the inertia matrix so two vehicles + /// with different inertial characteristics may have the same gain if other + /// parameters, such as the forceConstant, are kept the same. This is a + /// required parameter. /// - /// maxLinearAcceleration (x, y, z): Maximum limit on linear acceleration. + /// - `maxLinearAcceleration` (x, y, z): Maximum limit on linear acceleration. /// The default value is DBL_MAX. /// - /// maximumLinearVelocity (x, y, z): Maximum commanded linear velocity. The - /// default value is DBL_MAX. - - /// maximumAngularVelocity (roll, pitch, yaw): Maximum commanded angular + /// - `maximumLinearVelocity` (x, y, z): Maximum commanded linear velocity. + /// The default value is DBL_MAX. + /// + /// - `maximumAngularVelocity` (roll, pitch, yaw): Maximum commanded angular /// velocity. The default value is DBL_MAX. /// - /// linearVelocityNoiseMean (x, y, z): Mean of Gaussian noise on linear + /// - `linearVelocityNoiseMean` (x, y, z): Mean of Gaussian noise on linear /// velocity values obtained from simulation. The default value is (0, 0, 0). /// - /// linearVelocityNoiseStdDev (x, y, z): Standard deviation of Gaussian noise - /// on linear values obtained from simulation. A value of 0 implies noise is - /// NOT applied to the component. The default value is (0, 0, 0). + /// - `linearVelocityNoiseStdDev` (x, y, z): Standard deviation of Gaussian + /// noise on linear values obtained from simulation. A value of 0 implies + /// noise is NOT applied to the component. The default value is (0, 0, 0). /// - /// angularVelocityNoiseMean (roll, pitch, yaw): Mean of Gaussian noise on + /// - `angularVelocityNoiseMean` (roll, pitch, yaw): Mean of Gaussian noise on /// angular velocity values obtained from simulation. The default value is (0, /// 0, 0). /// - /// angularVelocityNoiseStdDev (roll, pitch, yaw): Standard deviation of + /// - `angularVelocityNoiseStdDev` (roll, pitch, yaw): Standard deviation of /// gaussian noise on angular velocity values obtained from simulation. A /// value of 0 implies noise is NOT applied to the component. The default /// value is (0, 0, 0). /// - /// rotorConfiguration: This contains a list of `` elements for each - /// rotor in the vehicle. This is a required parameter. + /// - `rotorConfiguration`: This contains a list of `` elements for + /// each rotor in the vehicle. This is a required parameter. /// - /// rotor: Contains information about a rotor in the vehicle. All the + /// - `rotor`: Contains information about a rotor in the vehicle. All the /// elements of `` are required parameters. /// - /// jointName: The name of the joint associated with this rotor. + /// - `jointName`: The name of the joint associated with this rotor. /// - /// forceConstant: A constant that multiplies with the square of the + /// - `forceConstant`: A constant that multiplies with the square of the /// rotor's velocity to compute its thrust. /// - /// momentConstant: A constant the multiplies with the rotor's thrust to - /// compute its moment. + /// - `momentConstant`: A constant the multiplies with the rotor's + /// thrust to compute its moment. + /// + /// - `direction`: Direction of rotation of the rotor. +1 is + /// counterclockwise and -1 is clockwise. /// - /// direction: Direction of rotation of the rotor. +1 is counterclockwise - /// and -1 is clockwise. + /// ## Examples /// - /// # Examples /// See examples/worlds/quadcopter.sdf for a demonstration. /// class MulticopterVelocityControl diff --git a/src/systems/pose_publisher/PosePublisher.hh b/src/systems/pose_publisher/PosePublisher.hh index db1a7e1d33..022ba2d4e6 100644 --- a/src/systems/pose_publisher/PosePublisher.hh +++ b/src/systems/pose_publisher/PosePublisher.hh @@ -37,33 +37,27 @@ namespace systems /// messages, or a single gz::msgs::Pose_V message if /// "use_pose_vector_msg" is true. /// - /// The following parameters are used by the system: + /// ## System Parameters /// - /// publish_link_pose : Set to true to publish link pose - /// publish_visual_pose : Set to true to publish visual pose - /// publish_collision_pose : Set to true to publish collision pose - /// publish_sensor_pose : Set to true to publish sensor pose - /// publish_model_pose : Set to true to publish model pose. - /// publish_nested_model_pose : Set to true to publish nested model pose. The - /// pose of the model that contains this system is - /// also published unless publish_model_pose is - /// set to false - /// use_pose_vector_msg : Set to true to publish an - /// gz::msgs::Pose_V message instead of - /// mulitple gz::msgs::Pose messages. - /// update_frequency : Frequency of pose publications in Hz. A - /// negative frequency publishes as fast as - /// possible (i.e, at the rate of the simulation - /// step) - /// static_publisher : Set to true to publish static poses on - /// a "/pose_static" topic. - /// This will cause only dynamic poses to be - /// published on the "/pose" - /// topic. - /// static_update_frequency : Frequency of static pose publications in Hz. A - /// negative frequency publishes as fast as - /// possible (i.e, at the rate of the simulation - /// step). + /// - ``: Set to true to publish link pose + /// - ``: Set to true to publish visual pose + /// - ``: Set to true to publish collision pose + /// - ``: Set to true to publish sensor pose + /// - ``: Set to true to publish model pose. + /// - ``: Set to true to publish nested model + /// pose. The pose of the model that contains this system is also published + /// unless publish_model_pose is set to false + /// - ``: Set to true to publish a gz::msgs::Pose_V + /// message instead of mulitple gz::msgs::Pose messages. + /// - ``: Frequency of pose publications in Hz. A negative + /// frequency publishes as fast as possible (i.e, at the rate of the + /// simulation step) + /// - ``: Set to true to publish static poses on a + /// "/pose_static" topic. This will cause only dynamic + /// poses to be published on the "/pose" topic. + /// - ``: Frequency of static pose publications in + /// Hz. A negative frequency publishes as fast as possible (i.e, at the + /// rate of the simulation step). class PosePublisher : public System, public ISystemConfigure, diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh index f035c684a4..0aa396bd00 100644 --- a/src/systems/user_commands/UserCommands.hh +++ b/src/systems/user_commands/UserCommands.hh @@ -38,38 +38,38 @@ namespace systems /// \todo(louise) In the future, an interface undo/redo commands will also /// be provided. /// - /// # Spawn entity + /// ### Spawn entity /// /// * **Service**: `/world//create` - /// * **Request type*: gz.msgs.EntityFactory - /// * **Response type*: gz.msgs.Boolean + /// * **Request type**: gz.msgs.EntityFactory + /// * **Response type**: gz.msgs.Boolean /// - /// # Spawn multiple entities + /// ### Spawn multiple entities /// /// This service can spawn multiple entities in the same iteration, /// thereby eliminating simulation steps between entity spawn times. /// /// * **Service**: `/world//create_multiple` - /// * **Request type*: gz.msgs.EntityFactory_V - /// * **Response type*: gz.msgs.Boolean + /// * **Request type**: gz.msgs.EntityFactory_V + /// * **Response type**: gz.msgs.Boolean /// - /// # Set entity pose + /// ### Set entity pose /// /// This service set the pose of entities /// /// * **Service**: `/world//set_pose` - /// * **Request type*: gz.msgs.Pose - /// * **Response type*: gz.msgs.Boolean + /// * **Request type**: gz.msgs.Pose + /// * **Response type**: gz.msgs.Boolean /// - /// # Set multiple entity poses + /// ### Set multiple entity poses /// /// This service set the pose of multiple entities /// /// * **Service**: `/world//set_pose_vector` - /// * **Request type*: gz.msgs.Pose_V - /// * **Response type*: gz.msgs.Boolean + /// * **Request type**: gz.msgs.Pose_V + /// * **Response type**: gz.msgs.Boolean /// - /// Try some examples described on examples/worlds/empty.sdf + /// Try some examples described in examples/worlds/empty.sdf class UserCommands final: public System, public ISystemConfigure, 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 15/21] 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 +) From 45233fc39a82f76a11cae9d4c8ea01b205f9c20c Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 3 Nov 2023 13:58:10 -0700 Subject: [PATCH 16/21] Add note about elevator example (#2227) Closes #2153 Signed-off-by: Michael Carroll --- examples/worlds/elevator.sdf | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/examples/worlds/elevator.sdf b/examples/worlds/elevator.sdf index f50ee17c59..0cbcd8702e 100644 --- a/examples/worlds/elevator.sdf +++ b/examples/worlds/elevator.sdf @@ -10,6 +10,12 @@ gz topic -e -t /model/elevator/state + Note that when commanding the lift to the ground floor: + + gz topic -t "/model/elevator/cmd" -m gz.msgs.Int32 -p "data: 0" + + The output of the topic echo command will stop as protobuf does not + distinguish between the un-set value and zero for integer fields. --> From 82fbdbabcc96d867bba0829e2b2d68fb158a9aa3 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 6 Nov 2023 16:40:46 -0600 Subject: [PATCH 17/21] Use `GZ_PI` instead of `M_PI` to fix windows builds (#2230) Fixes #2229 Signed-off-by: Addisu Z. Taddese --- examples/standalone/marker/marker.cc | 2 +- src/systems/advanced_lift_drag/AdvancedLiftDrag.cc | 4 ++-- test/integration/added_mass.cc | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc index 5cf7e725ba..68334cffc3 100644 --- a/examples/standalone/marker/marker.cc +++ b/examples/standalone/marker/marker.cc @@ -172,7 +172,7 @@ int main(int _argc, char **_argv) gz::msgs::Set(markerMsg.add_point(), gz::math::Vector3d(0, 0, 0.05)); double radius = 2; - for (double t = 0; t <= M_PI; t+= 0.01) + for (double t = 0; t <= GZ_PI; t+= 0.01) { gz::msgs::Set(markerMsg.add_point(), gz::math::Vector3d(radius * cos(t), radius * sin(t), 0.05)); diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc index 42d6efc189..a4da3147e4 100644 --- a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc +++ b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc @@ -615,7 +615,7 @@ void AdvancedLiftDragPrivate::Update(EntityComponentManager &_ecm) { components::JointPosition *tmp_controlJointPosition = controlJointPosition_vec[i]; - controlAngle = tmp_controlJointPosition->Data()[0] * 180/M_PI; + controlAngle = tmp_controlJointPosition->Data()[0] * 180 / GZ_PI; } // AVL's and Gazebo's direction of "positive" deflection may be different. @@ -671,7 +671,7 @@ void AdvancedLiftDragPrivate::Update(EntityComponentManager &_ecm) 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 * + CD = (1 - sigma) * (this->CD0 + (CL*CL) / (GZ_PI * this->AR * this->eff)) + sigma * abs( CD_fp * (0.5 - 0.5 * cos(2 * this->alpha))); diff --git a/test/integration/added_mass.cc b/test/integration/added_mass.cc index a751b10bba..231cb8f516 100644 --- a/test/integration/added_mass.cc +++ b/test/integration/added_mass.cc @@ -50,13 +50,13 @@ const double kRate = 1000; const double kForceVec[3] = {2000, 2000, 0}; // Force excitation angular velocity [rad / s]. -const double kForceAngVel = 3 * M_PI; +const double kForceAngVel = 3 * GZ_PI; // Torque excitation amplitude and direction. const double kTorqueVec[3] = {200, 200, 0}; // Torque excitation angular velocity [rad / s]. -const double kTorqueAngVel = 2 * M_PI; +const double kTorqueAngVel = 2 * GZ_PI; // Total duration of the motion in iterations. const uint64_t kIter = 1000; From fdbda5839ca2fd1f5386faf1f74f669c3de91eb6 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Tue, 7 Nov 2023 19:50:48 -0500 Subject: [PATCH 18/21] Standardize Doxygen parameter formatting for systems O-Z (#2212) * standardize system parameter doxygen for plugins O-Z * 80 chars * Update TrackedVehicle.hh * fix lint * gcc warning about multi-line comment Signed-off-by: Mabel Zhang --- src/systems/buoyancy_engine/BuoyancyEngine.hh | 19 ++- .../odometry_publisher/OdometryPublisher.hh | 28 ++-- .../OpticalTactilePlugin.hh | 68 ++++---- .../particle_emitter/ParticleEmitter.hh | 4 +- .../performer_detector/PerformerDetector.hh | 18 ++- src/systems/physics/Physics.hh | 10 +- .../PythonSystemLoader.hh | 8 +- src/systems/rf_comms/RFComms.hh | 47 +++--- src/systems/sensors/Sensors.hh | 8 +- src/systems/shader_param/ShaderParam.hh | 66 ++++---- src/systems/thruster/Thruster.hh | 115 +++++++------ src/systems/touch_plugin/TouchPlugin.hh | 12 +- .../track_controller/TrackController.hh | 65 ++++---- src/systems/tracked_vehicle/TrackedVehicle.hh | 125 +++++++------- .../trajectory_follower/TrajectoryFollower.hh | 152 ++++++++++-------- .../triggered_publisher/TriggeredPublisher.hh | 17 +- .../velocity_control/VelocityControl.hh | 8 +- src/systems/wheel_slip/WheelSlip.hh | 25 ++- src/systems/wind_effects/WindEffects.hh | 36 +++-- 19 files changed, 449 insertions(+), 382 deletions(-) diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.hh b/src/systems/buoyancy_engine/BuoyancyEngine.hh index 422f5a703a..d7a92b2996 100644 --- a/src/systems/buoyancy_engine/BuoyancyEngine.hh +++ b/src/systems/buoyancy_engine/BuoyancyEngine.hh @@ -74,16 +74,19 @@ namespace systems /// gz sim buoyancy_engine.sdf /// ``` /// Enter the following in a separate terminal: - /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ - /// -p "data: 0.003" - /// ``` + /** ``` + gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ + -p "data: 0.003" + ``` + **/ /// to see the box float up. - /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ - /// -p "data: 0.001" - /// ``` + /** ``` + gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ + -p "data: 0.001" + ``` + **/ /// to see the box go down. + /// /// To see the current volume enter: /// ``` /// gz topic -t /model/buoyant_box/buoyancy_engine/current_volume -e diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index f7b462da7c..cab9bd6d14 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -36,44 +36,44 @@ namespace systems /// order to periodically publish 2D or 3D odometry data in the form of /// gz::msgs::Odometry messages. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of the world-fixed coordinate frame for the + /// - ``: Name of the world-fixed coordinate frame for the // odometry message. This element is optional, and the default value /// is `{name_of_model}/odom`. /// - /// ``: Name of the coordinate frame rigidly attached + /// - ``: Name of the coordinate frame rigidly attached /// to the mobile robot base. This element is optional, and the default /// value is `{name_of_model}/base_footprint`. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element is optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish - /// odometry with covariance messages. This element is optional, and the - /// default value is `/model/{name_of_model}/odometry_with_covariance`. + /// - ``: Custom topic on which this system will + /// publish odometry with covariance messages. This element is optional, and + /// the default value is `/model/{name_of_model}/odometry_with_covariance`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `odom_frame` to `robot_base_frame`. This element is /// optional, and the default value is `/model/{name_of_model}/pose`. /// - /// ``: Number of dimensions to represent odometry. Only 2 and 3 + /// - ``: Number of dimensions to represent odometry. Only 2 and 3 /// dimensional spaces are supported. This element is optional, and the /// default value is 2. /// - /// ``: Position offset relative to the body fixed frame, the + /// - ``: Position offset relative to the body fixed frame, the /// default value is 0 0 0. This offset will be added to the odometry /// message. /// - /// ``: Rotation offset relative to the body fixed frame, the + /// - ``: Rotation offset relative to the body fixed frame, the /// default value is 0 0 0. This offset will be added to the odometry - /// message. + /// message. /// - /// ``: Standard deviation of the Gaussian noise to be added + /// - ``: Standard deviation of the Gaussian noise to be added /// to pose and twist messages. This element is optional, and the default /// value is 0. class OdometryPublisher diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 664c109412..348cff0acf 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -37,54 +37,52 @@ namespace systems /// /// It requires that contact sensor and depth camera be placed in at least /// one link on the model on which this plugin is attached. - /// TODO: + /// + /// \todo(anyone) /// Currently, the contacts returned from the physics engine (which tends to /// be sparse) and the normal forces separately computed from the depth /// camera (which is dense, resolution adjustable) are disjoint. It is /// left as future work to combine the two sets of data. /// - /// Parameters: + /// ## System Parameters /// - /// Set this to true so the plugin works from the start and - /// doesn't need to be enabled. This element is optional, and the - /// default value is true. + /// - ``: Set this to true so the plugin works from the start and + /// doesn't need to be enabled. This element is optional, and the + /// default value is true. /// - /// Namespace for transport topics/services. If there are more - /// than one optical tactile plugins, their namespaces should - /// be different. - /// This element is optional, and the default value is - /// "optical_tactile_sensor". - /// //enable : Service used to enable and disable the - /// plugin. - /// //normal_forces : Topic where a message is - /// published each time the - /// normal forces are computed + /// - ``: Namespace for transport topics/services. If there are + /// more than one optical tactile plugins, their namespaces should + /// be different. This element is optional, and the default value is + /// "optical_tactile_sensor". + /// - `//enable`: Service used to enable and disable the plugin. + /// - `//normal_forces`: Topic where a message is + /// published each time the normal forces are computed /// - /// Number n of pixels to skip when visualizing - /// the forces. One vector representing a normal - /// force is computed for every nth pixel. This - /// element must be positive and it is optional. - /// The default value is 30. + /// - ``: Number n of pixels to skip when + /// visualizing the forces. One vector representing a normal + /// force is computed for every nth pixel. This + /// element must be positive and it is optional. + /// The default value is 30. /// - /// Length in meters of the forces visualized if - /// is set to true. This parameter is - /// optional, and the default value is 0.01. + /// - ``: Length in meters of the forces visualized if + /// is set to true. This parameter is + /// optional, and the default value is 0.01. /// - /// Extended sensing distance in meters. The sensor will - /// output data coming from its collision geometry plus - /// this distance. This element is optional, and the - /// default value is 0.001. + /// - ``: Extended sensing distance in meters. The sensor + /// will output data coming from its collision geometry plus + /// this distance. This element is optional, and the + /// default value is 0.001. /// - /// Whether to visualize the sensor or not. This element - /// is optional, and the default value is false. + /// - ``: Whether to visualize the sensor or not. This + /// element is optional, and the default value is false. /// - /// Whether to visualize the contacts from the contact - /// sensor based on physics. This element is optional, - /// and the default value is false. + /// - ``: Whether to visualize the contacts from the + /// contact sensor based on physics. This element is optional, + /// and the default value is false. /// - /// Whether to visualize normal forces computed from the - /// depth camera. This element is optional, and the - /// default value is false. + /// - ``: Whether to visualize normal forces computed from + /// the depth camera. This element is optional, and the + /// default value is false. class OpticalTactilePlugin : public System, diff --git a/src/systems/particle_emitter/ParticleEmitter.hh b/src/systems/particle_emitter/ParticleEmitter.hh index 848fa21a61..f6086bc2bb 100644 --- a/src/systems/particle_emitter/ParticleEmitter.hh +++ b/src/systems/particle_emitter/ParticleEmitter.hh @@ -32,10 +32,10 @@ namespace systems class ParticleEmitterPrivate; /// \brief A system for running and managing particle emitters. A particle - /// emitter is defined using the SDF element. + /// emitter is defined using the `` SDF element. /// /// This system will create a transport subscriber for each - /// using the child name. If a is not + /// `` using the child `` name. If a `` is not /// specified, the following topic naming scheme will be used: /// `/model/{model_name}/link/{link_name}/particle_emitter/{emitter_name}/cmd` class ParticleEmitter diff --git a/src/systems/performer_detector/PerformerDetector.hh b/src/systems/performer_detector/PerformerDetector.hh index bbdb9e3ddf..cd0c7f6f50 100644 --- a/src/systems/performer_detector/PerformerDetector.hh +++ b/src/systems/performer_detector/PerformerDetector.hh @@ -45,8 +45,9 @@ namespace systems /// PerformerDetector's region, which is also represented by an /// gz::math::AxisAlignedBox. When a performer is detected, the system /// publishes a gz.msgs.Pose message with the pose of the detected - /// performer with respect to the model containing the PerformerDetector. The - /// name and id fields of the Pose message will be set to the name and the + /// performer with respect to the model containing the PerformerDetector. + /// + /// The name and id fields of the Pose message will be set to the name and the /// entity of the detected performer respectively. The header of the Pose /// message contains the time stamp of detection. The `data` field of the /// header will contain the key "frame_id" with a value set to the name of @@ -62,19 +63,22 @@ namespace systems /// The system does not assume that levels are enabled, but it does require /// performers to be specified. /// - /// ## System parameters + /// ## System Parameters /// - /// ``: Custom topic to be used for publishing when a performer is + /// - ``: Custom topic to be used for publishing when a performer is /// detected. If not set, the default topic with the following pattern would /// be used "/model//performer_detector/status". The topic type /// is gz.msgs.Pose - /// ``: Detection region. Currently, only the `` geometry is + /// + /// - ``: Detection region. Currently, only the `` geometry is /// supported. The position of the geometry is derived from the pose of the /// containing model. - /// ``: Additional pose offset relative to the parent model's pose. + /// + /// - ``: Additional pose offset relative to the parent model's pose. /// This pose is added to the parent model pose when computing the /// detection region. Only the position component of the `` is used. - /// ``: Zero or more key-value pairs that will be + /// + /// - ``: Zero or more key-value pairs that will be /// included in the header of the detection messages. A `` /// element should have child `` and `` elements whose /// contents are interpreted as strings. Keys value pairs are stored in a diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index d7944668e2..cec62d341c 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -64,9 +64,15 @@ namespace systems /// \class Physics Physics.hh gz/sim/systems/Physics.hh /// \brief Base class for a System. - /// Includes optional parameter : . When set + /// + /// ## System Parameters + /// + /// - ``: Optional. When set /// to false, the name of colliding entities is not populated in - /// the contacts. Remains true by default. Usage : + /// the contacts. Remains true by default. + /// + /// ## Example + /// /// ``` /// : Name of python module to be loaded. The search path +/// ## System Parameters +/// * `` : Name of python module to be loaded. The search path /// includes `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as /// `PYTHONPATH`. /// -/// The contents of the tag will be available in the configure method +/// The contents of the `` tag will be available in the configure method /// of the Python system // TODO(azeey) Add ParameterConfigure class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader final diff --git a/src/systems/rf_comms/RFComms.hh b/src/systems/rf_comms/RFComms.hh index 550ca70b12..16ccf0c0cd 100644 --- a/src/systems/rf_comms/RFComms.hh +++ b/src/systems/rf_comms/RFComms.hh @@ -38,31 +38,37 @@ namespace systems /// This communication model has been ported from: /// https://github.com/osrf/subt . /// + /// ## System Parameters + /// /// This system can be configured with the following SDF parameters: /// - /// * Optional parameters: - /// Element used to capture the range configuration based on a - /// log-normal distribution. This block can contain any of the - /// next parameters: - /// * : Hard limit on range (meters). No communication will - /// happen beyond this range. Default is 50. - /// * : Fading exponent used in the normal distribution. - /// Default is 2.5. - /// * : Path loss at the reference distance (1 meter) in dBm. - /// Default is 40. - /// * : Standard deviation of the normal distribution. - /// Default is 10. + /// Optional parameters: + /// + /// - ``: Element used to capture the range configuration based + /// on a log-normal distribution. This block can contain any of the + /// next parameters: + /// - ``: Hard limit on range (meters). No communication will + /// happen beyond this range. Default is 50. + /// - ``: Fading exponent used in the normal distribution. + /// Default is 2.5. + /// - ``: Path loss at the reference distance (1 meter) in dBm. + /// Default is 40. + /// - ``: Standard deviation of the normal distribution. + /// Default is 10. + /// + /// - ``: Element used to capture the radio configuration. + /// This block can contain any of the + /// next parameters: + /// - ``: Capacity of radio in bits-per-second. + /// Default is 54000000 (54 Mbps). + /// - ``: Transmitter power in dBm. Default is 27dBm (500mW). + /// - ``: Noise floor in dBm. Default is -90dBm. + /// - ``: Supported modulations: ["QPSK"]. Default is "QPSK". /// - /// Element used to capture the radio configuration. - /// This block can contain any of the - /// next parameters: - /// * : Capacity of radio in bits-per-second. - /// Default is 54000000 (54 Mbps). - /// * : Transmitter power in dBm. Default is 27dBm (500mW). - /// * : Noise floor in dBm. Default is -90dBm. - /// * : Supported modulations: ["QPSK"]. Default is "QPSK". + /// ## Example /// /// Here's an example: + /// ``` /// @@ -79,6 +85,7 @@ namespace systems /// QPSK /// /// + /// ``` class RFComms : public comms::ICommsModel { diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index caf533ce29..41a314e757 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -40,14 +40,14 @@ namespace systems /// /// ## System Parameters /// - /// - `` Name of the render engine, such as 'ogre' or 'ogre2'. - /// - `` Color used for the scene's background. This + /// - ``: Name of the render engine, such as "ogre" or "ogre2". + /// - ``: Color used for the scene's background. This /// will override the background color specified in a world's SDF /// element. This background color is used by sensors, not the GUI. - /// - `` Color used for the scene's ambient light. This + /// - ``: Color used for the scene's ambient light. This /// will override the ambient value specified in a world's SDF /// element. This ambient light is used by sensors, not the GUI. - /// - `` Disable sensors if the model's + /// - ``: Disable sensors if the model's /// battery plugin charge reaches zero. Sensors that are in nested /// models are also affected. /// diff --git a/src/systems/shader_param/ShaderParam.hh b/src/systems/shader_param/ShaderParam.hh index 6c6d4a4215..8197d08015 100644 --- a/src/systems/shader_param/ShaderParam.hh +++ b/src/systems/shader_param/ShaderParam.hh @@ -35,42 +35,42 @@ namespace systems /// \brief A plugin for setting shaders to a visual and its params /// - /// Plugin parameters: + /// ## System Parameters /// - /// Shader to use - can be repeated to specify shader programs - /// written in different languages. - /// Attributes: - /// language - Shader language. Possible values: glsl, metal - /// Default to glsl if not specified - /// Path to vertex program - /// Path to fragment program - /// Shader parameter - can be repeated within plugin SDF element - /// Name of uniform variable bound to the shader - /// Type of shader, i.e. vertex, fragment - /// Variable type: float, int, float_array, int_array - /// Value to set the shader parameter to. The vallue string can - /// be an int, float, or a space delimited array of ints or - /// floats. It can also be 'TIME', in which case the value will - /// be bound to sim time. + /// - ``: Shader to use - can be repeated to specify shader programs + /// written in different languages. + /// - Attributes: + /// - `language`: Shader language. Possible values: glsl, metal + /// Default to glsl if not specified + /// - ``: Path to vertex program + /// - ``: Path to fragment program + /// - ``: Shader parameter - can be repeated within plugin SDF element + /// - ``: Name of uniform variable bound to the shader + /// - ``: Type of shader, i.e. vertex, fragment + /// - ``: Variable type: float, int, float_array, int_array + /// - ``: Value to set the shader parameter to. The vallue string can + /// be an int, float, or a space delimited array of ints or + /// floats. It can also be "TIME", in which case the value will + /// be bound to sim time. /// - /// Example usage: + /// ## Example /// - /// \verbatim - /// - /// - /// materials/my_vs.glsl - /// materials/my_fs.glsl - /// - /// - /// - /// ambient - /// fragment - /// float_array - /// 1.0 0.0 0.0 1.0 - /// - /// - /// \endverbatim + /// ``` + /// + /// + /// materials/my_vs.glsl + /// materials/my_fs.glsl + /// + /// + /// + /// ambient + /// fragment + /// float_array + /// 1.0 0.0 0.0 1.0 + /// + /// + /// ``` class ShaderParam : public System, public ISystemConfigure, diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 5c9cdab562..708fddf883 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -46,64 +46,75 @@ namespace systems /// `/model/{ns}/joint/{joint_name}/force`. /// /// ## System Parameters - /// - - The namespace in which the robot exists. The plugin will + /// + /// - ``: The namespace in which the robot exists. The plugin will /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`or /// `/model/{namespace}/joint/{joint_name}/cmd_vel` depending on the mode of /// operation. If {topic} is set then the plugin will listen on /// {namespace}/{topic} /// [Optional] - /// - - The topic for receiving thrust commands. [Optional] - /// - - This is the joint in the model which corresponds to the + /// - ``: The topic for receiving thrust commands. [Optional] + /// - ``: This is the joint in the model which corresponds to the /// propeller. [Required] - /// - - If set to true will make the thruster + /// - ``: If set to true will make the thruster /// plugin accept commands in angular velocity in radians per seconds in /// terms of newtons. [Optional, Boolean, defaults to false] - /// - - The fluid density of the liquid in which the thruster + /// - ``: The fluid density of the liquid in which the thruster /// is operating in. [Optional, kg/m^3, defaults to 1000 kg/m^3] - /// - - The diameter of the propeller in meters. + /// - ``: The diameter of the propeller in meters. /// [Optional, m, defaults to 0.02m] - /// - - This is the coefficient which relates the angular - /// velocity to thrust. A positive coefficient corresponds to a clockwise - /// propeller, which is a propeller that spins clockwise under positive - /// thrust when viewed along the parent link from stern (-x) to bow (+x). - /// [Optional, no units, defaults to 1.0] - /// - /// omega = sqrt(thrust / - /// (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) - /// - /// Where omega is the propeller's angular velocity in rad/s. - /// - - If true, use joint velocity commands to rotate the + /// - ``: This is the coefficient which relates the + /// angular velocity to thrust. A positive coefficient corresponds to a + /// clockwise propeller, which is a propeller that spins clockwise under + /// positive thrust when viewed along the parent link from stern (-x) to + /// bow (+x). [Optional, no units, defaults to 1.0] + /// ``` + /// omega = sqrt(thrust / + /// (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) + /// ``` + /// where omega is the propeller's angular velocity in rad/s. + /// - ``: If true, use joint velocity commands to rotate the /// propeller. If false, use a PID controller to apply wrenches directly to /// the propeller link instead. [Optional, defaults to false]. - /// - - Proportional gain for joint PID controller. [Optional, - /// no units, defaults to 0.1] - /// - - Integral gain for joint PID controller. [Optional, - /// no units, defaults to 0.0] - /// - - Derivative gain for joint PID controller. [Optional, - /// no units, defaults to 0.0] - /// - - Maximum input thrust or angular velocity command. - /// [Optional, defaults to 1000N or 1000rad/s] - /// - - Minimum input thrust or angular velocity command. - /// [Optional, defaults to -1000N or -1000rad/s] - /// - - Deadband of the thruster. Absolute value below which the - /// thruster won't spin nor generate thrust. This value can - /// be changed at runtime using a topic. The topic is either - /// `/model/{ns}/joint/{jointName}/enable_deadband` or - /// `{ns}/{topic}/enable_deadband` depending on other params - /// - - Relative speed reduction between the water + /// - ``: Proportional gain for joint PID controller. [Optional, + /// no units, defaults to 0.1] + /// - ``: Integral gain for joint PID controller. [Optional, + /// no units, defaults to 0.0] + /// - ``: Derivative gain for joint PID controller. [Optional, + /// no units, defaults to 0.0] + /// - ``: Maximum input thrust or angular velocity command. + /// [Optional, defaults to 1000N or 1000rad/s] + /// - ``: Minimum input thrust or angular velocity command. + /// [Optional, defaults to -1000N or -1000rad/s] + /// - ``: Deadband of the thruster. Absolute value below which the + /// thruster won't spin nor generate thrust. This value can + /// be changed at runtime using a topic. The topic is either + /// `/model/{ns}/joint/{jointName}/enable_deadband` or + /// `{ns}/{topic}/enable_deadband` depending on other params + /// - ``: Relative speed reduction between the water /// at the propeller (Va) vs behind the vessel. /// [Optional, defults to 0.2] - /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: - /// Va = (1 - wake_fraction) * advance_speed - /// - - Constant given by the open water propeller diagram. Used - /// in the calculation of the thrust coefficient (Kt). - /// [Optional, defults to 1] - /// - - Constant given by the open water propeller diagram. Used - /// in the calculation of the thrust coefficient (Kt). - /// [Optional, defults to 0] - /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: - /// Kt = alpha_1 * alpha_2 * (Va/(propeller_revolution * propeller_diameter)) + /// + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// ``` + /// Va = (1 - wake_fraction) * advance_speed + /// ``` + /// + /// - ``: Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 1] + /// - ``: Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 0] + /// + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// ``` + /// Kt = alpha_1 * alpha_2 * + /// (Va / (propeller_revolution * propeller_diameter)) + /// ``` + /// /// ## Example + /// /// An example configuration is installed with Gazebo. The example /// uses the LiftDrag plugin to apply steering controls. It also uses the /// thruster plugin to propell the craft and the buoyancy plugin for buoyant @@ -112,15 +123,17 @@ namespace systems /// gz sim auv_controls.sdf /// ``` /// To control the rudder of the craft run the following: - /// ``` - /// gz topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos - /// -m gz.msgs.Double -p 'data: -0.17' - /// ``` + /** ``` + gz topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos \ + -m gz.msgs.Double -p 'data: -0.17' + ``` + **/ /// To apply a thrust you may run the following command: - /// ``` - /// gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust - /// -m gz.msgs.Double -p 'data: -31' - /// ``` + /** ``` + gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust \ + -m gz.msgs.Double -p 'data: -31' + ``` + **/ /// The vehicle should move in a circle. class Thruster: public gz::sim::System, diff --git a/src/systems/touch_plugin/TouchPlugin.hh b/src/systems/touch_plugin/TouchPlugin.hh index 0912671ca0..da1be1e841 100644 --- a/src/systems/touch_plugin/TouchPlugin.hh +++ b/src/systems/touch_plugin/TouchPlugin.hh @@ -42,7 +42,7 @@ namespace systems /// The plugin requires that a contact sensors is placed in at least one /// link on the model on which this plugin is attached. /// - /// Parameters: + /// ## System Parameters /// /// - `` Name, or substring of a name, that identifies the target /// collision entity/entities. @@ -50,15 +50,15 @@ namespace systems /// entities, so it can possibly match more than one collision. /// For example, using the name of a model will match all of its /// collisions (scoped name - /// /model_name/link_name/collision_name). + /// `/model_name/link_name/collision_name`). /// /// - `