Franka Robot demo 关节速度运动生成器和力矩控制(motion_with_control.cpp)

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

#include <Poco/DateTimeFormatter.h>
#include <Poco/File.h>
#include <Poco/Path.h>

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

#include "examples_common.h"

/**
 * @example motion_with_control.cpp
 * An example showing how to use a joint velocity motion generator and torque control.
 *
 * Additionally, this example shows how to capture and write logs in case an exception is thrown
 * during a motion.
 *
 * @warning Before executing this example, make sure there is enough space in front of the robot.
 */
/**
 * @example motion_with_control.cpp
 * 一个展示如何使用关节速度运动生成器和力矩控制的示例。
 *
 * 此外,该示例还展示了如何在运动过程中出现异常时捕获和记录日志。
 *
 * @warning 在执行此示例之前,请确保机器人前面有足够的空间。
 */


namespace {
/**
 * 这是一个 C++ 类 Controller，用于实现关节速度运动生成器和力矩控制
 */
class Controller {
 public:
  // 初始化滤波器相关的成员变量,并分配内存空间用于存储关节速度缓冲区
  Controller(size_t dq_filter_size,// 用于关节速度平滑的滤波器大小
             const std::array<double, 7>& K_P,  //7个比例增益参数   // NOLINT(readability-identifier-naming)
             const std::array<double, 7>& K_D)  //7个微分增益参数   // NOLINT(readability-identifier-naming)
      : dq_current_filter_position_(0), dq_filter_size_(dq_filter_size), K_P_(K_P), K_D_(K_D) {
    std::fill(dq_d_.begin(), dq_d_.end(), 0);
    dq_buffer_ = std::make_unique<double[]>(dq_filter_size_ * 7);
    std::fill(&dq_buffer_.get()[0], &dq_buffer_.get()[dq_filter_size_ * 7], 0);
  }

  // 输入 franka::RobotState 对象,代表机器人的当前状态。 
  // 根据机器人当前的状态 (franka::RobotState) 计算出期望的关节力矩 (franka::Torques)
  inline franka::Torques step(const franka::RobotState& state) {
    // 调用 updateDQFilter() 更新关节速度滤波器。
    // 最新的关节速度数据更新关节速度滤波器。这个函数会将新的关节速度数据写入 dq_buffer_ 缓冲区,
    // 并更新 dq_current_filter_position_ 变量。
    updateDQFilter(state);

    std::array<double, 7> tau_J_d;  // NOLINT(readability-identifier-naming)
    for (size_t i = 0; i < 7; i++) {
      // 它实现了一个典型的 PD (比例-微分) 控制器公式
      // PD 控制器是一种常见的闭环反馈控制算法,它由两部分组成:
      // 比例 (Proportional, P) 项:
      // 这部分根据误差大小生成一个与误差成正比的控制量。
      // 在这个例子中,误差就是目标角度 state.q_d[i] 与当前角度 state.q[i] 之间的差。
      // 比例增益 K_P_[i] 控制了这部分控制量的大小。
      // 微分 (Derivative, D) 项:
      // 这部分根据误差变化率生成一个与误差变化率成正比的控制量。
      // 在这个例子中,误差变化率就是目标角速度 dq_d_[i] 与滤波后的当前角速度 getDQFiltered(i) 之间的差。
      // 微分增益 K_D_[i] 控制了这部分控制量的大小。
      // 通过比例项减小角度偏差,微分项减小角速度偏差,PD 控制器可以实现良好的跟踪性能和响应特性。这种 PD 控制结构在机器人关节控制中非常常见和有效。
      tau_J_d[i] = K_P_[i] * (state.q_d[i] - state.q[i]) + K_D_[i] * (dq_d_[i] - getDQFiltered(i));
    }
    return tau_J_d;
  }
  // 输入 franka::RobotState 对象,更新关节速度缓冲区
  void updateDQFilter(const franka::RobotState& state) {
    // 使用循环将当前关节速度写入缓冲区,并更新缓冲区的写入位置
    for (size_t i = 0; i < 7; i++) {
      dq_buffer_.get()[dq_current_filter_position_ * 7 + i] = state.dq[i];
    }
    dq_current_filter_position_ = (dq_current_filter_position_ + 1) % dq_filter_size_;
  }

  double getDQFiltered(size_t index) const {
    // 输入关节索引 index,计算并返回该关节速度的滤波值。
    double value = 0;
    //通过遍历缓冲区中对应关节的值并求平均来实现滤波
    for (size_t i = index; i < 7 * dq_filter_size_; i += 7) {
      value += dq_buffer_.get()[i];
    }
    return value / dq_filter_size_;
  }

 private:
  size_t dq_current_filter_position_;// 当前在关节速度缓冲区中的写入位置。
  size_t dq_filter_size_; // 关节速度滤波器的大小

  const std::array<double, 7> K_P_;  // NOLINT(readability-identifier-naming)
  const std::array<double, 7> K_D_;  // NOLINT(readability-identifier-naming)

