Franka Robot demo 关节阻抗控制(joint_impedance_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 <atomic>
#include <cmath>
#include <functional>
#include <iostream>
#include <iterator>
#include <mutex>
#include <thread>

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

#include "examples_common.h"

namespace {
template <class T, size_t N>
std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
  ostream << "[";
  std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
  std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
  ostream << "]";
  return ostream;
}
}  // anonymous namespace

/**
 * @example joint_impedance_control.cpp
 * An example showing a joint impedance type control that executes a Cartesian motion in the shape
 * of a circle. The example illustrates how to use the internal inverse kinematics to map a
 * Cartesian trajectory to joint space. The joint space target is tracked by an impedance control
 * that additionally compensates coriolis terms using the libfranka model library. This example
 * also serves to compare commanded vs. measured torques. The results are printed from a separate
 * thread to avoid blocking print functions in the real-time loop.
 */
/**
 * @example joint_impedance_control.cpp
 * 一个示例，展示了一种关节阻抗类型控制，执行一个圆形的笛卡尔运动。
 * 该示例演示了如何使用内部逆运动学将笛卡尔轨迹映射到关节空间。
 * 关节空间目标由阻抗控制跟踪，同时使用 libfranka 模型库补偿科氏力项。
 * 该示例还用于比较命令力矩和测量力矩。结果会从单独的线程打印，以避免在实时循环中阻塞打印函数。
 */

