Pull request #282: wip: changes for the mobile robot urdf getter

Merge in MOCTRL/libfranka from PRCUN-5824-mobile-model-urdf to main

* commit '858daa4d486250ba33dc2a9c2549a97762acce66':
  remove the example
  update changelog
  Adds URDF robot type support
  Update test utilities and fix existing tests
  Add mobile robot example and test data
  Implement MobileModel class for mobile robot kinematics
  Update Model class to reject mobile robot URDFs
  Add mobile robot detection to Robot class
  Add URDF robot type detection utility

Author: BarisYazici

CHANGELOG.md

@@ -2,6 +2,12 @@
 
 All notable changes to libfranka in this file.
 
+## Unreleased
+
+- Add `franka::MobileModel` class for mobile robot forward kinematics (swerve drive module poses via Pinocchio).
+- `franka::Model` now rejects mobile robot URDFs with a `ModelException`; use `franka::MobileModel` instead.
+- `Robot::loadModel()` throws `InvalidOperationException` for mobile robots; use `Robot::getRobotModel()` with `MobileModel` directly.
+
 ## [0.20.5]
 ### libfranka - C++
 - fix: Sometimes the PTP motion could get stuck in 'idle' after reaching the goal while doing async PTP motions

CMakeLists.txt

@@ -109,6 +109,8 @@ add_library(franka SHARED
   src/vacuum_gripper.cpp
   src/vacuum_gripper_state.cpp
   src/robot_model.cpp
+  src/urdf_robot_type.cpp
+  src/mobile_model.cpp
 
   src/async_control/async_position_control_handler.cpp
 

include/franka/mobile_model.h

@@ -0,0 +1,113 @@
+// Copyright (c) 2026 Franka Robotics GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
+
+#include <array>
+#include <cstdint>
+#include <memory>
+#include <string>
+
+/**
+ * @file mobile_model.h
+ * Contains the franka::MobileModel type for mobile robot kinematics.
+ */
+
+namespace franka {
+
+/**
+ * Enumerates the frames of a mobile robot's drive modules.
+ */
+enum class MobileFrame : std::uint8_t {
+  kFrontDriveModule,  ///< Front swerve drive module frame
+  kRearDriveModule    ///< Rear swerve drive module frame
+};
+
+/// Number of swerve drive modules.
+inline constexpr size_t kNumDriveModules = 2;
+
+/// Number of joints per drive module (steering + drive).
+inline constexpr size_t kJointsPerDriveModule = 2;
+
+/**
+ * Joint positions for all controllable subsystems of a mobile robot.
+ *
+ * All arrays are default-initialized to zero (neutral configuration).
+ */
+struct MobileJointPositions {
+  /// Drive module joint positions: {front_steering, front_drive, rear_steering, rear_drive}.
+  std::array<double, kNumDriveModules * kJointsPerDriveModule> drive_modules{};
+};
+
+/**
+ * Calculates poses for mobile robot frames using Pinocchio.
+ *
+ * This class provides forward kinematics for mobile robots. It is constructed from a mobile robot
+ * URDF and will reject arm robot URDFs.
+ *
+ * @note The pose() method is RT-safe (zero heap allocation). Passive joints (casters, rocker arm)
+ * are held at their neutral configuration internally.
+ */
+class MobileModel final {
+ public:
+  /// Number of swerve drive modules.
+  static constexpr size_t kNumModules = kNumDriveModules;
+
+  /// Number of joints per drive module (steering + drive).
+  static constexpr size_t kJointsPerModule = kJointsPerDriveModule;
+
+  /**
+   * Constructs a MobileModel from a URDF string.
+   *
+   * @param[in] urdf_model The URDF model string describing a mobile robot.
+   *
+   * @throw ModelException if the URDF describes an arm robot rather than a mobile robot.
+   */
+  explicit MobileModel(const std::string& urdf_model);
+
+  /**
+   * Move-constructs a new MobileModel instance.
+   *
+   * @param[in] other Other MobileModel instance.
+   */
+  MobileModel(MobileModel&& other) noexcept;
+
+  /**
+   * Move-assigns this MobileModel from another MobileModel instance.
+   *
+   * @param[in] other Other MobileModel instance.
+   *
+   * @return MobileModel instance.
+   */
+  MobileModel& operator=(MobileModel&& other) noexcept;
+
+  /**
+   * Destructor.
+   */
+  ~MobileModel() noexcept;
+
+  /// @cond DO_NOT_DOCUMENT
+  MobileModel(const MobileModel&) = delete;
+  MobileModel& operator=(const MobileModel&) = delete;
+  /// @endcond
+
+  /**
+   * Gets the 4x4 pose matrix for the given mobile frame relative to the robot's base frame
+   * (URDF root link, body-fixed).
+   *
+   * The pose is represented as a 4x4 matrix in column-major format.
+   * This method is RT-safe: it performs zero heap allocation.
+   *
+   * @param[in] frame The desired drive module frame (kFrontDriveModule or kRearDriveModule).
+   * @param[in] joint_positions Joint positions for all controllable subsystems. Angles are in
+   * radians; the conversion to the internal Pinocchio representation is handled automatically.
+   *
+   * @return Vectorized 4x4 pose matrix, column-major.
+   */
+  std::array<double, 16> pose(MobileFrame frame, const MobileJointPositions& joint_positions) const;
+
+ private:
+  class Data;
+  std::unique_ptr<Data> data_;
+};
+
+}  // namespace franka

include/franka/robot.h

@@ -778,12 +778,23 @@ class Robot {
    * @}
    */
 
+  /**
+   * Returns whether the connected robot is a mobile robot (TMR).
+   *
+   * This is detected at connection time from the robot's URDF.
+   *
+   * @return true if the robot is a mobile robot, false if it is an arm robot.
+   */
+  bool isMobileRobot() const noexcept;
+
   /**
    * Loads the model library from the robot.
    *
    * @return Model instance.
    *
    * @throw ModelException if the model library cannot be loaded.
+   * @throw InvalidOperationException if called on a mobile robot. Use
+   * franka::MobileModel with the URDF from getRobotModel() instead.
    * @throw NetworkException if the connection is lost, e.g. after a timeout.
    */
   Model loadModel();

src/mobile_model.cpp

@@ -0,0 +1,178 @@
+// Copyright (c) 2026 Franka Robotics GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka/exception.h>
+#include <franka/mobile_model.h>
+
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+
+// Ignore warnings from Pinocchio includes
+#if defined(__GNUC__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+#if defined(__GNUC__)
+#pragma GCC diagnostic pop
+#endif
+
+#include <Eigen/Core>
+
+#include "urdf_robot_type.h"
+
+namespace franka {
+
+class MobileModel::Data {
+ public:
+  static constexpr size_t kNumDriveJoints = kNumModules * kJointsPerModule;
+
+  // Upper bound on Pinocchio configuration vector dimension (nq) for any supported mobile robot
+  // URDF. Continuous joints use 2 config elements (cos, sin); revolute joints use 1. The TMR
+  // URDF has ~15 joints, so nq is well below this limit. Using a fixed-max-size Eigen vector
+  // keeps the storage inline and guarantees zero heap allocation in the RT hot path (pose()).
+  static constexpr int kMaxConfigDim = 64;
+  using ConfigVector = Eigen::Matrix<double, Eigen::Dynamic, 1, 0, kMaxConfigDim, 1>;
+
+  pinocchio::Model model;
+  mutable pinocchio::Data data;
+
+  // Pinocchio joint indices for the drive module frames, looked up by name at construction.
+  std::array<pinocchio::JointIndex, MobileModel::kNumModules> module_joint_indices{};
+
+  // Mapping from drive joint index to Pinocchio config index for the 4 drive module joints.
+  struct JointMapping {
+    int idx_q;           // Index into the Pinocchio configuration vector
+    bool is_continuous;  // true if nq==2 (cos/sin pair), false if nq==1
+  };
+  std::array<JointMapping, kNumDriveJoints> joint_mappings{};
+
+  // Pre-allocated configuration buffer (neutral config for passive joints, updated for active).
+  // Uses fixed-max-size Eigen vector: storage is inline, zero heap allocation at runtime.
+  mutable ConfigVector q_buffer;
+
+  explicit Data(const std::string& urdf_model) {
+    if (!isMobileRobotUrdf(urdf_model)) {
+      throw ModelException(
+          "libfranka: Cannot create MobileModel: the provided URDF does not describe "
+          "a mobile robot (robot name must start with 'tmr').");
+    }
+
+    pinocchio::urdf::buildModelFromXML(urdf_model, model);
+    data = pinocchio::Data(model);
+
+    if (model.nq > kMaxConfigDim) {
+      throw ModelException("libfranka: MobileModel: URDF configuration dimension (" +
+                           std::to_string(model.nq) + ") exceeds maximum (" +
+                           std::to_string(kMaxConfigDim) + ").");
+    }
+
+    // Initialize q_buffer with neutral configuration.
+    // Revolute joints: 0.0, Continuous joints: (cos(0)=1, sin(0)=0).
+    q_buffer = ConfigVector::Zero(model.nq);
+    for (int j = 1; j < model.njoints; ++j) {
+      if (model.joints[j].nv() == 1 && model.joints[j].nq() == 2) {
+        q_buffer[model.joints[j].idx_q()] = 1.0;      // cos(0)
+        q_buffer[model.joints[j].idx_q() + 1] = 0.0;  // sin(0)
+      }
+    }
+
+    // Find the drive module frame joints by name suffix.
+    // Convention: front drive module ends with "_joint_1", rear with "_joint_3".
+    constexpr std::array<const char*, kNumModules> kModuleSuffixes = {"_joint_1", "_joint_3"};
+
+    for (size_t module_idx = 0; module_idx < kNumModules; ++module_idx) {
+      const std::string suffix(kModuleSuffixes[module_idx]);
+      bool found = false;
+      for (pinocchio::JointIndex j = 1; j < static_cast<pinocchio::JointIndex>(model.njoints);
+           ++j) {
+        const std::string& name = model.names[j];
+        if (name.size() >= suffix.size() &&
+            name.compare(name.size() - suffix.size(), suffix.size(), suffix) == 0) {
+          module_joint_indices[module_idx] = j;
+          found = true;
+          break;
+        }
+      }
+      if (!found) {
+        throw ModelException(
+            "libfranka: MobileModel: could not find drive module joint ending with '" + suffix +
+            "' in the URDF.");
+      }
+    }
+
+    // Find and map the 4 drive module joints (steering + drive per module).
+    // User array order: {front_steering, front_drive, rear_steering, rear_drive}
+    // Convention: suffixes "_joint_0", "_joint_1", "_joint_2", "_joint_3".
+    constexpr std::array<const char*, kNumDriveJoints> kJointSuffixes = {"_joint_0", "_joint_1",
+                                                                         "_joint_2", "_joint_3"};
+
+    for (size_t i = 0; i < kNumDriveJoints; ++i) {
+      const std::string suffix(kJointSuffixes[i]);
+      bool found = false;
+      for (pinocchio::JointIndex j = 1; j < static_cast<pinocchio::JointIndex>(model.njoints);
+           ++j) {
+        const std::string& name = model.names[j];
+        if (name.size() >= suffix.size() &&
+            name.compare(name.size() - suffix.size(), suffix.size(), suffix) == 0) {
+          joint_mappings[i].idx_q = model.joints[j].idx_q();
+          joint_mappings[i].is_continuous =
+              (model.joints[j].nq() == 2 && model.joints[j].nv() == 1);
+          found = true;
+          break;
+        }
+      }
+      if (!found) {
+        throw ModelException("libfranka: MobileModel: could not find joint ending with '" + suffix +
+                             "' in the URDF.");
+      }
+    }
+  }
+};
+
+MobileModel::MobileModel(const std::string& urdf_model)
+    : data_(std::make_unique<Data>(urdf_model)) {}
+
+MobileModel::MobileModel(MobileModel&&) noexcept = default;
+MobileModel& MobileModel::operator=(MobileModel&&) noexcept = default;
+MobileModel::~MobileModel() noexcept = default;
+
+std::array<double, 16> MobileModel::pose(MobileFrame frame,
+                                         const MobileJointPositions& joint_positions) const {
+  // Update only the 4 drive module joints in the pre-allocated buffer. Zero heap allocation.
+  for (size_t i = 0; i < kNumModules * kJointsPerModule; ++i) {
+    const auto& mapping = data_->joint_mappings[i];
+    if (mapping.is_continuous) {
+      data_->q_buffer[mapping.idx_q] = std::cos(joint_positions.drive_modules[i]);
+      data_->q_buffer[mapping.idx_q + 1] = std::sin(joint_positions.drive_modules[i]);
+    } else {
+      data_->q_buffer[mapping.idx_q] = joint_positions.drive_modules[i];
+    }
+  }
+
+  pinocchio::forwardKinematics(data_->model, data_->data, data_->q_buffer);
+
+  // Map MobileFrame to Pinocchio joint index via the pre-computed lookup table
+  size_t module_index = 0;
+  switch (frame) {
+    case MobileFrame::kFrontDriveModule:
+      module_index = 0;
+      break;
+    case MobileFrame::kRearDriveModule:
+      module_index = 1;
+      break;
+    default:
+      throw std::invalid_argument("libfranka: Invalid MobileFrame given.");
+  }
+
+  pinocchio::JointIndex joint_index = data_->module_joint_indices[module_index];
+
+  std::array<double, 16> result;
+  Eigen::Map<Eigen::Matrix4d>(result.data()) = data_->data.oMi[joint_index].toHomogeneousMatrix();
+  return result;
+}
+
+}  // namespace franka

src/model.cpp

@@ -2,6 +2,7 @@
 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
 #include "robot_model.h"
 
+#include <franka/exception.h>
 #include <franka/model.h>
 
 #include <iostream>
@@ -13,6 +14,8 @@
 #include <fstream>
 #include <sstream>
 
+#include "urdf_robot_type.h"
+
 using namespace std::string_literals;  // NOLINT(google-build-using-namespace)
 
 namespace franka {
@@ -24,6 +27,11 @@ Frame operator++(Frame& frame, int /* dummy */) noexcept {
 }
 
 Model::Model(const std::string& urdf_model) {
+  if (isMobileRobotUrdf(urdf_model)) {
+    throw ModelException(
+        "libfranka: Cannot create arm Model from a mobile robot URDF. "
+        "Use franka::MobileModel instead.");
+  }
   robot_model_ = std::make_unique<RobotModel>(urdf_model);
 }
 

src/robot.cpp

@@ -338,7 +338,16 @@ void Robot::stop() {
   impl_->executeCommand<research_interface::robot::StopMove>();
 }
 
+bool Robot::isMobileRobot() const noexcept {
+  return impl_->isMobileRobot();
+}
+
 Model Robot::loadModel() {
+  if (impl_->isMobileRobot()) {
+    throw InvalidOperationException(
+        "libfranka: loadModel() is not available for mobile robots. "
+        "Use franka::MobileModel with the URDF from getRobotModel() instead.");
+  }
   return impl_->loadModel(getRobotModel());
 }