Force Visuallization

[arjo129]

🎉 New feature

Summary

Force visualization - Opening PR for visibility.

This PR adds the ability to visualize forces via a GUI plugin as they get applied by plugins. It depends on the following PR in ignition msgs: gazebosim/gz-msgs#200

Here are some fancier gifs:

Test it

Go to the three dots and select "Visuallize Forces".

TODO:

• Add link names into panel instead of entity ID
• Add support for it in more plugins
• Use arrows (do this in a separate PR)

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

[chapulina]

This requires a new message, it should target garden I think.

Adding a new message is backwards compatible, so I think we should be able to target this at Citadel 🏰

[chapulina]

Here are some preliminary comments

[chapulina]

Can these be member variables? Otherwise all links will share the same node and publisher.

It would also be good to namespace the topic with the world name, in case there are multiple simulations running.

Suggested change

	static auto pub = node.Advertise<msgs::WrenchVisual>("/force_viz");
	static auto pub = node.Advertise<msgs::WrenchVisual>("/world/" + worldName + "/view_forces");

[chapulina]

Even with your ign-gui marker performance PR (gazebosim/gz-gui#307), I'm still getting really bad performance for the markers.

I think we could drastically reduce the number of markers if we skip publishing the wrench visuals in some cases:

1. If the wrench is zero we shouldn't even add it and return straight away
2. If the wrench is the same as before, we don't need to publish new markers.

I started prototyping these ideas, but it still needs some cleanup. Let me know what you think, here's what I have so far:

diff

diff --git a/src/Link.cc b/src/Link.cc
index ef30f76a..f3222bee 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -44,6 +44,9 @@ class ignition::gazebo::LinkPrivate
 {
   /// \brief Id of link entity.
   public: Entity id{kNullEntity};
+
+  /// \brief
+  public: std::pair<math::Vector3d, math::Vector3d> lastPublishedWrench;
 };
 
 using namespace ignition;
@@ -362,6 +365,17 @@ void Link::AddAndVisualizeWorldWrench(EntityComponentManager &_ecm,
                          const math::Vector3d &_torque,
                          const std::string &_pluginName) const
 {
+  if (_force == math::Vector3d::Zero && _torque == math::Vector3d::Zero)
+    return;
+
+  this->AddWorldWrench(_ecm, _force, _torque);
+  if (_force.Equal(this->dataPtr->lastPublishedWrench.first, 0.1) &&
+      _torque.Equal(this->dataPtr->lastPublishedWrench.second, 0.1))
+  {
+    return;
+  }
+  this->dataPtr->lastPublishedWrench = {_force, _torque};
+
   static transport::Node node;
   static auto pub = node.Advertise<msgs::WrenchVisual>("/force_viz");
 
@@ -374,5 +388,4 @@ void Link::AddAndVisualizeWorldWrench(EntityComponentManager &_ecm,
   msgs::Set(wrenchVisual.mutable_wrench()->mutable_force(), _force);
   msgs::Set(wrenchVisual.mutable_wrench()->mutable_torque(), _torque);
   pub.Publish(wrenchVisual);
-  this->AddWorldWrench(_ecm, _force, _torque);
 }
diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc
index 49445ac5..61c5d224 100644
--- a/src/systems/buoyancy/Buoyancy.cc
+++ b/src/systems/buoyancy/Buoyancy.cc
@@ -127,6 +127,8 @@ class ignition::gazebo::systems::BuoyancyPrivate
   /// \brief Scoped names of entities that buoyancy should apply to. If empty,
   /// all links will receive buoyancy.
   public: std::unordered_set<std::string> enabled;
+
+  public: std::set<std::shared_ptr<Link>> links;
 };
 
 //////////////////////////////////////////////////
@@ -345,8 +347,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
       return true;
     }
 
-    Link link(_entity);
-
     std::vector<Entity> collisions = _ecm.ChildrenByComponents(
         _entity, components::Collision());
 
@@ -445,7 +445,21 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
       // World pose of the link.
       math::Pose3d linkWorldPose = worldPose(_entity, _ecm);
 