  std::array<double, 7> dq_d_; // 存储目标关节速度
  std::unique_ptr<double[]> dq_buffer_; // 关节速度缓冲区
};


/// @brief 函数的目的是生成一个平滑的速度轨迹，同时具有连续的加速度和速度曲线
/// @param a_max 最大加速度
/// @return 
std::vector<double> generateTrajectory(double a_max) {
  // Generating a motion with smooth velocity and acceleration.
  // Squared sine is used for the acceleration/deceleration phase.
  std::vector<double> trajectory;              // 用于存储生成的速度轨迹
  constexpr double kTimeStep = 0.001;          // 拟的时间步长，设置为 0.001 秒  [s]
  constexpr double kAccelerationTime = 1;      // 加速和减速所用的时间，设置为 1 秒, time spend accelerating and decelerating [s]
  constexpr double kConstantVelocityTime = 1;  // 匀速运动的持续时间，设置为 1 秒, time spend with constant speed [s]
  // obtained during the speed up
  // and slow down [rad/s^2]
  double a = 0;  // [rad/s^2] // 加速度
  double v = 0;  // [rad/s]   // 速度
  double t = 0;  // [s]       // 时间
  while (t < (2 * kAccelerationTime + kConstantVelocityTime)) {
    if (t <= kAccelerationTime) {// 加速
      a = pow(sin(t * M_PI / kAccelerationTime), 2) * a_max;
    } else if (t <= (kAccelerationTime + kConstantVelocityTime)) {
      a = 0;//匀速
    } else {// 减速
      const double deceleration_time =
          (kAccelerationTime + kConstantVelocityTime) - t;  // time spent in the deceleration phase
      a = -pow(sin(deceleration_time * M_PI / kAccelerationTime), 2) * a_max;
    }
    v += a * kTimeStep;
    t += kTimeStep;
    trajectory.push_back(v);
  }
  return trajectory;//循环结束后,返回生成的 trajectory 向量
}

}  // anonymous namespace

// 
void writeLogToFile(const std::vector<franka::Record>& log);

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

  // Parameters
  const size_t joint_number{3}; // 关节数量
  const size_t filter_size{5};  // 速度滤波器数量

  // NOLINTNEXTLINE(readability-identifier-naming)
  const std::array<double, 7> K_P{{200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0}};// PD控制器中 比例增益
  // NOLINTNEXTLINE(readability-identifier-naming)
  const std::array<double, 7> K_D{{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}};// PD控制器中 微分增益
  const double max_acceleration{1.0}; // 最大加速度

  Controller controller(filter_size, K_P, K_D); // 自定义控制器 根据机器人状态生成关节力距

  try {
    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}});

    size_t index = 0;
    std::vector<double> trajectory = generateTrajectory(max_acceleration);// 生成速度轨迹

    robot.control(
        [&](const franka::RobotState& robot_state, franka::Duration) -> franka::Torques {// 外部力距控制器
          return controller.step(robot_state);
        },
        [&](const franka::RobotState&, franka::Duration period) -> franka::JointVelocities {// 关节速度生成器
          index += period.toMSec();

          if (index >= trajectory.size()) {
            index = trajectory.size() - 1;
          }

          franka::JointVelocities velocities{{0, 0, 0, 0, 0, 0, 0}};
          velocities.dq[joint_number] = trajectory[index]; // 获取规划好的关节速度

          if (index >= trajectory.size() - 1) {
            return franka::MotionFinished(velocities);
          }
          return velocities;
        });
  } catch (const franka::ControlException& e) {
    std::cout << e.what() << std::endl;
    writeLogToFile(e.log);
    return -1;
  } catch (const franka::Exception& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }

  return 0;
}
/// @brief 输出日志
/// @param log 
void writeLogToFile(const std::vector<franka::Record>& log) {
  if (log.empty()) {
    return;
  }
  try {
    Poco::Path temp_dir_path(Poco::Path::temp());
    temp_dir_path.pushDirectory("libfranka-logs");

    Poco::File temp_dir(temp_dir_path);
    temp_dir.createDirectories();

    std::string now_string =
        Poco::DateTimeFormatter::format(Poco::Timestamp{}, "%Y-%m-%d-%h-%m-%S-%i");
    std::string filename = std::string{"log-" + now_string + ".csv"};
    Poco::File log_file(Poco::Path(temp_dir_path, filename));
    if (!log_file.createFile()) {
      std::cout << "Failed to write log file." << std::endl;
      return;
    }
    std::ofstream log_stream(log_file.path().c_str());
    log_stream << franka::logToCSV(log);

    std::cout << "Log file written to: " << log_file.path() << std::endl;
  } catch (...) {
    std::cout << "Failed to write log file." << std::endl;
  }
}

这段代码的主要功能包括：

• 使用关节速度运动生成器和力矩控制器控制机器人。
• 生成平滑的关节速度轨迹。
• 捕获并记录异常日志。

代码分为几个主要部分：

• Controller类：实现关节速度运动生成器和力矩控制。
• generateTrajectory函数：生成平滑的速度轨迹。
• main函数：程序入口，包含机器人控制逻辑。
• writeLogToFile函数：输出日志到文件。

 

代码详细总结
这段代码通过定义一个Controller类来实现关节速度运动生成器和力矩控制器。Controller类使用PD控制器公式来计算期望的关节力矩，并通过滤波器来平滑关节速度。

在main函数中，首先检查命令行参数并获取机器人主机名称。然后定义控制参数并创建Controller对象。通过连接机器人并设置默认行为，程序将机器人移动到适当的初始位置。接下来，设置碰撞行为并生成速度轨迹。最后，通过控制循环，使用关节速度生成器和力矩控制器来控制机器人。

如果在控制过程中出现异常，writeLogToFile函数会捕获并记录日志，便于后续分析和调试。

这段代码展示了如何使用Franka机器人库进行复杂的机器人控制任务，并通过日志记录机制提高系统的鲁棒性和可调试性。