1、问题概述

  在Gello主从遥操Franka机械臂场景中，机器人频繁出现爆红保护、急停锁机问题，极大影响遥操稳定性与数据采集效率。该问题并非硬件故障，核心源于Gello遥控基于位置差解算控制力矩的特性：人工操作抖动、姿态突变、负载惯性会产生瞬时超标力矩，无约束力矩会触发Franka伺服超限保护，同时运动惯性力易被机身碰撞检测算法误判为外部碰撞，最终导致机械臂爆红停机。本文从力位混合控制原理出发，拆解问题本质，提供针对性力矩限幅、碰撞参数调优方案，彻底解决遥操爆红问题。

2、Gello主从遥控整体逻辑

Gello遥操Franka的关节阻抗控制本质可等效为“虚拟弹簧阻尼模型”，源码力矩求解公式为：

tau_d = K_p(q_{goal}-q) - K_d 

其中刚度系数K_p对应弹簧劲度，主手Gello目标位q_{goa}与Franka实际位置q的偏差，相当于弹簧拉伸或压缩的形变量，系统会根据形变大小线性生成跟随力矩，配合阻尼项抑制关节振动，实现柔顺跟随效果。正常小幅操作下位置偏差小，弹簧形变量可控，输出力矩处于Franka安全阈值内；但人工快速遥操、姿态突变时会产生极大位置偏差，等同于虚拟弹簧被瞬间大幅拉伸，若不做力矩限制，公式会解算出极大的瞬时驱动力矩。该超标力矩远超Franka各关节额定安全力矩范围，直接触发机器人力矩超限保护机制，叠加运动惯性力矩干扰后，进一步触发机身碰撞检测误判，最终导致机械臂爆红急停。

3、优化方向

3.1、计算出力矩过大问题

  针对大位置偏差产生超大力矩、惯性引发碰撞误报两大问题，先修改阻抗控制器源码实现逐关节力矩硬限幅。直接替换以下文件即可，路径：/path/to/gello_software/ros2/src/franka_fr3_arm_controllers/franka_fr3_arm_controllers。在 hpp 增加tau_max_成员，cpp 注册并加载 yaml 各关节力矩参数，复用现有cwiseMax/cwiseMin钳位逻辑。即便虚拟弹簧算出很大理论力矩，下发力矩也被约束在 FR3 安全范围，解决力矩超限爆红。

1. joint_impedance_controller.hpp

// Copyright (c) 2025 Franka Robotics GmbH
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <Eigen/Eigen>
#include <controller_interface/controller_interface.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include "franka_fr3_arm_controllers/motion_generator.hpp"

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

namespace franka_fr3_arm_controllers {

/**
 * Controller to move the robot to a desired joint position.
 */
class JointImpedanceController : public controller_interface::ControllerInterface {
 public:
  using Vector7d = Eigen::Matrix<double, 7, 1>;
  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
      const override;
  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
      const override;
  controller_interface::return_type update(const rclcpp::Time& time,
                                           const rclcpp::Duration& period) override;
  CallbackReturn on_init() override;
  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;

 private:
  std::string arm_id_;
  std::string namespace_prefix_;
  std::string robot_description_;
  const int num_joints = 7;
  Vector7d q_;
  Vector7d dq_;
  Vector7d dq_filtered_;
  Vector7d k_gains_;
  Vector7d d_gains_;
  double k_alpha_;
  // FR3 joint torque limits [N·m]: joints 1-4 @ 87 N·m, joints 5-7 @ 12 N·m
  const Vector7d tau_max_ = (Vector7d() << 87.0, 87.0, 87.0, 87.0, 12.0, 12.0, 12.0).finished();
  bool move_to_start_position_finished_{false};
  bool motion_generator_initialized_{false};
  rclcpp::Time start_time_;
  std::unique_ptr<MotionGenerator> motion_generator_;
  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber_ = nullptr;
  bool gello_position_values_valid_ = false;
  std::array<double, 7> gello_position_values_{0, 0, 0, 0, 0, 0, 0};
  rclcpp::Time last_joint_state_time_;