-      Link link(_entity);
+      std::shared_ptr<Link> link{nullptr};
+      auto it = std::find_if(this->dataPtr->links.begin(), this->dataPtr->links.end(),
+          [&_entity](const std::shared_ptr<Link> &_link)
+      {
+        return _link->Entity() == _entity;
+      });
+      if (it != this->dataPtr->links.end())
+      {
+        link = *it;
+      }
+      else
+      {
+        link = std::make_shared<Link>(_entity);
+        this->dataPtr->links.insert(link);
+      }
 
       math::Vector3d buoyancy;
       // By Archimedes' principle,
@@ -467,7 +481,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
 
         // Apply the wrench to the link. This wrench is applied in the
         // Physics System.
-        link.AddAndVisualizeWorldWrench(_ecm,
+        link->AddAndVisualizeWorldWrench(_ecm,
           buoyancy, torque, "BuoyancyPlugin");
         return true;
       }
@@ -526,10 +540,10 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
           }
         }
         auto [force, torque] = this->dataPtr->ResolveForces(
-        link.WorldInertialPose(_ecm).value());
+        link->WorldInertialPose(_ecm).value());
         // Apply the wrench to the link. This wrench is applied in the
         // Physics System.
-        link.AddAndVisualizeWorldWrench(
+        link->AddAndVisualizeWorldWrench(
           _ecm, force, torque, "BuoyancyPlugin");
       }
 
diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.cc b/src/systems/buoyancy_engine/BuoyancyEngine.cc
index eda84a3c..9d4a4262 100644
--- a/src/systems/buoyancy_engine/BuoyancyEngine.cc
+++ b/src/systems/buoyancy_engine/BuoyancyEngine.cc
@@ -42,6 +42,8 @@ class ignition::gazebo::systems::BuoyancyEnginePrivateData
   public: void OnCmdBuoyancyEngine(
     const ignition::msgs::Double &_volumeSetPoint);
 
+  public: Link link{kNullEntity};
+
   /// \brief Current volume of bladder in m^3
   public: double bladderVolume = 3e-5;
 
@@ -118,6 +120,7 @@ void BuoyancyEnginePlugin::Configure(
       << "] was not found in model [" << model.Name(_ecm) << "]" << std::endl;
     return;
   }
+  this->dataPtr->link = Link(this->dataPtr->linkEntity);
 
   if (_sdf->HasElement("min_volume"))
   {
@@ -235,8 +238,7 @@ void BuoyancyEnginePlugin::PreUpdate(
     zForce = - gravity->Data() * this->dataPtr->fluidDensity
       * (this->dataPtr->bladderVolume - this->dataPtr->neutralVolume);
   }
-  ignition::gazebo::Link link(this->dataPtr->linkEntity);
-  link.AddAndVisualizeWorldWrench(_ecm, zForce, {0, 0, 0}, "Buoyancy Engine");
+  this->dataPtr->link.AddAndVisualizeWorldWrench(_ecm, zForce, {0, 0, 0}, "Buoyancy Engine");
 }
 
 IGNITION_ADD_PLUGIN(
diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc
index f797a314..be3f0956 100644
--- a/src/systems/lift_drag/LiftDrag.cc
+++ b/src/systems/lift_drag/LiftDrag.cc
@@ -59,6 +59,9 @@ class ignition::gazebo::systems::LiftDragPrivate
   /// \brief Model interface
   public: Model model{kNullEntity};
 
+  /// \brief Model interface
+  public: Link link{kNullEntity};
+
   /// \brief Coefficient of Lift / alpha slope.
   /// Lift = C_L * q * S
   /// where q (dynamic pressure) = 0.5 * rho * v^2
@@ -198,6 +201,7 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm,
       this->validConfig = false;
       return;
     }
+    this->link = Link(this->linkEntity);
   }
   else
   {
@@ -467,8 +471,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm)
   // \todo(addisu) Create a convenient API for applying forces at offset
   // positions
   const auto totalTorque = torque + cpWorld.Cross(force);
-  Link link(this->linkEntity);
-  link.AddAndVisualizeWorldWrench(_ecm, force, totalTorque, "LiftDrag");
+  this->link.AddAndVisualizeWorldWrench(_ecm, force, totalTorque, "LiftDrag");
 
   // Debug
   // auto linkName = _ecm.Component<components::Name>(this->linkEntity)->Data();

