Feature request: ability to updating link mass during runtime.

[JOHNI1]

I need change the link's mass during runtime. This was possible in gazebo classic through SetMass and UpdateMass functions but I can't find such ability in gz sim.

https://stackoverflow.com/questions/70864861/gazebo-how-can-i-change-model-weigth-during-runtime

https://answers.gazebosim.org/question/19201/

Additionally I tried to make a plugin for this purpose that updates the link's mass from ros topic but it doesn't work...

CMakeLists.txt:

cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)

project(LinkMassChanger)  #says that doesn't follow naming convention...I don't care
#WARNING: Package name "LinkMassChanger" does not follow the naming conventions. 
#It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(gz-plugin2 REQUIRED COMPONENTS register)
find_package(gz-sim8 REQUIRED)

# Add plugin library
add_library(${PROJECT_NAME} SHARED
  LinkMassChanger.cc
)

target_include_directories(${PROJECT_NAME} PRIVATE
  ${CMAKE_CURRENT_SOURCE_DIR}
)

ament_target_dependencies(${PROJECT_NAME}
  rclcpp
  std_msgs
  gz-plugin2
  gz-sim8
)

# Install the plugin
install(
  TARGETS ${PROJECT_NAME}
  LIBRARY DESTINATION lib
)

# Register the package
ament_package()

LinkMassChanger.hh:

#ifndef SYSTEM_PLUGIN_LINKMASSCHANGER_HH_
#define SYSTEM_PLUGIN_LINKMASSCHANGER_HH_

#include <gz/sim/System.hh>
#include <gz/sim/Entity.hh>
#include <gz/transport/Node.hh>

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float32.hpp>

#include <string>
#include <thread>
#include <atomic>
#include <mutex>

namespace link_mass_changer_plugin
{
  class LinkMassChanger : public gz::sim::System,
                          public gz::sim::ISystemConfigure,
                          public gz::sim::ISystemPreUpdate
  {
  public:
    LinkMassChanger();
    ~LinkMassChanger() override;

    /// \brief Called once when the plugin is loaded
    void Configure(const gz::sim::Entity &_entity,
                   const std::shared_ptr<const sdf::Element> &_sdf,
                   gz::sim::EntityComponentManager &_ecm,
                   gz::sim::EventManager &_eventMgr) override;

    /// \brief Called every simulation iteration (before physics)
    void PreUpdate(const gz::sim::UpdateInfo &_info,
                   gz::sim::EntityComponentManager &_ecm) override;

  private:
    /// \brief Callback invoked when a new mass message arrives from ROS 2
    void OnRosMass(const std_msgs::msg::Float32::SharedPtr _msg);

    /// \brief Thread that spins the ROS 2 node so subscriptions can be received
    void RosSpin();

    /// \brief Helper to update the mass of the target link
    bool UpdateLinkMass(gz::sim::EntityComponentManager &_ecm, float newMass);

  private:
    // -------------------------------------------------------
    // GZ Transport (if needed for additional communication)
    // -------------------------------------------------------
    gz::transport::Node gzNode;

    // -------------------------------------------------------
    // ROS 2 node, subscription, etc.
    // -------------------------------------------------------
    rclcpp::Node::SharedPtr rosNode;
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr massSub;
    std::thread rosSpinThread;
    std::atomic<bool> rosRunning{true};  // to stop the spin gracefully

    // -------------------------------------------------------
    // Data needed in the plugin
    // -------------------------------------------------------
    /// The world entity where the plugin is loaded
    gz::sim::Entity worldEntity{gz::sim::kNullEntity};

    /// The target link entity to modify mass
    gz::sim::Entity targetLinkEntity{gz::sim::kNullEntity};

    /// Flag indicating whether the link has been found
    bool linkFound{false};

    /// Name of the link to modify (read from SDF)
    std::string linkName;

    /// Mutex to protect shared data
    std::mutex dataMutex;

    /// Stores the new mass value
    float newMass{0.0f};
  };
}

#endif  // SYSTEM_PLUGIN_LINKMASSCHANGER_HH_