  Vector7d calculateTauDGains_(const Vector7d& q_goal);
  bool validateGains_(const std::vector<double>& gains, const std::string& gains_name);
  bool initializeMotionGenerator_();
  void updateJointStates_();
  void validateGelloPositions_(const sensor_msgs::msg::JointState& msg);
  void jointStateCallback_(const sensor_msgs::msg::JointState msg);
};

}  // namespace franka_fr3_arm_controllers

joint_impedance_controller.cpp

// Copyright (c) 2025 Franka Robotics GmbH
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <franka_fr3_arm_controllers/joint_impedance_controller.hpp>

#include <Eigen/Eigen>
#include <cassert>
#include <cmath>
#include <exception>
#include <string>

using std::placeholders::_1;

namespace franka_fr3_arm_controllers {

controller_interface::InterfaceConfiguration
JointImpedanceController::command_interface_configuration() const {
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;

  for (int i = 1; i <= num_joints; ++i) {
    config.names.push_back(namespace_prefix_ + arm_id_ + "_joint" + std::to_string(i) + "/effort");
  }
  return config;
}

controller_interface::InterfaceConfiguration
JointImpedanceController::state_interface_configuration() const {
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  for (int i = 1; i <= num_joints; ++i) {
    config.names.push_back(namespace_prefix_ + arm_id_ + "_joint" + std::to_string(i) +
                           "/position");
    config.names.push_back(namespace_prefix_ + arm_id_ + "_joint" + std::to_string(i) +
                           "/velocity");
  }
  return config;
}

controller_interface::return_type JointImpedanceController::update(
    const rclcpp::Time& /*time*/,
    const rclcpp::Duration& /*period*/) {
  updateJointStates_();
  Vector7d q_goal;
  Vector7d tau_d_calculated;

  if (!motion_generator_initialized_) {
    // After starting the controller we wait for valid joint states from the input topic
    // Until we get valid joint states we will send zero torques to the robot
    // to allow the user to reposition the robot
    motion_generator_initialized_ = initializeMotionGenerator_();

    if (!motion_generator_initialized_) {
      for (int i = 0; i < num_joints; ++i) {
        command_interfaces_[i].set_value(0.0);
      }

      return controller_interface::return_type::OK;
    }
  }

  if (!move_to_start_position_finished_) {
    // We have received valid joint states and initialized the motion generator
    // Now we move smoothly to the first joint position received from the input topic
    auto trajectory_time = this->get_node()->now() - start_time_;
    auto motion_generator_output = motion_generator_->getDesiredJointPositions(trajectory_time);
    move_to_start_position_finished_ = motion_generator_output.second;

    q_goal = motion_generator_output.first;
  }

  if (move_to_start_position_finished_) {
    // After reaching the start position we follow the joint position from the input topic
    // This is the normal operation mode of the controller
    if (!gello_position_values_valid_) {
      RCLCPP_FATAL(get_node()->get_logger(), "Timeout: No valid joint states received from Gello");
      rclcpp::shutdown();  // Exit the node permanently
    }
    for (int i = 0; i < num_joints; ++i) {
      q_goal(i) = gello_position_values_[i];
    }
  }

  tau_d_calculated = calculateTauDGains_(q_goal);

  for (int i = 0; i < num_joints; ++i) {
    command_interfaces_[i].set_value(tau_d_calculated(i));
  }

  return controller_interface::return_type::OK;
}

void JointImpedanceController::jointStateCallback_(const sensor_msgs::msg::JointState msg) {
  if (last_joint_state_time_.seconds() == 0.0) {
    return;
  }

  if (msg.position.size() < gello_position_values_.size()) {
    RCLCPP_WARN(get_node()->get_logger(),
                "Received joint state size is smaller than expected size.");
    return;
  }

  std::copy(msg.position.begin(), msg.position.begin() + gello_position_values_.size(),
            gello_position_values_.begin());

  validateGelloPositions_(msg);
  last_joint_state_time_ = msg.header.stamp;
}

CallbackReturn JointImpedanceController::on_init() {
  try {
    auto_declare<std::string>("arm_id", "");
    auto_declare<std::vector<double>>("k_gains", {});
    auto_declare<std::vector<double>>("d_gains", {});
  } catch (const std::exception& e) {
    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
    return CallbackReturn::ERROR;
  }
  return CallbackReturn::SUCCESS;
}

CallbackReturn JointImpedanceController::on_configure(
    const rclcpp_lifecycle::State& /*previous_state*/) {
  arm_id_ = get_node()->get_parameter("arm_id").as_string();
  namespace_prefix_ = get_node()->get_namespace();
  if (namespace_prefix_ == "/" || namespace_prefix_.empty()) {
    namespace_prefix_.clear();
  } else {
    // Remove leading slash and add trailing underscore
    namespace_prefix_ = namespace_prefix_.substr(1) + "_";
  }

  auto k_gains = get_node()->get_parameter("k_gains").as_double_array();
  auto d_gains = get_node()->get_parameter("d_gains").as_double_array();
  auto k_alpha = get_node()->get_parameter("k_alpha").as_double();

  if (!validateGains_(k_gains, "k_gains") || !validateGains_(d_gains, "d_gains")) {
    return CallbackReturn::FAILURE;
  }

  for (int i = 0; i < num_joints; ++i) {
    d_gains_(i) = d_gains.at(i);
    k_gains_(i) = k_gains.at(i);
  }

  if (k_alpha < 0.0 || k_alpha > 1.0) {
    RCLCPP_FATAL(get_node()->get_logger(), "k_alpha should be in the range [0, 1]");
    return CallbackReturn::FAILURE;
  }

  k_alpha_ = k_alpha;

  dq_filtered_.setZero();

  auto parameters_client =
      std::make_shared<rclcpp::AsyncParametersClient>(get_node(), "robot_state_publisher");
  parameters_client->wait_for_service();

  auto future = parameters_client->get_parameters({"robot_description"});
  auto result = future.get();
  if (!result.empty()) {
    robot_description_ = result[0].value_to_string();
  } else {
    RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter.");
  }

  joint_state_subscriber_ = get_node()->create_subscription<sensor_msgs::msg::JointState>(
      "gello/joint_states", 1,
      [this](const sensor_msgs::msg::JointState& msg) { jointStateCallback_(msg); });

  return CallbackReturn::SUCCESS;
}

CallbackReturn JointImpedanceController::on_activate(
    const rclcpp_lifecycle::State& /*previous_state*/) {
  last_joint_state_time_ = get_node()->now();
  dq_filtered_.setZero();
  start_time_ = this->get_node()->now();

  return CallbackReturn::SUCCESS;
}

auto JointImpedanceController::calculateTauDGains_(const Vector7d& q_goal) -> Vector7d {
  dq_filtered_ = (1 - k_alpha_) * dq_filtered_ + k_alpha_ * dq_;
  Vector7d tau_d_calculated;
  tau_d_calculated = k_gains_.cwiseProduct(q_goal - q_) + d_gains_.cwiseProduct(-dq_filtered_);

  // Clamp torques to FR3 hardware limits to prevent safety reflex / brake triggering
  tau_d_calculated = tau_d_calculated.cwiseMax(-tau_max_).cwiseMin(tau_max_);

  return tau_d_calculated;
}

bool JointImpedanceController::validateGains_(const std::vector<double>& gains,
                                              const std::string& gains_name) {
  if (gains.empty()) {
    RCLCPP_FATAL(get_node()->get_logger(), "%s parameter not set", gains_name.c_str());
    return false;
  }

  if (gains.size() != static_cast<uint>(num_joints)) {
    RCLCPP_FATAL(get_node()->get_logger(), "%s should be of size %d but is of size %ld",
                 gains_name.c_str(), num_joints, gains.size());
    return false;
  }

  return true;
}

void JointImpedanceController::validateGelloPositions_(const sensor_msgs::msg::JointState& msg) {
  const double max_time_diff = 0.5;
  auto current_time = get_node()->now();
  auto time_since_last_joint_state = (current_time - last_joint_state_time_).seconds();
  auto time_since_msg_stamp = (current_time - msg.header.stamp).seconds();
  gello_position_values_valid_ =
      (time_since_last_joint_state < max_time_diff && time_since_msg_stamp < max_time_diff);
  if (!gello_position_values_valid_) {
    RCLCPP_WARN(get_node()->get_logger(),
                "Gello position values are not valid. Time since last joint state: %f // Time "
                "since message stamp: %f",
                time_since_last_joint_state, time_since_msg_stamp);
  }
}

void JointImpedanceController::updateJointStates_() {
  for (auto i = 0; i < num_joints; ++i) {
    const auto& position_interface = state_interfaces_.at(2 * i);
    const auto& velocity_interface = state_interfaces_.at(2 * i + 1);

    assert(position_interface.get_interface_name() == "position");
    assert(velocity_interface.get_interface_name() == "velocity");

    q_(i) = position_interface.get_value();
    dq_(i) = velocity_interface.get_value();
  }
}

bool JointImpedanceController::initializeMotionGenerator_() {
  if (!gello_position_values_valid_) {
    // Only send a warning once every 10 seconds in order not to spam the log
    RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 10 * 1000,
                         "Waiting for valid joint states...");
    return false;
  }

