How to Integrate Force/Torque Sensors with DART's Simulation World: A Complete Guide

To integrate force/torque sensors with DART's simulation world, derive a custom class from dart::sensor::Sensor, override updateImpl to query BodyNode::getExternalForce() and getExternalTorque(), attach the sensor to a body via setParentFrame, and register it with World::addSensor to receive automatic updates each simulation step.

DART (Dynamic Animation and Robotics Toolkit) provides a flexible, extensible sensor architecture through the dartsim/dart repository. While the library does not ship with a concrete force/torque sensor implementation, its generic dart::sensor::Sensor base class allows you to build custom sensors that read external wrenches acting on any body node. This guide walks through the complete architecture, implementation details, and working code required to add realistic FT sensing to your DART simulation.

Architecture Overview

DART’s sensor system relies on three core components working together during the simulation loop.

Core Components

Component	Role	Source File
dart::sensor::Sensor	Abstract base defining the sensor lifecycle (construction, update, reset, enable/disable).	dart/sensor/sensor.hpp
dart::sensor::SensorManager	Maintains the sensor registry attached to a World, handles naming, and triggers updates.	dart/sensor/sensor_manager.hpp
dart::simulation::World	Provides the public API addSensor, removeSensor, and forwards timing context to the manager.	dart/simulation/world.hpp

Sensor Lifecycle

1. Construction – You instantiate your sensor, optionally setting an update rate, name, and parent frame.
2. Registration – World::addSensor forwards the sensor to SensorManager, which validates the unique name.
3. World Step – Each call to World::step() triggers SensorManager::updateSensors.
4. Update – The manager builds a SensorUpdateContext (containing simulation time, time step, and frame number) and invokes Sensor::update, which delegates to your updateImpl override.
5. Data Access – Your implementation queries the BodyNode for external forces and stores the measurement for later retrieval.

Implementing a Force/Torque Sensor

Because DART does not provide a built-in FT sensor, you must derive from dart::sensor::Sensor and implement the measurement logic yourself.

Header Implementation (ForceTorqueSensor.hpp)

#pragma once

#include <dart/sensor/sensor.hpp>
#include <dart/dynamics/BodyNode.hpp>
#include <Eigen/Geometry>

class ForceTorqueSensor final : public dart::sensor::Sensor
{
public:
  struct Properties : public dart::sensor::Sensor::Properties
  {
    // Optional: sensor frame offset relative to the body node
    Eigen::Isometry3d offset{Eigen::Isometry3d::Identity()};
  };

  explicit ForceTorqueSensor(
      const Properties& props = Properties(),
      dart::dynamics::BodyNode* body = nullptr)
    : Sensor(props), mBodyNode(body)
  {
    // Attach the sensor to the body node (parent frame)
    if (body) setParentFrame(body);
  }

  /// Retrieve the last measured force expressed in the sensor frame.
  Eigen::Vector3d getForce() const { return mForce; }

  /// Retrieve the last measured torque expressed in the sensor frame.
  Eigen::Vector3d getTorque() const { return mTorque; }

protected:
  // Called by the world each simulation step
  void updateImpl(const dart::simulation::World& /*world*/,
                  const dart::sensor::SensorUpdateContext& /*ctx*/) override
  {
    if (!mBodyNode) return;

    // 1️⃣ Get the external force/torque applied to the body (world frame)
    const Eigen::Vector3d fWorld = mBodyNode->getExternalForce();
    const Eigen::Vector3d tWorld = mBodyNode->getExternalTorque();

    // 2️⃣ Transform to the sensor frame (parent frame + optional offset)
    Eigen::Isometry3d X = getWorldTransform();  // world → sensor
    mForce  = X.linear().transpose() * fWorld;
    mTorque = X.linear().transpose() * tWorld;
  }

  // Optional: clear stored measurement when the sensor is reset
  void resetImpl() override
  {
    mForce.setZero();
    mTorque.setZero();
  }

private:
  dart::dynamics::BodyNode* mBodyNode{nullptr};
  Eigen::Vector3d mForce{Eigen::Vector3d::Zero()};
  Eigen::Vector3d mTorque{Eigen::Vector3d::Zero()};
};

Critical implementation details:

• Inheritance: By deriving from dart::sensor::Sensor, you automatically gain access to naming, enable/disable toggles, and transform management via setParentFrame.
• Data source: BodyNode::getExternalForce() and getExternalTorque() return the total external wrench (gravity, contacts, joint reactions, user-applied forces) acting on the body.
• Frame transformation: getWorldTransform() returns the world-to-sensor transform, allowing you to rotate the world-frame wrench into the sensor frame using the transpose of the rotation matrix.

Registering the Sensor with the World

#include <dart/dart.hpp>
#include "ForceTorqueSensor.hpp"

