Buoyancy engine

examples/worlds/buoyancy_engine.sdf

@@ -0,0 +1,91 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="buoyant_tethys">
+    <scene>
+      <!-- For turquoise ambient -->
+      <ambient>0.0 1.0 1.0</ambient>
+      <background>0.0 0.7 0.8</background>
+    </scene>
+
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-contact-system"
+      name="ignition::gazebo::systems::Contact">
+    </plugin>
+
+    <plugin
+      filename="ignition-gazebo-buoyancy-system"
+      name="ignition::gazebo::systems::Buoyancy">
+      <uniform_fluid_density>1000</uniform_fluid_density>
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="buoyant_box">
+      <pose>0 0 0 0 0 0</pose>
+      <link name='body'>
+        <inertial>
+          <mass>1000</mass>
+          <inertia>
+            <ixx>133.3333</ixx>
+            <iyy>133.3333</iyy>
+            <izz>133.3333</izz>
+          </inertia>
+        </inertial>
+        <visual name='body_visual'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='body_collision'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+      <plugin
+        filename="libignition-gazebo-buoyancy-engine-system.so"
+        name="ignition::gazebo::systems::BuoyancyEngine">
+        <link_name>body</link_name>
+        <namespace>buoyant_box</namespace>
+        <min_volume>0.0</min_volume>
+        <neutral_volume>0.002</neutral_volume>
+        <default_volume>0.002</default_volume>
+        <max_volume>0.003</max_volume>
+        <max_inflation_rate>0.0003</max_inflation_rate>
+      </plugin>
+
+    </model>
+  </world>
+</sdf>

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)

src/systems/buoyancy_engine/BuoyancyEngine.cc

@@ -0,0 +1,224 @@
+/*
+ * 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 <algorithm>
+#include <chrono>
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include <ignition/gazebo/Link.hh>
+#include <ignition/msgs/double.pb.h>
+#include <ignition/plugin/Register.hh>
+#include <ignition/transport/Node.hh>
+
+#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 cc) 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
+  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;
+
+  /// \brief The gravitational force kg*m*s^-2
+  public: double gravity = 9.81;
+
+  /// \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 Get bladder status
+  public: ignition::transport::Node::Publisher statusPub;
+
+  /// \brief mutex
+  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;
+  igndbg << "Updating volume " << volume <<"\n";
+}
+
+//////////////////////////////////////////////////
+BuoyancyEnginePlugin::BuoyancyEnginePlugin()
+  : dataPtr(std::make_unique<BuoyancyEnginePrivateData>())
+{
+}
+
+//////////////////////////////////////////////////
+void BuoyancyEnginePlugin::Configure(
+  const ignition::gazebo::Entity &_entity,
+  const std::shared_ptr<const sdf::Element> &_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<std::string>("link_name"));
+
+  if (_sdf->HasElement("min_volume"))
+  {
+    this->dataPtr->minVolume = _sdf->Get<double>("min_volume");
+  }
+
+  if (_sdf->HasElement("max_volume"))
+  {
+    this->dataPtr->maxVolume = _sdf->Get<double>("max_volume");
+  }
+
+  if (_sdf->HasElement("fluid_density"))
+  {
+    this->dataPtr->fluidDensity = _sdf->Get<double>("fluid_density");
+  }
+
+  this->dataPtr->bladderVolume = this->dataPtr->minVolume;
+  if (_sdf->HasElement("default_volume"))
+  {
+    this->dataPtr->bladderVolume = _sdf->Get<double>("default_volume");
+    this->dataPtr->volumeSetPoint = this->dataPtr->bladderVolume;
+  }
+
+  if (_sdf->HasElement("neutral_volume"))
+  {
+    this->dataPtr->neutralVolume = _sdf->Get<double>("neutral_volume");
+  }
+
+  std::string cmdTopic = "/buoyancy_engine/";
+  std::string statusTopic = "/buoyancy_engine/current_volume";
+  if (_sdf->HasElement("namespace"))
+  {
+    cmdTopic = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + _sdf->Get<std::string>("namespace") + "/buoyancy_engine/");
+    statusTopic = ignition::transport::TopicUtils::AsValidTopic(
+      "/model/" + _sdf->Get<std::string>("namespace")
+      + "/buoyancy_engine/current_volume");
+  }
+  igndbg << "listening on topic: " << cmdTopic <<std::endl;
+
+  if(_sdf->HasElement("max_inflation_rate"))
+  {
+    this->dataPtr->maxInflationRate = _sdf->Get<double>("max_inflation_rate");
+  }
+
+  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<ignition::msgs::Double>(statusTopic);
+}
+
+//////////////////////////////////////////////////
+void BuoyancyEnginePlugin::PreUpdate(
+  const ignition::gazebo::UpdateInfo &_info,
+  ignition::gazebo::EntityComponentManager &_ecm)
+{
+  if (_info.paused)
+    return;
+
+  typedef std::chrono::duration<float, std::ratio<1L, 1L>> DurationInSecs;
+  auto dt = std::chrono::duration_cast<DurationInSecs>(_info.dt).count();
+
+  ignition::msgs::Double msg;
+
+  double forceMag;
+  {
+    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.
+    forceMag = this->dataPtr->gravity * this->dataPtr->fluidDensity
+      * (this->dataPtr->bladderVolume - this->dataPtr->neutralVolume);
+  }
+  ignition::gazebo::Link link(this->dataPtr->linkEntity);
+  link.AddWorldWrench(_ecm, {0, 0, forceMag}, {0, 0, 0});
+}
+
+IGNITION_ADD_PLUGIN(
+  BuoyancyEnginePlugin,
+  ignition::gazebo::System,
+  BuoyancyEnginePlugin::ISystemConfigure,
+  BuoyancyEnginePlugin::ISystemPreUpdate)
+
+IGNITION_ADD_PLUGIN_ALIAS(BuoyancyEnginePlugin,
+                          "ignition::gazebo::systems::BuoyancyEngine")