  Vector7d q_goal;
  updateJointStates_();
  for (int i = 0; i < num_joints; ++i) {
    q_goal(i) = gello_position_values_[i];
  }
  RCLCPP_INFO(get_node()->get_logger(), "q_goal of motion generator: [%f, %f, %f, %f, %f, %f, %f]",
              q_goal(0), q_goal(1), q_goal(2), q_goal(3), q_goal(4), q_goal(5), q_goal(6));

  const double motion_generator_speed_factor = 0.2;
  motion_generator_ = std::make_unique<MotionGenerator>(motion_generator_speed_factor, q_, q_goal);
  return true;
}

}  // namespace franka_fr3_arm_controllers
#include "pluginlib/class_list_macros.hpp"
// NOLINTNEXTLINE
PLUGINLIB_EXPORT_CLASS(franka_fr3_arm_controllers::JointImpedanceController,
                       controller_interface::ControllerInterface)

3.2、运动惯性力距触发力矩保护

  力矩限幅只能规避力矩超限爆红，无法消除运动惯性造成的碰撞检测误触发。新建 ROS2 Humble Python 包，在scripts目录下编写碰撞参数配置脚本；进入脚本目录执行

chmod 777 *

赋予可执行权限，再在setup.py完成脚本注册安装。在Franka控制器启动后，运行脚本可动态修改 Franka 机械臂碰撞检测灵敏度，对运动惯性带来的虚假碰撞信号做过滤，抑制遥操过程中惯性负载触发的误报爆红。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from franka_msgs.srv import SetFullCollisionBehavior