LinkMassChanger.cc:

  #include "LinkMassChanger.hh"
  
  #include <gz/plugin/Register.hh>
  #include <gz/sim/components/Name.hh>
  #include <gz/sim/components/Inertial.hh>
  #include <gz/sim/EntityComponentManager.hh>
  #include <gz/sim/World.hh>
  
  #include <gz/msgs/entity_factory.pb.h>
  #include <gz/msgs/boolean.pb.h>
  #include <gz/common/Console.hh>
  
  #include <std_msgs/msg/float32.hpp>
  
  // Register plugin with Gazebo
  GZ_ADD_PLUGIN(
    link_mass_changer_plugin::LinkMassChanger,
    gz::sim::System,
    link_mass_changer_plugin::LinkMassChanger::ISystemConfigure,
    link_mass_changer_plugin::LinkMassChanger::ISystemPreUpdate
  )
  
  using namespace link_mass_changer_plugin;
  
  // ///////////////////////////////////////////
  LinkMassChanger::LinkMassChanger()
  {
    gzdbg << "[LinkMassChanger] Plugin Loaded!" << std::endl;
  
    // Initialize the ROS 2 node
    if(!rclcpp::ok())
    {
      gzdbg << "[LinkMassChanger] Initializing ROS 2..." << std::endl;
      rclcpp::init(0, nullptr);
    }
    this->rosNode = rclcpp::Node::make_shared("link_mass_changer_plugin_node");
    gzdbg << "[LinkMassChanger] ROS 2 node created." << std::endl;
  
    // Spin the node in a separate thread so we don’t block
    this->rosSpinThread = std::thread(&LinkMassChanger::RosSpin, this);
    gzdbg << "[LinkMassChanger] ROS 2 spinning thread started." << std::endl;
  }
  
  // ///////////////////////////////////////////
  LinkMassChanger::~LinkMassChanger()
  {
    gzdbg << "[LinkMassChanger] Destructor called. Shutting down ROS 2 spin thread..." << std::endl;
  
    // Stop ROS spin
    this->rosRunning = false;
    if (this->rosSpinThread.joinable())
      this->rosSpinThread.join();
  
    // Shutdown ROS if desired (only if no other nodes are needed)
    if (rclcpp::ok())
    {
      rclcpp::shutdown();
      gzdbg << "[LinkMassChanger] ROS 2 shutdown completed." << std::endl;
    }
    else
    {
      gzdbg << "[LinkMassChanger] ROS 2 was already shutdown." << std::endl;
    }
  }
  
  // ///////////////////////////////////////////
  void LinkMassChanger::Configure(const gz::sim::Entity &_entity,
                                   const std::shared_ptr<const sdf::Element> &_sdf,
                                   gz::sim::EntityComponentManager & /*_ecm*/,
                                   gz::sim::EventManager & /*_eventMgr*/)
  {
    gzdbg << "[LinkMassChanger] Configure called." << std::endl;
  
    // Store the world entity
    this->worldEntity = _entity;
    gzdbg << "[LinkMassChanger] World entity stored: " << this->worldEntity << std::endl;
  
    // -----------------------------------------------------------
    // 1) Read the link name from the SDF parameter (if provided)
    // -----------------------------------------------------------
    if (_sdf && _sdf->HasElement("link"))
    {
      this->linkName = _sdf->Get<std::string>("link");
      gzdbg << "[LinkMassChanger] Link parameter set to: " << this->linkName << std::endl;
    }
    else
    {
      // Fallback link name
      this->linkName = "default_link_name";
      gzdbg << "[LinkMassChanger] Link parameter not found, defaulting to: " << this->linkName << std::endl;
    }
  
    // -----------------------------------------------------------
    // 2) Read the topic name from the SDF parameter (if provided)
    // -----------------------------------------------------------
    std::string topicName = "/link_mass";
    if (_sdf && _sdf->HasElement("topic"))
    {
      topicName = _sdf->Get<std::string>("topic");
      gzdbg << "[LinkMassChanger] Topic parameter set to: " << topicName << std::endl;
    }
    else
    {
      gzdbg << "[LinkMassChanger] Topic parameter not found, defaulting to: " << topicName << std::endl;
    }
  
    // -----------------------------------------------------------
    // 3) Subscribe to the ROS 2 topic (Float32)
    // -----------------------------------------------------------
    using namespace std::placeholders;
    this->massSub = this->rosNode->create_subscription<std_msgs::msg::Float32>(
        topicName,
        10,  // queue size
        std::bind(&LinkMassChanger::OnRosMass, this, _1));
  
    gzdbg << "[LinkMassChanger] Subscribed to ROS 2 topic " << topicName << "\n";
  }
  
  // ///////////////////////////////////////////
  void LinkMassChanger::OnRosMass(const std_msgs::msg::Float32::SharedPtr _msg)
  {
    std::lock_guard<std::mutex> lock(this->dataMutex);
    this->newMass = _msg->data;
    gzdbg << "[LinkMassChanger] Received new mass: " << this->newMass << " kg (ROS 2)" << std::endl;
  }
  
  // ///////////////////////////////////////////
  void LinkMassChanger::RosSpin()
  {
    gzdbg << "[LinkMassChanger] ROS 2 spin thread running." << std::endl;
    
    // rclcpp::spin() blocks, so we do a manual spin
    rclcpp::Rate rate(50); // 50 Hz spin
    while (rclcpp::ok() && this->rosRunning)
    {
      rclcpp::spin_some(this->rosNode);
      rate.sleep();
    }
  
    gzdbg << "[LinkMassChanger] ROS 2 spin thread exiting." << std::endl;
  }
  
  // ///////////////////////////////////////////
  void LinkMassChanger::PreUpdate(const gz::sim::UpdateInfo & /*_info*/,
                                   gz::sim::EntityComponentManager &_ecm)
  {
    // -------------------------------------------------------------------
    // A) In every PreUpdate, find the target link by name if not yet found
    // -------------------------------------------------------------------
    if (!this->linkFound)
    {
      gzdbg << "[LinkMassChanger] Searching for link [" << this->linkName << "]..." << std::endl;
      _ecm.Each<gz::sim::components::Name>(
        [&](const gz::sim::Entity &_entity,
            const gz::sim::components::Name *_name) -> bool
        {
          if (_name->Data() == this->linkName)
          {
            this->targetLinkEntity = _entity;
            this->linkFound = true;
            gzdbg << "[LinkMassChanger] Found link [" << this->linkName
                  << "] with entity ID: " << this->targetLinkEntity << std::endl;
            // Once found, break out of iteration
            return false;
          }
          return true;
        });
      
      if (!this->linkFound)
      {
        gzdbg << "[LinkMassChanger] Link [" << this->linkName << "] not found yet." << std::endl;
      }
    }
  
    // -------------------------------------------------------------------
    // B) If the link is found, check for new mass updates
    // -------------------------------------------------------------------
    if (this->linkFound && this->targetLinkEntity != gz::sim::kNullEntity)
    {
      float massToSet = 0.0f;
      {
        std::lock_guard<std::mutex> lock(this->dataMutex);
        massToSet = this->newMass;
        // Reset the mass after reading
        this->newMass = 0.0f;
      }
  
      if (massToSet > 0.0f)
      {
        gzdbg << "[LinkMassChanger] Attempting to update mass to " << massToSet << " kg for link [" << this->linkName << "]." << std::endl;
        if (this->UpdateLinkMass(_ecm, massToSet))
        {
          gzdbg << "[LinkMassChanger] Successfully updated mass of link [" << this->linkName
                << "] to " << massToSet << " kg." << std::endl;
        }
        else
        {
          gzerr << "[LinkMassChanger] Failed to update mass of link [" << this->linkName << "]." << std::endl;
        }
      }
    }
  }
  
  // ///////////////////////////////////////////
  bool LinkMassChanger::UpdateLinkMass(gz::sim::EntityComponentManager &_ecm, float newMass)
  {
      gzdbg << "[LinkMassChanger] Entering UpdateLinkMass method." << std::endl;
  
      // Access the Inertial component of the link
      auto inertialComp = _ecm.Component<gz::sim::components::Inertial>(this->targetLinkEntity);
      if (!inertialComp)
      {
          gzerr << "[LinkMassChanger] Inertial component not found for link [" << this->linkName << "]." << std::endl;
          return false;
      }
      gzdbg << "[LinkMassChanger] Inertial component accessed successfully." << std::endl;
  
      // Retrieve the current Inertial data (gz::math::Inertiald)
      gz::math::Inertiald newInertial = inertialComp->Data();
      gzdbg << "[LinkMassChanger] Current Inertial data retrieved." << std::endl;
  
      // Log current mass
      double currentMass = newInertial.MassMatrix().Mass();
      gzdbg << "[LinkMassChanger] Current mass: " << currentMass << " kg." << std::endl;
  
      // Check if the new mass is different
      if (static_cast<double>(newMass) == currentMass) {
          gzdbg << "[LinkMassChanger] New mass is the same as current mass. No update needed." << std::endl;
          return true;
      }
  
      // Calculate mass scale
      double massScale = (currentMass > 0.0) ? (static_cast<double>(newMass) / currentMass) : 1.0;
      gzdbg << "[LinkMassChanger] Mass scale factor: " << massScale << std::endl;
  
      // Create a mutable copy of MassMatrix3d
      gz::math::MassMatrix3d massMatrix = newInertial.MassMatrix();
      gzdbg << "[LinkMassChanger] Created mutable copy of MassMatrix3d." << std::endl;
  
      // Set the new mass
      massMatrix.SetMass(static_cast<double>(newMass));
      gzdbg << "[LinkMassChanger] New mass set to: " << massMatrix.Mass() << " kg." << std::endl;
  
      // Adjust the inertia tensor based on the new mass
      if (currentMass > 0.0)
      {
          gz::math::Vector3d diagonal = massMatrix.DiagonalMoments();
          gzdbg << "[LinkMassChanger] Original diagonal moments: (" << diagonal.X() << ", " << diagonal.Y() << ", " << diagonal.Z() << ")" << std::endl;
          diagonal *= massScale;
          massMatrix.SetDiagonalMoments(diagonal);
          gzdbg << "[LinkMassChanger] Inertia tensor scaled by factor: " << massScale << "." << std::endl;
          gzdbg << "[LinkMassChanger] New diagonal moments: (" << diagonal.X() << ", " << diagonal.Y() << ", " << diagonal.Z() << ")" << std::endl;
      }
      else
      {
          gzerr << "[LinkMassChanger] Current mass is zero or negative. Skipping inertia scaling." << std::endl;
      }
  
      // Update the MassMatrix3d in the Inertiald object
      newInertial.SetMassMatrix(massMatrix);
      gzdbg << "[LinkMassChanger] Inertial MassMatrix updated." << std::endl;
  
      // Update the Inertial component with the new data
      bool success = _ecm.SetComponentData<gz::sim::components::Inertial>(this->targetLinkEntity, newInertial);
  
      if (!success)
      {
          gzerr << "[LinkMassChanger] Failed to set new inertial data for link [" << this->linkName << "]." << std::endl;
      }
      else
      {
          gzdbg << "[LinkMassChanger] Successfully updated mass for link [" << this->linkName << "]." << std::endl;
      }
  
      return success;
  }

package.xml:

<?xml version="1.0"?>
<package format="3">
  <name>LinkMassChanger</name> <!--says that doesn't follow naming convention...I don't care-->
  <!-- WARNING: Package name "LinkMassChanger" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes. -->
  <version>0.0.0</version>
  <description>Ground Collision Gazebo Harmonic Plugin</description>

  <maintainer email="[email protected]">takahashi yoni</maintainer>
  <license>Apache 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>gazebo_dev</depend>
  <depend>gazebo_plugins</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>