ROS 2 Payload Mass and Center-of-Mass Estimation

A ROS 2 package for identifying the static characteristics of a tool mounted on a six-axis force/torque sensor.

The package estimates:

• payload mass;
• payload center of mass relative to the F/T sensor origin;
• constant force-sensor bias;
• constant torque-sensor bias.

The identified parameters can then be used to remove the payload weight and the gravity-induced moment from the measured wrench.

Features

• Interactive collection of measurements from multiple static robot poses.
• Linear least-squares estimation of mass, center of mass, and sensor biases.
• YAML export of the identified parameters.
• ROS 2 validation node for checking the result at independent poses.
• Synthetic unit test for the identification mathematics.
• Generic C++ example showing how to compensate a measured wrench.

Repository structure

ros2_ft_payload_identification/
├── README.md
├── LICENSE
├── cpp_example/
│   ├── README.md
│   └── gravity_compensation_example.cpp
└── ros2_ws/
    └── src/
        └── ft_payload_estimator/
            ├── config/
            │   ├── estimator.yaml
            │   └── validator.yaml
            ├── ft_payload_estimator/
            │   ├── mass_com_estimator.py
            │   ├── payload_model.py
            │   └── validate_payload_estimate.py
            ├── test/
            │   └── test_payload_math.py
            ├── package.xml
            ├── setup.cfg
            └── setup.py

Measurement model

For every static robot pose, gravity is expressed in the F/T sensor frame as

g_sensor = R_sensor_to_baseᵀ g_base

The static wrench measured by the sensor is modeled as

f_measured   = m g_sensor + b_f
tau_measured = r × (m g_sensor) + b_tau

where:

• m is the payload mass;
• r is the vector from the F/T sensor origin to the payload center of mass;
• b_f is the constant force bias;
• b_tau is the constant torque bias.

All vectors in these equations are expressed in the F/T sensor frame.

The torque equation is linear in the first moment

h = m r

rather than directly in m and r. The estimator therefore solves for h and recovers the center of mass using

r = h / m

after the least-squares solution.

Important sensor-zeroing requirement

Before collecting calibration data, zero or tare the F/T sensor with:

• the complete tool already mounted;
• the robot stationary;
• no external contact;
• no changing cable force;
• the end-effector local z axis perpendicular to the horizontal ground plane.

In this reference pose, the end-effector z axis is aligned with the gravity direction, subject to the sign convention of the robot frame.

This is the sensor-zeroing reference pose, not the only pose used for identification. After zeroing the sensor, the calibration procedure must still use several different roll and pitch orientations.

Do not zero the sensor again in another orientation after identification. A new tare changes the bias convention, so the calibration should be repeated.

Requirements

The package requires ROS 2 and Python 3 together with NumPy, SciPy, PyYAML, and pytest.

On Ubuntu, the additional Python dependencies can normally be installed with:

sudo apt update
sudo apt install \
  python3-numpy \
  python3-scipy \
  python3-yaml \
  python3-pytest

Before publishing the repository, replace replace-me@example.com in package.xml and setup.py with the maintainer email that you want to expose.

ROS 2 inputs

The default configuration subscribes to the following topics.

Force/torque measurements

Topic:
  /bus0/ft_sensor0/ft_sensor_readings/wrench

Type:
  geometry_msgs/msg/WrenchStamped

Sensor orientation

Topic:
  /gravity_compensation/data_impedance

Type:
  std_msgs/msg/Float64MultiArray

The orientation array must contain:

[qx, qy, qz, qw]

The quaternion must represent the orientation of the F/T sensor frame in the robot base frame. The estimator uses its transpose, or inverse rotation, to express gravity in the sensor frame.

When the available pose corresponds to a flange or tool frame that is not aligned with the F/T sensor frame, apply the fixed transform between those frames before using the orientation.

Build

Clone the repository and build the ROS 2 workspace:

git clone <repository-url>
cd ros2_ft_payload_identification/ros2_ws

colcon build --symlink-install
source install/setup.bash

Replace <repository-url> with the GitHub URL after publishing the repository.

Configure the estimator

Edit:

ros2_ws/src/ft_payload_estimator/config/estimator.yaml

Default configuration:

mass_com_estimator:
  ros__parameters:
    wrench_topic: /bus0/ft_sensor0/ft_sensor_readings/wrench
    orientation_topic: /gravity_compensation/data_impedance
    number_of_poses: 7
    samples_per_pose: 2000
    gravity_magnitude: 9.80665
    output_file: payload_estimate.yaml

Use at least six or seven well-separated orientations when possible. Three poses are only the mathematical minimum and can produce a poorly conditioned solution.

Run the calibration

Because the estimator asks the user to press Enter at each pose, run it directly:

cd ros2_ft_payload_identification/ros2_ws
source install/setup.bash