class CollisionBehaviorSetter(Node):
    def __init__(self):
        super().__init__('collision_behavior_setter')
        
        # 匹配你当前单臂加载的服务名（如果你使用的是带命名空间的双臂，可按需调整前缀）
        self.client = self.create_client(
            SetFullCollisionBehavior, 
            '/service_server/set_full_collision_behavior'
        )
        
        self.set_behavior()

    def set_behavior(self):
        if not self.client.wait_for_service(timeout_sec=5.0):
            self.get_logger().error('Franka 碰撞行为设置服务未响应，请检查控制器是否启动')
            return

        request = SetFullCollisionBehavior.Request()
        # 遥操作推荐的稳健阈值
        request.lower_torque_thresholds_nominal = [80.0, 80.0, 80.0, 80.0, 10.0, 10.0, 10.0]
        request.upper_torque_thresholds_nominal = [80.0, 80.0, 80.0, 80.0, 10.0, 10.0, 10.0]
        request.lower_force_thresholds_nominal = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]
        request.upper_force_thresholds_nominal = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]

        self.get_logger().info('正在配置机械臂碰撞阈值...')
        
        # 使用同步调用等待结果，确保请求成功发送并接收到回复
        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self, future, timeout_sec=3.0)
        
        if future.done():
            try:
                response = future.result()
                self.get_logger().info('碰撞阈值设置成功！')
            except Exception as e:
                self.get_logger().error(f'服务调用失败: {e}')
        else:
            self.get_logger().error('服务调用超时')

def main():
    rclpy.init()
    node = CollisionBehaviorSetter()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()