企业官网建设流程全解析

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目录下编写碰撞参数配置脚本

cd /path/to/gello/ros2/src #创建ros2 humble python 包 ros2 pkg create --build-type ament_python set_collision_behavior --dependencies rclpy cd ./set_collision_behavior mkdir scripts && cd scripts touch set_collision_behavior.py #2. 编辑脚本文件，复制下列代码于脚本中 gedit set_collision_behavior.py

#!/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()

进入脚本目录执行赋予可执行权限，再在setup.py完成脚本注册安装。

chmod 777 *

#在setup.py完成注册 scripts=[ 'scripts/set_collision_behavior.py', ], ) #编译 cd /path/to/gello_software/ros2 colcon build --symlink-install source install/setup.bash # Start the GELLO node ros2 launch franka_gello_state_publisher main.launch.py config_file:=franka_gello_single.yaml # Start the Franka Hand node ros2 launch franka_gripper_manager franka_gripper_client.launch.py config_file:=example_fr3_config_franka_hand.yaml # Hold the GELLO and place it in a comfortable position with some distance from the base pin # Start the robot node (the robot position will sync directly to the GELLO position. Avoid moving the GELLO abruptly during synchronization) ros2 launch franka_fr3_arm_controllers franka_fr3_arm_controllers.launch.py robot_config_file:=example_fr3_config.yaml # start set_collison_behavior node ros2 run set_collision_behavior set_collision_behavior.py

在Franka控制器启动后，运行脚本可动态修改 Franka 机械臂碰撞检测灵敏度，对运动惯性带来的虚假碰撞信号做过滤，抑制遥操过程中惯性负载触发的误报爆红。