ros2 run ft_payload_estimator estimate_payload --ros-args \
  --params-file src/ft_payload_estimator/config/estimator.yaml

Calibration procedure

1. Mount the complete tool that will be used during operation.
2. Place the end-effector z axis perpendicular to the ground plane.
3. Ensure that the tool is not touching the environment.
4. Zero or tare the F/T sensor.
5. Start the estimator.
6. For each requested pose, move the robot to a new static orientation.
7. Wait until the robot, tool, and cables have settled.
8. Press Enter and keep the robot stationary during sample collection.
9. Repeat using substantially different roll and pitch angles.

Avoid collecting poses that differ only by rotation around the gravity axis. Such poses do not sufficiently change the gravity direction in the sensor frame.

Calibration output

The estimator writes a YAML file such as:

mass_kg: 0.135617
com_sensor_m: [0.012, -0.018, 0.047]
force_bias_N: [0.08, -0.04, 0.12]
torque_bias_Nm: [0.002, -0.003, 0.001]
diagnostics:
  rank: 10
  required_rank: 10
  condition_number: 12.3
  force_rmse_N: 0.02
  torque_rmse_Nm: 0.0008

The values above only illustrate the file format. Use the values produced by the calibration performed on your robot.

The output fields are:

Field	Unit	Description
mass_kg	kg	Identified payload mass
com_sensor_m	m	Sensor-origin-to-CoM vector in the sensor frame
force_bias_N	N	Constant force bias
torque_bias_Nm	N m	Constant torque bias
rank	—	Rank of the least-squares design matrix
condition_number	—	Numerical conditioning of the identification
force_rmse_N	N	Force-model fitting error
torque_rmse_Nm	N m	Torque-model fitting error

A valid pose set should normally produce a design-matrix rank of 10.

Validate the estimated parameters

Validation should use static orientations that were not used during identification.

Edit:

ros2_ws/src/ft_payload_estimator/config/validator.yaml

Set calibration_file to the generated YAML file, then run:

cd ros2_ft_payload_identification/ros2_ws
source install/setup.bash

ros2 run ft_payload_estimator validate_payload --ros-args \
  --params-file src/ft_payload_estimator/config/validator.yaml

The validation node computes:

force_residual =
    force_measured
    - mass * gravity_sensor
    - force_bias

torque_residual =
    torque_measured
    - com_sensor × (mass * gravity_sensor)
    - torque_bias

For a stationary tool with no external contact, a correct calibration should produce:

• small residual forces;
• small residual torques;
• no systematic residual change with roll or pitch.

Residual torque is particularly important for validating the center of mass. An orientation-dependent residual torque usually indicates an incorrect CoM, frame convention, quaternion direction, wrench sign, or tare convention.

The default validation thresholds are examples. Adapt them to the sensor noise and accuracy required by the application.

Use the calibration results in C++

The four calibration outputs can be inserted into any wrench-processing callback.

The example below assumes:

• the incoming wrench is expressed in the F/T sensor frame;
• current_pose.pose.orientation gives the sensor-frame orientation in the robot base frame;
• compensated_wrench is ordered as [Fx, Fy, Fz, Tx, Ty, Tz];
• the sensor was zeroed using the reference orientation described above.

#include <array>
#include <cmath>

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
#include <kdl/frames.hpp>

geometry_msgs::msg::PoseStamped current_pose;
std::array<double, 6> compensated_wrench{};

