Franka Robot 笛卡尔阻抗控制示例(cartesian_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 <cmath>
#include <functional>
#include <iostream>

#include <Eigen/Dense>

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

#include "examples_common.h"

/**
 * @example cartesian_impedance_control.cpp
 * An example showing a simple cartesian impedance controller without inertia shaping
 * that renders a spring damper system where the equilibrium is the initial configuration.
 * After starting the controller try to push the robot around and try different stiffness levels.
 *
 * @warning collision thresholds are set to high values. Make sure you have the user stop at hand!
 */
/**
 * @example cartesian_impedance_control.cpp
 * 一个展示简单笛卡尔阻抗控制器的例子，该控制器没有惯性整形，
 * 它渲染了一个平衡点为初始配置的弹簧阻尼系统。
 * 在启动控制器后，尝试推动机器人并尝试不同的刚度级别。
 *
 * @warning 碰撞阈值设置为较高的值。请确保手边有用户停止按钮！
 */
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;
  }

  // Compliance parameters 阻抗参数 平移刚度
  const double translational_stiffness{150.0};
  const double rotational_stiffness{10.0}; //阻抗系数 旋转刚度
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);// 刚度 阻尼系数
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *  Eigen::MatrixXd::Identity(3, 3);

  try {
    // connect to robot
    franka::Robot robot(argv[1]); // 链接机器人
    setDefaultBehavior(robot); // 设置机器人默认行为
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel(); // 加载运动学和动态模型

    franka::RobotState initial_state = robot.readOnce();// 读取机器人初始化状态

    // equilibrium point is the initial position  平衡点是初始位置。
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));// 使用齐次变换矩阵初始化仿射变换矩阵  齐次变换矩阵和仿射变换对象的主要目的是
                                                                                         // 为了方便进行各种空间变换操作，如平移、旋转和缩放
    Eigen::Vector3d position_d(initial_transform.translation()); // 提取期望平移
    Eigen::Quaterniond orientation_d(initial_transform.rotation()); // 提取期望旋转

    // set collision behavior 设置碰撞行为
    // 设置碰撞行为 在这个例子中，当机器人在操作过程中检测到的力矩在 100 之内时，它会继续操作。
    // 一旦检测到的力矩超过 100.0 时，机器人会认为发生了碰撞，并采取相应的安全措施，如停止运动或减速。
    // 类似地，笛卡尔空间中的力阈值也是如此设置的。
    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}});

    // define callback for the torque control loop 为力距控制循环定义回调函数
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                         franka::Duration /*duration*/) -> franka::Torques {
      // get state variables
      std::array<double, 7> coriolis_array = model.coriolis(robot_state); // 计算机器人当前状态下的科里奥利力矩。科里奥利力矩是机器人动态方程中的一部分，它取决于机器人的速度和运动状态。
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);//使用机器人状态计算末端执行器相对于几座表的的雅克比矩阵  雅可比矩阵是机器人控制中的一个重要概念，它描述了机器人关节空间速度和笛卡尔空间速度之间的关系。

      // convert to Eigen 转换为 Eigen 矩阵格式，可以让我们利用 Eigen 库提供的强大线性代数工具来处理雅可比矩阵。
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data()); // 科里奥利力矩  使用 Eigen::Map 将 zeroJacobian 函数的输出直接映射为 Eigen 矩阵
      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data()); // 雅克比矩阵
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());         // 各个关节当前角度
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());       // 各个关节当前角速度
      Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));    // 当前末端执行器相对于基坐标的位姿 转化为3D仿射变换
      Eigen::Vector3d position(transform.translation());  // 拆分平移分量
      Eigen::Quaterniond orientation(transform.rotation()); // 拆分旋转分量

      // compute error to desired equilibrium pose 计算到期望姿态的误差
      // position error 位置误差
      Eigen::Matrix<double, 6, 1> error; 
      error.head(3) << position - position_d; // 计算当前位置与初始化位置偏移量

      // orientation error 旋转误差
      // "difference" quaternion 四元数误差
      if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
        orientation.coeffs() << -orientation.coeffs(); // 四元数取反  获得相反方向的旋转姿态
      }
      // "difference" quaternion //orientation.inverse() 计算当前姿态 orientation 的逆四元数。四元数的逆代表着旋转方向的反向。
      Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d); // 计算从当前姿态到期望姿态的相对姿态变换。
      error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();// 缓存旋转误差分量
      // Transform to base frame 
      error.tail(3) << -transform.rotation() * error.tail(3);// 将四元数误差从末端执行器的局部坐标系转换到基坐标系下

      // compute control Eigen::VectorXd 是 Eigen 库中表示动态大小的向量的数据类型
      Eigen::VectorXd tau_task(7), tau_d(7);

      // Spring damper system with damping ratio=1  阻尼比为1的Spring-阻尼系统
      // jacobian.transpose() 使用了雅可比矩阵的转置, 雅可比矩阵用于将关节空间的量转换到笛卡尔空间,
      // 而转置则是相反的转换过程,即从笛卡尔空间到关节空间
      // -stiffness * error 这部分表示基于位置误差的反馈力矩
      // -damping * (jacobian * dq) 这部分表示基于关节速度的阻尼力矩
      // 这行代码实现了基于位置误差和关节速度的反馈控制,将其转换到关节空间的力矩 tau_task。这种控制方法常用于机器人关节空间的控制中
      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
      // 这行代码的作用是计算最终的关节驱动力矩 tau_d。
      // tau_task 这是前一行代码计算得到的任务空间中的力矩, 包含了基于位置误差和关节速度的反馈力矩
      // coriolis 这是科氏力矩,是由于机器人关节运动而产生的力矩
      tau_d << tau_task + coriolis;
     
      std::array<double, 7> tau_d_array{};
      // 将计算得到的关节驱动力矩 tau_d 赋值到一个数组 
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
      return tau_d_array;
    };

    // start real-time control loop
    std::cout << "WARNING: Collision thresholds are set to high values. "
              << "Make sure you have the user stop at hand!" << std::endl
              << "After starting try to push the robot and see how it reacts." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(impedance_control_callback); // 外部阻抗控制器 控制机器人运动

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

  return 0;
}

 

