libfranka——motion_with_control.cpp例程解析

motion_with_control.cpp例程解析

核心代码：

class Controller {
 public:
  /**
   * @brief Construct a new Controller object
   *
   * @param dq_filter_size 滤波器缓冲器大小
   * @param K_P PD控制参数
   * @param K_D PD控制参数
   */
  Controller(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)
      : 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);                      // set all dq_d_ zero
    dq_buffer_ = std::make_unique<double[]>(dq_filter_size_ * 7);  // dq缓冲器(生成智能指针)
    std::fill(&dq_buffer_.get()[0], &dq_buffer_.get()[dq_filter_size_ * 7],
              0);  // set all dq_buffer_ zero
  }

  // 内联函数
  /**
   * @brief 7个关节的力矩控制律(PD控制),返回关节力矩
   *
   * @param state 机器人状态
   * @return franka::Torques 关节力矩
   */
  inline franka::Torques step(const franka::RobotState& state) {
    updateDQFilter(state);

    std::array<double, 7> tau_J_d;  // NOLINT(readability-identifier-naming)
    // 7个关节的力矩控制律(PD控制)
    for (size_t i = 0; i < 7; i++) {
      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;
  }

  /**
   * @brief 更新DQ滤波器dq_buffer_内的数据
   *
   * @param state
   */
  void updateDQFilter(const franka::RobotState& state) {
    // 获取7个关节的速度
    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_;
  }

  /**
   * @brief 从dq_buffer_中求某一关节前5个时间的关节速度平均值
   *
   * @param index 关节索引,代表哪一个关节
   * @return double 关节速度平均值
   */
  double getDQFiltered(size_t index) const {
    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> 
 */
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;  // [s] 时间步长
  constexpr double kAccelerationTime = 1;  // time spend accelerating and decelerating [s] 加速和减速的时间
  constexpr double kConstantVelocityTime = 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;  // 减速阶段花费的时间
      a = -pow(sin(deceleration_time * M_PI / kAccelerationTime), 2) * a_max;
    }
    v += a * kTimeStep; // 速度为加速度对时间的积分
    t += kTimeStep; // 时间累加
    trajectory.push_back(v); // 轨迹生成
  }
  return trajectory;
}

提出问题：

• 这个例程实现了什么功能？——使用DQ滤波器对关节速度进行滤波，使得关节运动更加平稳。