void wrenchCallback(
  const geometry_msgs::msg::WrenchStamped::SharedPtr wrench)
{
  // Replace these example values with payload_estimate.yaml.
  const double payload_mass = 0.135617;  // mass_kg [kg]

  // Sensor origin -> payload CoM, expressed in sensor coordinates [m].
  const KDL::Vector payload_com_sensor(0.012, -0.018, 0.047);

  // Identified constant biases.
  const KDL::Vector force_bias(0.08, -0.04, 0.12);       // [N]
  const KDL::Vector torque_bias(0.002, -0.003, 0.001);   // [N m]

  // Copy the raw wrench.
  compensated_wrench[0] = wrench->wrench.force.x;
  compensated_wrench[1] = wrench->wrench.force.y;
  compensated_wrench[2] = wrench->wrench.force.z;
  compensated_wrench[3] = wrench->wrench.torque.x;
  compensated_wrench[4] = wrench->wrench.torque.y;
  compensated_wrench[5] = wrench->wrench.torque.z;

  // Validate and normalize the orientation quaternion.
  const auto & q_msg = current_pose.pose.orientation;

  const bool quaternion_is_finite =
    std::isfinite(q_msg.x) &&
    std::isfinite(q_msg.y) &&
    std::isfinite(q_msg.z) &&
    std::isfinite(q_msg.w);

  const double quaternion_norm_squared =
    q_msg.x * q_msg.x +
    q_msg.y * q_msg.y +
    q_msg.z * q_msg.z +
    q_msg.w * q_msg.w;

  if (!quaternion_is_finite || quaternion_norm_squared < 1.0e-12) {
    return;
  }

  const double inverse_norm = 1.0 / std::sqrt(quaternion_norm_squared);

  const KDL::Rotation rotation_sensor_to_base =
    KDL::Rotation::Quaternion(
      q_msg.x * inverse_norm,
      q_msg.y * inverse_norm,
      q_msg.z * inverse_norm,
      q_msg.w * inverse_norm);

  // Express gravity in the F/T sensor frame.
  const KDL::Vector gravity_base(0.0, 0.0, -9.80665);
  const KDL::Vector gravity_sensor =
    rotation_sensor_to_base.Inverse() * gravity_base;

  const KDL::Vector gravity_force =
    payload_mass * gravity_sensor;

  // Gravity-induced torque around the sensor origin:
  // tau_g = r_sensor_to_com x F_g.
  const KDL::Vector gravity_torque(
    payload_com_sensor.y() * gravity_force.z() -
      payload_com_sensor.z() * gravity_force.y(),
    payload_com_sensor.z() * gravity_force.x() -
      payload_com_sensor.x() * gravity_force.z(),
    payload_com_sensor.x() * gravity_force.y() -
      payload_com_sensor.y() * gravity_force.x());

  // Remove the identified payload wrench and sensor biases.
  compensated_wrench[0] -= gravity_force.x() + force_bias.x();
  compensated_wrench[1] -= gravity_force.y() + force_bias.y();
  compensated_wrench[2] -= gravity_force.z() + force_bias.z();

  compensated_wrench[3] -= gravity_torque.x() + torque_bias.x();
  compensated_wrench[4] -= gravity_torque.y() + torque_bias.y();
  compensated_wrench[5] -= gravity_torque.z() + torque_bias.z();
}

After compensation:

compensated_wrench ≈ external_contact_wrench

for static or quasi-static operation.

Why the center of mass is required

The payload mass determines the gravitational force:

f_gravity = m g_sensor

When the center of mass is displaced from the sensor origin, this force also produces a moment:

tau_gravity = r × f_gravity

Compensating only the force leaves an orientation-dependent torque in the measurement. The estimated center of mass is therefore required for full six-axis compensation.

Using ROS 2 parameters

For a reusable implementation, load the values as ROS 2 parameters instead of hard-coding them:

this->declare_parameter<double>("payload.mass", 0.0);

this->declare_parameter<std::vector<double>>(
  "payload.com_sensor", {0.0, 0.0, 0.0});

this->declare_parameter<std::vector<double>>(
  "payload.force_bias", {0.0, 0.0, 0.0});

this->declare_parameter<std::vector<double>>(
  "payload.torque_bias", {0.0, 0.0, 0.0});

Example parameter file:

wrench_processing:
  ros__parameters:
    payload:
      mass: 0.135617
      com_sensor: [0.012, -0.018, 0.047]
      force_bias: [0.08, -0.04, 0.12]
      torque_bias: [0.002, -0.003, 0.001]

Run the mathematical unit test

From the ROS 2 workspace:

cd ros2_ft_payload_identification/ros2_ws

colcon test --packages-select ft_payload_estimator
colcon test-result --verbose

The test creates synthetic measurements for a known mass, CoM, and bias, estimates the parameters, and verifies that the compensated wrench is zero.

It can also be run directly:

cd ros2_ft_payload_identification/ros2_ws
pytest -q src/ft_payload_estimator/test/test_payload_math.py

Troubleshooting

Estimated mass is negative

Check:

• wrench sign convention;
• quaternion direction;
• gravity direction in the base frame;
• whether the wrench and orientation use compatible frames.

Design-matrix rank is below 10

The poses do not sufficiently excite all model parameters. Add more diverse roll and pitch orientations.

Residual torque changes with orientation

Check:

• CoM sign;
• sensor-frame axis convention;
• fixed transform between the sensor and reported pose frame;
• cable forces;
• accidental contact;
• whether the sensor was re-zeroed after calibration.

Python validation works but the C++ result does not

The most likely cause is a mismatch between the frame, sign, quaternion, or tare convention used in Python and C++.

Assumptions and limitations

• The tool is rigidly mounted.
• The payload does not change during calibration or use.
• Measurements are collected while the robot is stationary.
• External contact is absent during calibration and validation.
• Cable forces remain small and approximately constant.
• The bias is treated as constant.
• Dynamic inertial forces and moments are not modeled.
• The compensation is intended primarily for static or quasi-static operation.

For dynamic motions, acceleration-dependent inertial terms should also be modeled.

License

This project is released under the MIT License. See LICENSE.