From 924b50673d53166cc9cdec4f50dc67078ecad392 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Sat, 18 Sep 2021 00:06:10 +0800 Subject: [PATCH] Buoyancy engine (#1009) Signed-off-by: Arjo Chakravarty Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel Signed-off-by: William Lew --- examples/worlds/buoyancy_engine.sdf | 142 ++++++++++ src/systems/CMakeLists.txt | 1 + src/systems/buoyancy_engine/BuoyancyEngine.cc | 249 ++++++++++++++++++ src/systems/buoyancy_engine/BuoyancyEngine.hh | 114 ++++++++ src/systems/buoyancy_engine/CMakeLists.txt | 7 + test/integration/CMakeLists.txt | 1 + test/integration/buoyancy_engine.cc | 152 +++++++++++ test/worlds/buoyancy_engine.sdf | 72 +++++ 8 files changed, 738 insertions(+) create mode 100644 examples/worlds/buoyancy_engine.sdf create mode 100644 src/systems/buoyancy_engine/BuoyancyEngine.cc create mode 100644 src/systems/buoyancy_engine/BuoyancyEngine.hh create mode 100644 src/systems/buoyancy_engine/CMakeLists.txt create mode 100644 test/integration/buoyancy_engine.cc create mode 100644 test/worlds/buoyancy_engine.sdf diff --git a/examples/worlds/buoyancy_engine.sdf b/examples/worlds/buoyancy_engine.sdf new file mode 100644 index 00000000000..145e955cab7 --- /dev/null +++ b/examples/worlds/buoyancy_engine.sdf @@ -0,0 +1,142 @@ + + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0 0 0 0 + + + 1000 + + 133.3333 + 133.3333 + 133.3333 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + body + buoyant_box + 0.0 + 0.002 + 0.002 + 0.003 + 0.0003 + + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index eac401c9095..1a3012fa23a 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -92,6 +92,7 @@ add_subdirectory(apply_joint_force) add_subdirectory(battery_plugin) add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) +add_subdirectory(buoyancy_engine) add_subdirectory(collada_world_exporter) add_subdirectory(contact) add_subdirectory(camera_video_recorder) diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.cc b/src/systems/buoyancy_engine/BuoyancyEngine.cc new file mode 100644 index 00000000000..14131f6c2d9 --- /dev/null +++ b/src/systems/buoyancy_engine/BuoyancyEngine.cc @@ -0,0 +1,249 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "BuoyancyEngine.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::BuoyancyEnginePrivateData +{ + /// \brief Callback for incoming commands + /// \param[in] _volumeSetPoint - ignition message containing the desired + /// volume (in m^3) to fill/drain bladder to. + public: void OnCmdBuoyancyEngine( + const ignition::msgs::Double &_volumeSetPoint); + + /// \brief Current volume of bladder in m^3 + public: double bladderVolume = 3e-5; + + /// \brief Maximum inflation rate in m^3*s^-1 + public: double maxInflationRate = 3e-6; + + /// \brief Set-point for volume, in m^3 + public: double volumeSetPoint = 0.000030; + + /// \brief Minimum volume of bladder in m^3 + public: double minVolume = 0.000030; + + /// \brief Maximum volume of bladder in m^3 + public: double maxVolume = 0.000990; + + /// \brief The link which the bladder is attached to + public: ignition::gazebo::Entity linkEntity{kNullEntity}; + + /// \brief The world entity + public: Entity world{kNullEntity}; + + /// \brief The fluid density in kg*m^-3 + public: double fluidDensity = 1000; + + /// \brief The neutral volume in m^3 + public: double neutralVolume = 0.0003; + + /// \brief Trasport node for control + public: ignition::transport::Node node; + + /// \brief Publishes bladder status + public: ignition::transport::Node::Publisher statusPub; + + /// \brief mutex for protecting bladder volume and set point. + public: std::mutex mtx; +}; + +////////////////////////////////////////////////// +void BuoyancyEnginePrivateData::OnCmdBuoyancyEngine( + const ignition::msgs::Double &_volumeSetpoint) +{ + auto volume = std::max(this->minVolume, _volumeSetpoint.data()); + volume = std::min(volume, this->maxVolume); + + std::lock_guard lock(this->mtx); + this->volumeSetPoint = volume; +} + +////////////////////////////////////////////////// +BuoyancyEnginePlugin::BuoyancyEnginePlugin() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void BuoyancyEnginePlugin::Configure( + const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/) +{ + auto model = ignition::gazebo::Model(_entity); + if (!_sdf->HasElement("link_name")) + { + ignerr << "Buoyancy Engine must be attached to some link." << std::endl; + return; + } + + this->dataPtr->linkEntity = + model.LinkByName(_ecm, _sdf->Get("link_name")); + if (this->dataPtr->linkEntity == kNullEntity) + { + ignerr << "Link [" << _sdf->Get("link_name") + << "] was not found in model [" << model.Name(_ecm) << "]" << std::endl; + return; + } + + if (_sdf->HasElement("min_volume")) + { + this->dataPtr->minVolume = _sdf->Get("min_volume"); + } + + if (_sdf->HasElement("max_volume")) + { + this->dataPtr->maxVolume = _sdf->Get("max_volume"); + } + + if (_sdf->HasElement("fluid_density")) + { + this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); + } + + this->dataPtr->bladderVolume = this->dataPtr->minVolume; + if (_sdf->HasElement("default_volume")) + { + this->dataPtr->bladderVolume = _sdf->Get("default_volume"); + this->dataPtr->volumeSetPoint = this->dataPtr->bladderVolume; + } + + if (_sdf->HasElement("neutral_volume")) + { + this->dataPtr->neutralVolume = _sdf->Get("neutral_volume"); + } + + if(_sdf->HasElement("max_inflation_rate")) + { + this->dataPtr->maxInflationRate = _sdf->Get("max_inflation_rate"); + } + + this->dataPtr->world = _ecm.EntityByComponents(components::World()); + if (this->dataPtr->world == kNullEntity) + { + ignerr << "World entity not found" <HasElement("namespace")) + { + cmdTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + _sdf->Get("namespace") + "/buoyancy_engine/"); + statusTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + _sdf->Get("namespace") + + "/buoyancy_engine/current_volume"); + } + + if(!this->dataPtr->node.Subscribe(cmdTopic, + &BuoyancyEnginePrivateData::OnCmdBuoyancyEngine, this->dataPtr.get())) + { + ignerr << "Failed to subscribe to [" << cmdTopic << "]" << std::endl; + } + + this->dataPtr->statusPub = + this->dataPtr->node.Advertise(statusTopic); + + igndbg << "Listening to commands on [" << cmdTopic + << "], publishing status on [" << statusTopic << "]" <> DurationInSecs; + auto dt = std::chrono::duration_cast(_info.dt).count(); + + ignition::msgs::Double msg; + + const components::Gravity *gravity = _ecm.Component( + this->dataPtr->world); + if (!gravity) + { + ignerr << "World has no gravity component" << std::endl; + return; + } + + math::Vector3d zForce; + { + std::lock_guard lock(this->dataPtr->mtx); + // Adjust the bladder volume using the pump. Assume ability to pump at + // max flow rate + if (this->dataPtr->bladderVolume < this->dataPtr->volumeSetPoint) + { + this->dataPtr->bladderVolume += + std::min( + dt * this->dataPtr->maxInflationRate, + this->dataPtr->volumeSetPoint - this->dataPtr->bladderVolume + ); + } + else if (this->dataPtr->bladderVolume > this->dataPtr->volumeSetPoint) + { + this->dataPtr->bladderVolume -= + std::min( + dt * this->dataPtr->maxInflationRate, + this->dataPtr->bladderVolume - this->dataPtr->volumeSetPoint + ); + } + + /// Populate status message + msg.set_data(this->dataPtr->bladderVolume); + this->dataPtr->statusPub.Publish(msg); + + // Simply use Archimede's principle to apply a force at the desired link + // position. We take off the neutral buoyancy element in order to simulate + // the mass of the oil in the bladder. + zForce = - gravity->Data() * this->dataPtr->fluidDensity + * (this->dataPtr->bladderVolume - this->dataPtr->neutralVolume); + } + ignition::gazebo::Link link(this->dataPtr->linkEntity); + link.AddWorldWrench(_ecm, zForce, {0, 0, 0}); +} + +IGNITION_ADD_PLUGIN( + BuoyancyEnginePlugin, + ignition::gazebo::System, + BuoyancyEnginePlugin::ISystemConfigure, + BuoyancyEnginePlugin::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(BuoyancyEnginePlugin, + "ignition::gazebo::systems::BuoyancyEngine") diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.hh b/src/systems/buoyancy_engine/BuoyancyEngine.hh new file mode 100644 index 00000000000..c4dd70cf809 --- /dev/null +++ b/src/systems/buoyancy_engine/BuoyancyEngine.hh @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_BUOYANCYENGINE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_BUOYANCYENGINE_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + class BuoyancyEnginePrivateData; + + /// \brief This class provides a simple mechanical bladder which is used to + /// control the buoyancy of an underwater glider. It uses Archimedes' + /// principle to apply an upward force based on the volume of the bladder. It + /// listens to the topic `buoyancy_engine` or + /// `/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 + /// `/model/{namespace}/buoyancy_engine` [optional, string] + /// - 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, + /// default=0.0003m^3] + /// - Maximum volume of the engine [optional, float, + /// default=0.00099m^3] + /// - 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] + /// + /// ## Topics + /// * Subscribes to a ignition::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine`. This is the set point for the + /// engine. + /// * Publishes a ignition::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine/current_volume` on the current volume + /// + /// ## Examples + /// To get started run: + /// ``` + /// ign gazebo buoyancy_engine.sdf + /// ``` + /// Enter the following in a separate terminal: + /// ``` + /// ign topic -t /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double + /// -p "data: 0.003" + /// ``` + /// To see the box float up. + /// ``` + /// ign topic -t /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double + /// -p "data: 0.001" + /// ``` + /// To see the box go down. + /// To see the current volume enter: + /// ``` + /// ign topic -t /model/buoyant_box/buoyancy_engine/current_volume -e + /// ``` + class BuoyancyEnginePlugin: + public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate + { + /// \brief Constructor + public: BuoyancyEnginePlugin(); + + // Documentation inherited + public: void Configure( + const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/ + ); + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/src/systems/buoyancy_engine/CMakeLists.txt b/src/systems/buoyancy_engine/CMakeLists.txt new file mode 100644 index 00000000000..b849e893096 --- /dev/null +++ b/src/systems/buoyancy_engine/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(buoyancy-engine + SOURCES + BuoyancyEngine.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 9753c3ef2c9..d4fb16a10ae 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -8,6 +8,7 @@ set(tests battery_plugin.cc breadcrumbs.cc buoyancy.cc + buoyancy_engine.cc collada_world_exporter.cc components.cc contact_system.cc diff --git a/test/integration/buoyancy_engine.cc b/test/integration/buoyancy_engine.cc new file mode 100644 index 00000000000..822666d22c7 --- /dev/null +++ b/test/integration/buoyancy_engine.cc @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +#include +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Model.hh" + +#include "ignition/gazebo/test_config.hh" +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class BuoyancyEngineTest : public InternalFixture<::testing::Test> +{ + // Documentation inherited + protected: void SetUp() override + { + InternalFixture::SetUp(); + this->pub = this->node.Advertise( + "/model/buoyant_box/buoyancy_engine/"); + } + + /// \brief Node for communication + public: ignition::transport::Node node; + + /// \brief Publishes commands + public: ignition::transport::Node::Publisher pub; +}; + +///////////////////////////////////////////////// +TEST_F(BuoyancyEngineTest, TestDownward) +{ + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "buoyancy_engine.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::size_t iterations = 10000; + + test::Relay testSystem; + std::vector poses; + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &_ecm) + { + // Check pose + Entity buoyantBox = _ecm.EntityByComponents( + components::Model(), components::Name("buoyant_box")); + EXPECT_NE(kNullEntity, buoyantBox); + + auto submarineBuoyantPose = _ecm.Component(buoyantBox); + EXPECT_NE(submarineBuoyantPose, nullptr); + if (submarineBuoyantPose == nullptr) + { + ignerr << "Unable to get pose" <Data()); + }); + + server.AddSystem(testSystem.systemPtr); + ignition::msgs::Double volume; + volume.set_data(0); + this->pub.Publish(volume); + server.Run(true, iterations, false); + + EXPECT_LT(poses.rbegin()->Pos().Z(), poses.begin()->Pos().Z()); + EXPECT_NEAR(poses.rbegin()->Pos().X(), poses.begin()->Pos().X(), 1e-3); + EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3); +} + +///////////////////////////////////////////////// +TEST_F(BuoyancyEngineTest, TestUpward) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/buoyancy_engine.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::size_t iterations = 10000; + + test::Relay testSystem; + std::vector poses; + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &_ecm) + { + // Check pose + Entity buoyantBox = _ecm.EntityByComponents( + components::Model(), components::Name("buoyant_box")); + EXPECT_NE(kNullEntity, buoyantBox); + + auto submarineBuoyantPose = _ecm.Component(buoyantBox); + EXPECT_NE(submarineBuoyantPose, nullptr); + if (submarineBuoyantPose == nullptr) + { + ignerr << "Unable to get pose" <Data()); + }); + + server.AddSystem(testSystem.systemPtr); + ignition::msgs::Double volume; + volume.set_data(10); + this->pub.Publish(volume); + server.Run(true, iterations, false); + + EXPECT_GT(poses.rbegin()->Pos().Z(), poses.begin()->Pos().Z()); + EXPECT_NEAR(poses.rbegin()->Pos().X(), poses.begin()->Pos().X(), 1e-3); + EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3); +} + diff --git a/test/worlds/buoyancy_engine.sdf b/test/worlds/buoyancy_engine.sdf new file mode 100644 index 00000000000..51ce5897bab --- /dev/null +++ b/test/worlds/buoyancy_engine.sdf @@ -0,0 +1,72 @@ + + + + + 0.001 + 1.0 + + + + + 1000 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0 0 0 0 + + + 1000 + + 133.3333 + 133.3333 + 133.3333 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + body + buoyant_box + 0.001 + 0.002 + 0.002 + 0.003 + 0.0003 + + + + +