这段示例代码展示了如何实现一个简单的笛卡尔阻抗控制器，用于控制 FRANKA 机器人的末端执行器在笛卡尔空间内的运动。以下是代码的关键部分和功能解释：

1. 命令行参数检查：

   • 程序开始时检查是否传入了正确的机器人主机名，确保可以连接到指定的机器人。

2. 阻抗参数定义：

   • 定义了平移和旋转的阻抗参数，包括平移刚度 translational_stiffness 和旋转刚rotational_stiffness。
   • 使用这些参数构造了笛卡尔空间的刚度矩阵 stiffness 和阻尼矩阵 damping。

3. 机器人连接和设置：

   • 使用指定的机器人主机名连接到机器人，并设置默认的碰撞行为，确保在操作过程中安全。

4. 获取初始机器人状态：

   • 通过 robot.readOnce() 方法获取机器人的初始状态，包括末端执行器的初始位姿。

5. 控制回调函数：

   • 定义了一个控制回调函数 impedance_control_callback，用于实现笛卡尔阻抗控制。
   • 函数中计算了当前机器人状态下的位置和姿态误差，并基于阻抗控制原理计算所需的关节力矩 tau_d。
   • 使用了机器人模型 model 提供的雅可比矩阵和科氏力矩，以及当前状态的关节角度和角速度。

6. 控制循环启动：

   • 在启动实时控制循环前，输出警告信息，提醒用户注意碰撞阈值设置较高，确保操作安全。
   • 用户按下 Enter 键后，调用 robot.control() 启动实时控制，将阻抗控制回调函数传入，控制机器人的笛卡尔空间运动。

这段代码通过实时控制机器人的力矩，实现了在笛卡尔空间内的阻抗控制，使机器人能够在受力时表现出弹簧和阻尼系统的特性，保持稳定的运动状态。