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>

[azeey]

There is no API to do update the mass of a link at runtime. The pattern of using components to update values typically starts with creating a <Property>Cmd component and having one of the core systems like the Physics system be responsible for handling that component. For example, to change the force applied by a joint, there's a JointForceCmd (

gz-sim/include/gz/sim/components/JointForceCmd.hh

Line 1 in 1739bdb

/*

) component, which other systems can set to update the joint force (

gz-sim/src/systems/joint_controller/JointController.cc

Lines 327 to 338 in 1739bdb

	auto forceComp =
	_ecm.Component<components::JointForceCmd>(joint);
	if (forceComp == nullptr)
	{
	_ecm.CreateComponent(joint,
	components::JointForceCmd({force}));
	}
	else
	{
	*forceComp = components::JointForceCmd({force});
	}

), and the Physics system takes care of applying the force in the physics engine

gz-sim/src/systems/physics/Physics.cc

Lines 2317 to 2335 in 1739bdb

	auto velCmd = _ecm.Component<components::JointVelocityCmd>(_entity);
	if (force)
	{
	if (force->Data().size() != jointPhys->GetDegreesOfFreedom())
	{
	gzwarn << "There is a mismatch in the degrees of freedom between "
	<< "Joint [" << _name->Data() << "(Entity=" << _entity
	<< ")] and its JointForceCmd component. The joint has "
	<< jointPhys->GetDegreesOfFreedom() << " while the "
	<< " component has " << force->Data().size() << ".\n";
	}
	std::size_t nDofs = std::min(force->Data().size(),
	jointPhys->GetDegreesOfFreedom());
	for (std::size_t i = 0; i < nDofs; ++i)
	{
	jointPhys->SetForce(i, force->Data()[i]);
	}
	}

So, if you're interested in working on this, I'd recommend starting by creating InertialCmd component and updating the Physics system to apply the new inertial in the physics engine.

[JOHNI1]

Thank you for your response and guidance.

From what I understand, updating a link’s mass at runtime requires an InertialCmd component, similar to JointForceCmd. However, since the Physics system doesn’t process InertialCmd by default, I’d need to modify it to apply these updates to the physics engine. This is why changes to the Physics system are necessary, and I assume that's why this issue was moved to Core development. Essentially, inertial data remains static after being loaded, so modifying Physics is required to enable runtime updates.

I appreciate your insights and would love to hear your thoughts.

[JOHNI1]

This problem also reminds me when I tried to move a model's position that I set to be static in sdf, as I changing its position, the changes did reflect in the gui (even when I set to show the collider of the model) but the actual collider that was registered to the physics system did not update the position and the the invisible static object stayed in the same place and other models would collide with it. So I guess the same solution is needed for this problem too, to make a Cmd that updates the Component values in Physics System.

[azeey]

Thank you for your response and guidance.

From what I understand, updating a link’s mass at runtime requires an InertialCmd component, similar to JointForceCmd. However, since the Physics system doesn’t process InertialCmd by default, I’d need to modify it to apply these updates to the physics engine. This is why changes to the Physics system are necessary, and I assume that's why this issue was moved to Core development. Essentially, inertial data remains static after being loaded, so modifying Physics is required to enable runtime updates.

I appreciate your insights and would love to hear your thoughts.

Yes, that is all correct. I've thought about this some more, and I believe we'll also need to add an API in gz-physics to allow setting link inertias similar to gazebosim/gz-physics#462.

This problem also reminds me when I tried to move a model's position that I set to be static in sdf, as I changing its position, the changes did reflect in the gui (even when I set to show the collider of the model) but the actual collider that was registered to the physics system did not update the position and the the invisible static object stayed in the same place and other models would collide with it. So I guess the same solution is needed for this problem too, to make a Cmd that updates the Component values in Physics System.

You'll need to use the WorldPoseCmd component instead

gz-sim/include/gz/sim/components/PoseCmd.hh

Lines 36 to 40 in 0e02d36

	/// \brief A component type that contains commanded pose of an
	/// entity in the world frame represented by gz::math::Pose3d.
	using WorldPoseCmd = Component<math::Pose3d, class WorldPoseCmdTag>;
	GZ_SIM_REGISTER_COMPONENT("gz_sim_components.WorldPoseCmd",
	WorldPoseCmd)