int main()
{
  // 1️⃣ Create a world and load a robot (any URDF/SDF works)
  auto world = dart::simulation::World::create();
  auto skel  = dart::loadSkeletonDart("path/to/robot.urdf");
  world->addSkeleton(skel);

  // 2️⃣ Choose the body node that hosts the FT sensor (e.g., wrist link)
  auto* wrist = skel->getBodyNode("wrist_link");

  // 3️⃣ Create the sensor and attach it
  ForceTorqueSensor::Properties props;
  props.name = "wrist_ft";
  props.updateRate = 1000.0;           // Hz (0 ⇒ every simulation step)
  props.relativeTransform = Eigen::Isometry3d::Identity(); // sensor frame = body frame
  auto ftSensor = std::make_shared<ForceTorqueSensor>(props, wrist);

  // 4️⃣ Register the sensor with the world
  world->addSensor(ftSensor);

  // 5️⃣ Run the simulation and read measurements
  for (int i = 0; i < 1000; ++i)
  {
    world->step();   // triggers ftSensor->updateImpl(...)
    if (ftSensor->isEnabled())
    {
      Eigen::Vector3d f = ftSensor->getForce();
      Eigen::Vector3d t = ftSensor->getTorque();
      std::cout << "Force: " << f.transpose()
                << "  Torque: " << t.transpose() << '\n';
    }
  }
}

Integration flow:

• world->addSensor delegates to SensorManager, which validates unique naming and stores the shared pointer.
• world->step triggers SensorManager::updateSensors, constructing a SensorUpdateContext containing the current simulation time, timestep, and frame number.
• Your updateImpl override queries the BodyNode state and stores the measurement, which you can retrieve via your custom accessors after the step completes.

Key Source Files for Sensor Integration

Understanding these files is essential for debugging or extending your force/torque sensor implementation.

File	Purpose	Location
dart/sensor/sensor.hpp	Defines the abstract Sensor base class, SensorUpdateContext, and transform management APIs like setParentFrame and getWorldTransform.	sensor.hpp
dart/sensor/sensor_manager.hpp	Implements the SensorManager class that owns all sensors, handles name uniqueness, and orchestrates the updateSensors call during world steps.	sensor_manager.hpp
dart/simulation/world.hpp	Provides the public interface addSensor, removeSensor, and getSensor, and forwards the simulation context to the sensor manager.	world.hpp
tests/integration/simulation/test_sensors.cpp	Contains a minimal working example (CountingSensor) demonstrating the sensor lifecycle and update flow; useful as a reference implementation.	test_sensors.cpp

Common Integration Pitfalls

Symptom	Root Cause	Solution
Sensor never updates	Sensor added after simulation start without re-registration, or setEnabled(false) was invoked.	Ensure world->addSensor is called before the first world->step, and verify sensor->isEnabled() returns true.
Zero force/torque readings	The BodyNode pointer is null, or the sensor’s parent frame is not attached to the body.	Pass a valid BodyNode* to the constructor and confirm setParentFrame(bodyNode) was called.
Measurements in wrong coordinate frame	Ignoring the sensor’s relative transform offset when rotating the world-frame wrench.	Apply the rotation from getWorldTransform() (world-to-sensor) to transform forces into the sensor frame.
Simulation slowdown	Setting updateRate higher than the simulation frequency causes unnecessary processing overhead.	Set updateRate to 0 (update every step) or keep it ≤ 1.0 / world->getTimeStep().

Extending the Sensor for Production Use

Once your basic force/torque sensor is functional, consider these enhancements for realistic robotics applications:

• Noise injection – Add Gaussian noise to mForce and mTorque inside updateImpl to simulate sensor noise characteristics.
• Calibration bias – Store a bias vector computed during a tare operation and subtract it from measurements in updateImpl.
• Data streaming – Expose a callback mechanism or integrate with ROS by publishing geometry_msgs/WrenchStamped messages from within your sensor class.
• Multiple contact points – If you need per-contact wrenches rather than the net external wrench, iterate over BodyNode's contact constraints directly instead of using getExternalForce().

Summary

• DART does not include a built-in force/torque sensor class, but provides the dart::sensor::Sensor abstraction for custom implementations.
• Derive your sensor from dart::sensor::Sensor and override updateImpl to query BodyNode::getExternalForce() and getExternalTorque().
• Attach the sensor to a body using setParentFrame and register it with the world via World::addSensor to receive automatic updates.
• Transform measurements from world frame to sensor frame using getWorldTransform() to obtain realistic force/torque readings.
• Reference the test suite in tests/integration/simulation/test_sensors.cpp for a minimal working example of the sensor lifecycle.

Frequently Asked Questions

Does DART provide a built-in force/torque sensor class?

No, DART does not ship with a concrete force/torque sensor implementation. Instead, the library provides the abstract dart::sensor::Sensor base class in dart/sensor/sensor.hpp that you must subclass. By overriding the updateImpl method to query BodyNode::getExternalForce() and getExternalTorque(), you can create a fully functional FT sensor that integrates seamlessly with the simulation loop.

Why are my force/torque sensor readings always zero?

Zero readings typically indicate that the sensor is not properly attached to a body node or the body pointer is null. Ensure you pass a valid BodyNode* to your sensor constructor and call setParentFrame(bodyNode) to establish the transform relationship. Additionally, verify that the sensor is enabled by calling sensor->setEnabled(true) before stepping the world, as disabled sensors skip their update cycle.

How do I convert force measurements to the sensor's local coordinate frame?

The BodyNode methods getExternalForce() and getExternalTorque() return values expressed in the world frame. To obtain sensor-frame measurements, retrieve the world-to-sensor transform using getWorldTransform() inside your updateImpl method, then rotate the vectors using the transform's linear part (rotation matrix). Specifically, multiply the world-frame force by getWorldTransform().linear().transpose() to express it in the sensor frame.

Can I control how often my force/torque sensor updates?

Yes, you can set the sensor's update rate via the `Properties

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}