[arjo129]

I haven't had time to work in more performance improvements upstream. I do like the idea of not publishing a zero wmarker as the marker has been published already. Curious though how slow it is though. The cacheing in the buoyancy plugin seems like a good idea but doesn't belong in this PR. Fundamentally the upstream marker manager should be rewritten though as we seem to be doing a lot of work on a render thread.

[chapulina]

Curious though how slow it is though.

With the ign-gui PR, tens of thousands of markers get dropped for me, which causes the visualization to be outdated and eventually it doesn't match the model anymore.

The cacheing in the buoyancy plugin seems like a good idea but doesn't belong in this PR.

The caching that I proposed above happens within Link and is used directly by the new function being added in this PR.

I think it's ok for this kind of optimization to come later if we have a working version to start with. But this PR is not working for me. I'm not being able to reproduce your gifs from above, maybe I just need a different branch combination?

Fundamentally the upstream marker manager should be rewritten though as we seem to be doing a lot of work on a render thread.

I agree that the marker manager could be improved, but I think there are lots of improvements we can do in this PR too to avoid publishing unnecessary markers. Even if the marker manager is improved to handle a stream of thousands of repeated markers, I don't think we should be publishing that.

[srmainwaring]

Hi @arjo129, it would be great to have force visualisation supported and have started to look at / modify this PR on https://github.com/srmainwaring/gz-sim/commits/arjo/forceviz.

It's WIP but some thoughts so far:

• Throttle the publishing of the wrench_visual messages from the force generating plugins. For typical used this only needs to be at the render rate (say 50Hz) and not at the force update rate. It could be configurable if say you wanted to step one update at a time.
• Use visuals directly in the GUI plugin rather than use markers. Reduces the transport traffic and allows direct control over the visual lifetime and rendering.
• Replace wrench_visual msg with entity_wrench - this is not a drop in replacement as there are fields missing. Also it may be useful to see the point at which a wrench is applied when that is not the link origin?

It's still a gz::sim::GuiSystem plugin but I've added an eventFilter to manage the rendering. Not sure if there are any examples where the plugin monitors the ECM for updates (on the GUI thread) then renders (on the render thread) but would be interested on recommended best practice if there are.

[arjo129]

Thanks! It's great to have some help. Yeah those are great suggestions. For now your best bet is to publish messages as the gui client runs in a different process from the server.

[srmainwaring]

For now your best bet is to publish messages as the gui client runs in a different process from the server.

Yes, the physics plugins running in the server will still publish a wrench message of some type (at say 50Hz), but the visualization plugin will render directly rather than publish marker messages. This should help performance.

[arjo129]

That sounds perfect.

[srmainwaring]

I've got an initial version running using entity_wrench instead of the custom wrench_visual messages. I've used your approach to retrieve the messages via components from the ECM in the GUISystem plugin, however only the first message is retrieved and subsequent updates from Link are not captured. Do you have any insight what might be causing this? (your comments in the last few commits seem to indicate this was an outstanding problem).

[arjo129]

That is indeed something that I ran into. For whatever reason updating the message does not seem to work as expected. At this point my best guess is it may have something to do with serialization. Perhaps we aren't serializing the whole ecm when sending it over to the gui and the way that we handle incremental serialization may be broken.

[srmainwaring]

and the way that we handle incremental serialization may be broken.

Yes, there is something odd going on.

If I try to retrieve the pose and entityWrench (a new component containing msgs::EntityWrench) components using

    _ecm.Each<components::WorldPose, components::EntityWrench>(
    [&](const Entity &_entity,
        components::WorldPose *_worldPose,
        components::EntityWrench *_entityWrench) -> bool
    {
       ...
    }

only the first update is received.

On the other hand:

    _ecm.Each<components::WorldPose>(
    [&](const Entity &_entity,
        components::WorldPose *_worldPose) -> bool
    {
       ...
    }

updates the pose correctly. So perhaps there is something missing in the component data update in Link.cc?

Update

Inspecting the way that Physics.cc updates poses it appears that Link.cc is missing a call to _ecm.SetChanged:

    _ecm.SetChanged(this->dataPtr->id, components::EntityWrench::typeId, state);

And now it works!