int main(int argc, char** argv) {
  // Check whether the required arguments were passed. 传参检查
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }
  // Set and initialize trajectory parameters. 设置和初始化轨迹参数
  const double radius = 0.05;    // 半径
  const double vel_max = 0.25;   // 最大速度
  const double acceleration_time = 2.0; //加速时间 s
  const double run_time = 20.0; // 运行时间 s
  // Set print rate for comparing commanded vs. measured torques. 设置打印命令力距和测量力距的速率
  const double print_rate = 10.0;

  double vel_current = 0.0; // 当前速度
  double angle = 0.0; // 角度
  double time = 0.0;  // 耗时

  // Initialize data fields for the print thread. 初始化打印线程的数据字段
  struct {
    std::mutex mutex; // 互斥锁
    bool has_data; // 是否有数据
    std::array<double, 7> tau_d_last; // 最新期望力距
    franka::RobotState robot_state;   // 机器人状态
    std::array<double, 7> gravity;    // 重力 
  } print_data{};

  std::atomic_bool running{true}; // 原子布尔类型 运行标志

  // Start print thread.
  std::thread print_thread([print_rate, &print_data, &running]() { // 打印线程
    while (running) {
      // Sleep to achieve the desired print rate.
      std::this_thread::sleep_for(
          std::chrono::milliseconds(static_cast<int>((1.0 / print_rate * 1000.0)))); // 延迟指定时间

      // Try to lock data to avoid read write collisions.
      if (print_data.mutex.try_lock()) {// 尝试获取锁
        if (print_data.has_data) {
          std::array<double, 7> tau_error{};// 力距误差
          double error_rms(0.0);
          std::array<double, 7> tau_d_actual{};// 实际期望关节力距
          for (size_t i = 0; i < 7; ++i) {
            tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i]; // 实际期望关节力距 = 上一个周期计算出的期望关节扭矩 + 重力补偿项
            tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i];   // 力距误差 =  实际期望关节力距 - 当前状态下测量到的关节力矩
            error_rms += std::pow(tau_error[i], 2.0) / tau_error.size();        // 是均方根误差的平方根，表示了 tau_error 数组中所有关节力距误差的平均值
          }
          error_rms = std::sqrt(error_rms); // 均方根误差

          // Print data to console
          std::cout << "tau_error [Nm]: " << tau_error << std::endl
                    << "tau_commanded [Nm]: " << tau_d_actual << std::endl
                    << "tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl
                    << "root mean square of tau_error [Nm]: " << error_rms << std::endl
                    << "-----------------------" << std::endl;
          print_data.has_data = false;
        }
        print_data.mutex.unlock();
      }
    }
  });

  try {
    // Connect to robot.
    franka::Robot robot(argv[1]); // 连接机器人
    setDefaultBehavior(robot); // 初始化机器人默认行为

    // First move the robot to a suitable joint configuration 将机器人移动到一个合适的关节配置
    std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
    MotionGenerator motion_generator(0.5, q_goal);
    std::cout << "WARNING: This example will move the robot! "
              << "Please make sure to have the user stop button at hand!" << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(motion_generator);
    std::cout << "Finished moving to initial joint configuration." << std::endl;

    // Set additional parameters always before the control loop, NEVER in the control loop!
    // Set collision behavior.
    // 在控制循环之前，始终设置额外的参数，绝不要在控制循环中设置！
    // 设置碰撞行为。
    robot.setCollisionBehavior(
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});

    // Load the kinematics and dynamics model. 加载运动学和动力学模型
    franka::Model model = robot.loadModel();

    std::array<double, 16> initial_pose; // 初始化位姿

    // Define callback function to send Cartesian pose goals to get inverse kinematics solved. 
    // 定义一个回调函数，以发送笛卡尔姿态目标来求解逆运动学。
    auto cartesian_pose_callback = [=, &time, &vel_current, &running, &angle, &initial_pose](
                                       const franka::RobotState& robot_state,
                                       franka::Duration period) -> franka::CartesianPose {
      // Update time.
      time += period.toSec(); // 累加时间

      if (time == 0.0) {
        // Read the initial pose to start the motion from in the first time step.
        initial_pose = robot_state.O_T_EE_c;// 末端执行器（End-Effector）相对于基坐标系（Base）的当前位姿
      }

      // Compute Cartesian velocity. 计算迪卡尔速度
      if (vel_current < vel_max && time < run_time) {
        vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);// 线性求解当前速度
      }
      if (vel_current > 0.0 && time > run_time) {// 超时则当前速度为负数
        vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
      }
      vel_current = std::fmax(vel_current, 0.0);// 速度应 >= 0
      vel_current = std::fmin(vel_current, vel_max); // 速度应 <= vel_max

      // Compute new angle for our circular trajectory.
      angle += period.toSec() * vel_current / std::fabs(radius); // 累加角度 角度 = t * (v / r)
      if (angle > 2 * M_PI) {// 将角度控制在 0-2PI之间
        angle -= 2 * M_PI;
      }

      // Compute relative y and z positions of desired pose.
      double delta_y = radius * (1 - std::cos(angle));
      double delta_z = radius * std::sin(angle);
      franka::CartesianPose pose_desired = initial_pose; // 只修改y和z
      pose_desired.O_T_EE[13] += delta_y;
      pose_desired.O_T_EE[14] += delta_z;

      // Send desired pose. 设置期望位姿
      if (time >= run_time + acceleration_time) {// 超过运行时间+期望时间，则停止运动。
        running = false;
        return franka::MotionFinished(pose_desired);
      }

      return pose_desired;
    };

    // Set gains for the joint impedance control. 为关节阻抗运动设置增益
    // Stiffness
    const std::array<double, 7> k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}}; // k_gains 数组包含了每个关节的刚度值，单位为Nm/rad  刚度参数决定了关节对位移的反应程度
    // Damping
    const std::array<double, 7> d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}}; // 数组包含了每个关节的阻尼值，单位为Nm/(rad/s)。 阻尼参数则决定了关节对速度的反应程度

    // Define callback for the joint torque control loop. 为关节力距控制循环定义回调函数
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback =
            [&print_data, &model, k_gains, d_gains](
                const franka::RobotState& state, franka::Duration /*period*/) -> franka::Torques {
      // Read current coriolis terms from model. // 从模型中读取当前的科氏力项。
      std::array<double, 7> coriolis = model.coriolis(state);

      // Compute torque command from joint impedance control law.
      // Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one
      // time step delay.
      // 从关节阻抗控制法则计算扭矩命令。
      // 注意：我们的笛卡尔姿态逆运动学的答案始终在 state.q_d 中，有一个时间步骤的延迟。
      std::array<double, 7> tau_d_calculated; // 计算期望关节力距
      for (size_t i = 0; i < 7; i++) {
        tau_d_calculated[i] = // 位置增益 + 速度增益 + 科氏力项
            k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
      }

      // The following line is only necessary for printing the rate limited torque. As we activated
      // rate limiting for the control loop (activated by default), the torque would anyway be
      // adjusted!
      std::array<double, 7> tau_d_rate_limited =
          franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, state.tau_J_d);// 速率限制

      // Update data to print.
      if (print_data.mutex.try_lock()) {
        print_data.has_data = true;
        print_data.robot_state = state;
        print_data.tau_d_last = tau_d_rate_limited;
        print_data.gravity = model.gravity(state);
        print_data.mutex.unlock();
      }

      // Send torque command.
      return tau_d_rate_limited;
    };

    // Start real-time control loop. 开始实时控制循环 
    robot.control(impedance_control_callback, cartesian_pose_callback);

  } catch (const franka::Exception& ex) {
    running = false;
    std::cerr << ex.what() << std::endl;
  }

  if (print_thread.joinable()) {
    print_thread.join();
  }
  return 0;
}