Franka Robot demo 力控 force_control.cpp

// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <array>
#include <iostream>

#include <Eigen/Core>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

#include "examples_common.h"

/**
 * @example force_control.cpp
 * A simple PI force controller that renders in the Z axis the gravitational force corresponding
 * to a target mass of 1 kg.
 *
 * @warning: make sure that no endeffector is mounted and that the robot's last joint is in contact
 * with a horizontal rigid surface before starting.
 */

/**
 * @example force_control.cpp
 * 一个简单的 PI 力控制器，它在 Z 轴上施加相当于 1 kg 目标质量的重力。
 *
 * @警告: 确保在启动前没有安装末端执行器，并且机器人的最后一个关节与水平刚性表面接触。
 */

int main(int argc, char** argv) {
  // Check whether the required arguments were passed // 传入机器人IP参数
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }
  // parameters
  double desired_mass{0.0};
  constexpr double target_mass{1.0};    // 目标质量 1KG ，用于计算目标重力。 NOLINT(readability-identifier-naming)
  constexpr double k_p{1.0};            // 比例控制器增益，用于控制器中的比例项。 NOLINT(readability-identifier-naming)
  constexpr double k_i{2.0};            // 积分控制器增益，用于控制器中的积分项。 NOLINT(readability-identifier-naming)
  constexpr double filter_gain{0.001};  // 滤波增益，用于对传感器数据进行滤波以减少噪声。 NOLINT(readability-identifier-naming)

  try {
    // connect to robot
    franka::Robot robot(argv[1]);// 与机器人建立通信
    setDefaultBehavior(robot); // 设置机器人默认参数
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel(); // 加载运动学和动力学模型

    // set collision behavior 设置默认碰撞行为
    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

    franka::RobotState initial_state = robot.readOnce();// 读取机器人状态作为初始状态
    // initial_tau_ext 通常用于存储机器人每个关节的初始外部力矩。 tau_error_integral ： 通常用于存储积分误差，这在积分控制器（例如 PI 控制器）中非常常见
    Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7); 
    // Bias torque sensor 偏置力矩传感器
    std::array<double, 7> gravity_array = model.gravity(initial_state);// 计算机器人在初始化状态下的重力力矩
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());//测量到的关节力矩映射为一个 Eigen 向量
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());// 将 gravity_array 数组的数据映射为一个 Eigen 向量
    initial_tau_ext = initial_tau_measured - initial_gravity; // 测量到的关节力矩减去计算得到的重力力矩，来计算机器人受到的外部力矩。

    // init integrator 初始化力距积分误差
    tau_error_integral.setZero();

    // define callback for the torque control loop 定义力距控制循环的回调函数
    Eigen::Vector3d initial_position; // 初始化位置
    double time = 0.0;
    // lambda 函数,用于从机器人的状态中获取末端执行器的位置。
    auto get_position = [](const franka::RobotState& robot_state) {
      return Eigen::Vector3d(robot_state.O_T_EE[12], robot_state.O_T_EE[13],
                             robot_state.O_T_EE[14]);
    };

    // 定义力控回调函数
    auto force_control_callback = [&](const franka::RobotState& robot_state,
                                      franka::Duration period) -> franka::Torques {// 传入机器人状态，周期耗时， 返回力距
      time += period.toSec();

      if (time == 0.0) {
        initial_position = get_position(robot_state); // 获取初始化位置
      }

      //程序开始执行后的某个时间点,机器人的位置已经偏离了初始位置超过 10 毫米
      if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
        throw std::runtime_error("Aborting; too far away from starting pose!");
      }

      // get state variables 获取状态变量
      std::array<double, 42> jacobian_array = // 计算并存储了机器人末端执行器的 Jacobian 矩阵,为后续的运动学分析和控制提供了基础数据
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data()); // 转化雅克比矩阵
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data()); // 测量的力距
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data()); // 重力分量
      // tau_d(7): 这是一个大小为 7 的向量,用于存储期望的关节扭矩。
      // 大小为 6 的向量,用于存储期望的末端执行器力和扭矩。
      // 大小为 7 的向量,用于存储发送给机器人的最终关节扭矩命令。
      // 大小为 7 的向量,用于存储从机器人传感器测量得到的外部作用在关节上的扭矩。
      Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7); // 
      desired_force_torque.setZero();//初始化末端执行器扭矩
      desired_force_torque(2) = desired_mass * -9.81;//z轴施加1KG物体的重力
      // tau_measured 是从机器人关节传感器测量得到的实际关节扭矩。
      // gravity 是由于重力作用在关节上产生的扭矩。
      // initial_tau_ext 是在机器人启动时测量得到的初始外部扭矩。
      // 得到真正由外部力/扭矩作用在关节上产生的扭矩,也就是 tau_ext
      tau_ext << tau_measured - gravity - initial_tau_ext;
      tau_d << jacobian.transpose() * desired_force_torque; // 计算出期望的关节扭矩  将期望的末端执行器力/扭矩转换成期望的关节扭矩
      tau_error_integral += period.toSec() * (tau_d - tau_ext); // 累加积分误差 期望关节扭矩 - 真正外部关节扭矩
      // FF + PI control
      // tau_d 是期望的关节扭矩。
      // tau_ext 是从传感器测量得到的外部作用在关节上的扭矩。
      // tau_error_integral 是关节扭矩误差的积分项。
      // k_p 和 k_i 是比例和积分控制器的增益系数。
      // 实现了一个基于 PID 反馈控制的关节扭矩闭环控制算法,用于补偿外部载荷,使机器人的实际关节运动更加接近期望运动
      tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral; // 类似于PID 控制原理 

      // Smoothly update the mass to reach the desired target value
      // 实现了质量值的平滑更新,使其逐步达到目标质量值:
      desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;

      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;//将计算得到的 7 个关节的扭矩命令 tau_cmd 赋值给 tau_d_array 数组,
      return tau_d_array;
    };
    std::cout << "WARNING: Make sure sure that no endeffector is mounted and that the robot's last "
                 "joint is "
                 "in contact with a horizontal rigid surface before starting. Keep in mind that "
                 "collision thresholds are set to high values."
              << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    // start real-time control loop
    robot.control(force_control_callback);

  } catch (const std::exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }
  return 0;
}