Franka例程学习——examples_common

这一次我们学习Franka所有例程里面都要调用的examples_common.h和examples_common.cpp，一个是.h头文件放置声明的函数、类、变量以及宏等内容，.c文件里面是具体的函数实现。

一、源代码

examples_common.h

cpp 复制代码

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once

#include <array>

#include <Eigen/Core>

#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/robot.h>
#include <franka/robot_state.h>

/**
 * @file examples_common.h
 * Contains common types and functions for the examples.
 */

/**
 * Sets a default collision behavior, joint impedance, Cartesian impedance, and filter frequency.
 *
 * @param[in] robot Robot instance to set behavior on.
 */
void setDefaultBehavior(franka::Robot& robot);

/**
 * An example showing how to generate a joint pose motion to a goal position. Adapted from:
 * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots
 * (Kogan Page Science Paper edition).
 */
class MotionGenerator {
 public:
  /**
   * Creates a new MotionGenerator instance for a target q.
   *
   * @param[in] speed_factor General speed factor in range [0, 1].
   * @param[in] q_goal Target joint positions.
   */
  MotionGenerator(double speed_factor, const std::array<double, 7> q_goal);

  /**
   * Sends joint position calculations
   *
   * @param[in] robot_state Current state of the robot.
   * @param[in] period Duration of execution.
   *
   * @return Joint positions for use inside a control loop.
   */
  franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period);

 private:
  using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
  using Vector7i = Eigen::Matrix<int, 7, 1, Eigen::ColMajor>;

  bool calculateDesiredValues(double t, Vector7d* delta_q_d) const;
  void calculateSynchronizedValues();

  static constexpr double kDeltaQMotionFinished = 1e-6;
  const Vector7d q_goal_;

  Vector7d q_start_;
  Vector7d delta_q_;

  Vector7d dq_max_sync_;
  Vector7d t_1_sync_;
  Vector7d t_2_sync_;
  Vector7d t_f_sync_;
  Vector7d q_1_;

  double time_ = 0.0;

  Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();
  Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
  Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
};

examples_common.cpp

cpp 复制代码

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include "examples_common.h"

#include <algorithm>
#include <array>
#include <cmath>

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

void setDefaultBehavior(franka::Robot& robot) {
  robot.setCollisionBehavior(
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
  robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
  robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
}

MotionGenerator::MotionGenerator(double speed_factor, const std::array<double, 7> q_goal)
    : q_goal_(q_goal.data()) {
  dq_max_ *= speed_factor;
  ddq_max_start_ *= speed_factor;
  ddq_max_goal_ *= speed_factor;
  dq_max_sync_.setZero();
  q_start_.setZero();
  delta_q_.setZero();
  t_1_sync_.setZero();
  t_2_sync_.setZero();
  t_f_sync_.setZero();
  q_1_.setZero();
}

bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();
  Vector7d t_d = t_2_sync_ - t_1_sync_;
  Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
  std::array<bool, 7> joint_motion_finished{};

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
      (*delta_q_d)[i] = 0;
      joint_motion_finished[i] = true;
    } else {
      if (t < t_1_sync_[i]) {
        (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
                          (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0);
      } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
        (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
      } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
        (*delta_q_d)[i] =
            delta_q_[i] + 0.5 *
                              (1.0 / std::pow(delta_t_2_sync[i], 3.0) *
                                   (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
                                   std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) +
                               (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
                              dq_max_sync_[i] * sign_delta_q[i];
      } else {
        (*delta_q_d)[i] = delta_q_[i];
        joint_motion_finished[i] = true;
      }
    }
  }
  return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
                     [](bool x) { return x; });
}

void MotionGenerator::calculateSynchronizedValues() {
  Vector7d dq_max_reach(dq_max_);
  Vector7d t_f = Vector7d::Zero();
  Vector7d delta_t_2 = Vector7d::Zero();
  Vector7d t_1 = Vector7d::Zero();
  Vector7d delta_t_2_sync = Vector7d::Zero();
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();

  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
      }
      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
    }
  }
  double max_t_f = t_f.maxCoeff();
  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
      double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
      double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
      double delta = b * b - 4.0 * a * c;
      if (delta < 0.0) {
        delta = 0.0;
      }
      dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
      t_f_sync_[i] =
          (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
      t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
      q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
    }
  }
}

franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state,
                                                   franka::Duration period) {
  time_ += period.toSec();

  if (time_ == 0.0) {
    q_start_ = Vector7d(robot_state.q_d.data());
    delta_q_ = q_goal_ - q_start_;
    calculateSynchronizedValues();
  }

  Vector7d delta_q_d;
  bool motion_finished = calculateDesiredValues(time_, &delta_q_d);

  std::array<double, 7> joint_positions;
  Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d);
  franka::JointPositions output(joint_positions);
  output.motion_finished = motion_finished;
  return output;
}

二、.h代码解读

examples_common.h文件包含以下内容

setDefaultBehavior 函数：设置机器人实例默认行为

MotionGenerator 类：用于生成机器人的关节位置轨迹

1.`setDefaultBehavior` 函数：

cpp 复制代码

void setDefaultBehavior(franka::Robot& robot);

该函数接受一个 franka::Robot 实例作为参数，并设置该机器人实例的默认行为。默认行为通常涉及冲突检测（collision behavior）、关节阻抗（joint impedance）、笛卡尔阻抗（Cartesian impedance）和滤波频率等参数。此函数在代码中并没有实现，但可以猜测它会通过调用 robot 的方法来设置这些控制参数。

2. MotionGenerator 类

这个类是核心部分，用于生成机器人的关节位置轨迹。目标是从当前关节位置（q_start_）生成一个平滑的运动轨迹，到达目标位置 q_goal_。

2.1 构造函数

cpp 复制代码

MotionGenerator(double speed_factor, const std::array&lt;double, 7&gt; q_goal);

speed_factor: 控制运动速度的因子，值范围为 [0, 1]。速度因子用于调节运动的快慢。
q_goal: 目标关节位置，存储了一个7维数组，每个值代表一个关节的目标位置。

该构造函数初始化了机器人运动所需的各种参数。q_goal_ 被设定为目标位置，q_start_ 和其他参数会在后续计算中动态更新。

2.2 operator() 运算符重载

cpp 复制代码

franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period);

该方法是类的核心方法之一，表示根据当前的机器人状态和执行时间，计算目标的关节位置。它接受以下参数：

robot_state: 当前机器人的状态，包含了关节的位置、速度等信息。
period: 执行的时间，控制当前计算步骤的时长。

返回值是一个 franka::JointPositions 类型，表示机器人的关节位置。在控制循环中，机器人的位置会持续更新。

2.3 私有成员

cpp 复制代码

  using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
  using Vector7i = Eigen::Matrix<int, 7, 1, Eigen::ColMajor>;

这些成员主要用于计算和控制目标运动轨迹。

Vector7d 是一个 7 维的向量，使用Eigen::Matrix<double, 7, 1>来表示机器人的关节位置和速度。

2.4 计算运动轨迹的辅助函数

cpp 复制代码

bool calculateDesiredValues(double t, Vector7d*_q_d) const;
void calculateSynchronizedValues();

calculateDesiredValues：该方法会根据时间 t 计算目标关节位置的变化量 _q_d，具体的计算方式可以依赖于不同的运动模型。
calculateSynchronizedValues：该方法计算一组与同步相关的参数，例如可能是加速度、速度、目标位置的变化等。

2.5 常量和默认值

cpp 复制代码

 static constexpr double kDeltaQMotionFinished = 1e-6;
  const Vector7d q_goal_;

  Vector7d q_start_;
  Vector7d delta_q_;

  Vector7d dq_max_sync_;
  Vector7d t_1_sync_;
  Vector7d t_2_sync_;
  Vector7d t_f_sync_;
  Vector7d q_1_;

  double time_ = 0.0;

  Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();
  Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
  Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();

kDeltaQMotionFinished 是一个常量，用于表示运动是否已经完成。如果关节位置之间的差异小于这个值，表示运动已经完成。
q_goal_: 目标关节位置，用户指定的目标位置。
q_start_: 起始关节位置，通过机器人当前状态来初始化。
dq_max_: 最大关节速度（单位：rad/s），在运动过程中可能会限制最大速度。
ddq_max_start_ 和 ddq_max_goal_: 最大加速度限制，用于控制起始和结束的加速度。

这段代码的主要目的是生成一个目标关节位置的运动轨迹。它通过 MotionGenerator 类和各种辅助函数来计算运动轨迹，并返回新的关节位置值以供控制循环使用。关键的计算涉及关节位置、速度、加速度等物理量的计算，以及确保运动平稳且在允许的范围内。

三、.c代码解读

3.1 `setDefaultBehavior` 函数

cpp 复制代码

void setDefaultBehavior(franka::Robot& robot) {
  robot.setCollisionBehavior(
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
  robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
  robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
}

该函数设置机器人的 碰撞行为 和 阻抗控制参数 。
- setCollisionBehavior 设置碰撞检测的参数，数值越高，机器人遇到障碍物时的反应越强。
- setJointImpedance 设置关节的阻抗控制，阻抗高则机器人的刚度更大。
- setCartesianImpedance 设置笛卡尔空间的阻抗，控制机器人末端执行器的刚度。

3.2 `MotionGenerator` 类的构造函数

cpp 复制代码

MotionGenerator::MotionGenerator(double speed_factor, const std::array<double, 7> q_goal)
    : q_goal_(q_goal.data()) {
  dq_max_ *= speed_factor;
  ddq_max_start_ *= speed_factor;
  ddq_max_goal_ *= speed_factor;
  dq_max_sync_.setZero();
  q_start_.setZero();
  delta_q_.setZero();
  t_1_sync_.setZero();
  t_2_sync_.setZero();
  t_f_sync_.setZero();
  q_1_.setZero();
}

MotionGenerator 类是用于生成机器人运动轨迹的类。
构造函数接收一个 速度因子 和目标关节位置 q_goal。
各种最大速度（dq_max_）和加速度（ddq_max_start_, ddq_max_goal_）根据 速度因子 调整，从而控制运动的快慢。
初始化了其他成员变量为零，包括起始位置、关节差异等。

3.3 `calculateDesiredValues` 函数

cpp 复制代码

bool MotionGenerator::calculateDesiredValues(double t, Vector7d*_q_d) const {
  // 计算各个关节的期望运动值（_q_d）...
}

calculateDesiredValues函数根据当前时间 t 和机器人每个关节的运动参数，计算出每个关节期望的位移变化量 _q_d。它通过平滑加速、匀速和减速阶段来保证机器人运动的流畅性，并且通过返回 true 或 false 来指示运动是否完成。这个函数对于精确控制机器人关节运动非常重要，确保了机器人能够平滑且精确地从初始位置移动到目标位置。

3.3.1 初始化和符号处理

cpp 复制代码

Vector7i sign_q;
sign_q <<_q_.cwiseSign().cast<int>();
Vector7d t_d = t_2_sync_ - t_1_sync_;
Vector7d_t_2_sync = t_f_sync_ - t_2_sync_;
std::array<bool, 7> joint_motion_finished{};

sign_q：计算 q（目标关节位置的变化量）中每个分量的符号（正或负），以便在计算过程中知道每个关节的运动方向。
t_d：这是每个关节的加速阶段时间和减速阶段时间之间的差异，表示"总的同步时间差"。
_t_2_sync：这表示从同步的减速阶段开始到最终的目标时间点之间的差值，用于调整每个关节的运动时间。
joint_motion_finished：这是一个布尔数组，表示每个关节的运动是否完成。

3.3.2 逐关节计算期望位移（_q_d）

cpp 复制代码

for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
      (*delta_q_d)[i] = 0;
      joint_motion_finished[i] = true;
    } else {
      if (t < t_1_sync_[i]) {
        (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
                          (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0);
      } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
        (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
      } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
        (*delta_q_d)[i] =
            delta_q_[i] + 0.5 *
                              (1.0 / std::pow(delta_t_2_sync[i], 3.0) *
                                   (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
                                   std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) +
                               (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
                              dq_max_sync_[i] * sign_delta_q[i];
      } else {
        (*delta_q_d)[i] = delta_q_[i];
        joint_motion_finished[i] = true;
      }
    }
  }

判断是否已经完成运动：如果 q[i]（目标位置变化量）小于 kDeltaQMotionFinished（表示已经接近目标位置），则认为该关节的运动完成，(*_q_d)[i] 置为 0，并将 joint_motion_finished[i] 设置为 true。
加速阶段（t < t_1_sync_[i]）：如果当前时间 t 处于加速阶段（即小于 t_1_sync_[i]），则根据加速度和最大速度计算当前关节的运动。具体采用三次多项式插值公式来平滑地加速。
匀速阶段（t_1_sync_[i] <= t < t_2_sync_[i]）：如果时间 t 处于匀速阶段（即在 t_1_sync_[i] 到 t_2_sync_[i] 之间），则关节的变化速度是恒定的，按照线性关系计算关节位移。
减速阶段（t_2_sync_[i] <= t < t_f_sync_[i]）：如果时间 t 处于减速阶段（即在 t_2_sync_[i] 到 t_f_sync_[i] 之间），则通过三次多项式插值来平滑地减速，使得关节的运动接近目标。
完成阶段：如果当前时间 t 超过了目标时间，表示关节运动已经完成，(*_q_d)[i] 设置为目标位置的变化量 q[i]，并将 joint_motion_finished[i] 设置为 true。

3.3.3 返回运动是否完成

cpp 复制代码

return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
                   [](bool x) { return x; });

最后，函数通过 std::all_of 来检查 joint_motion_finished 数组中的所有关节是否都完成了运动。如果所有关节的 joint_motion_finished 都是 true，则表示所有关节的运动已完成，返回 true；否则，返回 false。

3.4 `calculateSynchronizedValues` 函数

cpp 复制代码

void MotionGenerator::calculateSynchronizedValues() {
  // 计算运动的同步时长和参数
}

函数 **calculateSynchronizedValues()**计算了机器人每个关节在完成从当前位置到目标位置的运动时，如何根据速度和加速度的限制，计算各个关节的同步运动参数。特别地，它考虑了加速、减速和匀速运动的时间分配，并通过计算得到同步的最大速度、时间和位移，确保多个自由度的运动能够协调一致。这种计算方式对于需要精确控制多个关节的机器人系统是非常常见的，尤其是在关节数较多的情况下。

3.4.1 变量概述

cpp 复制代码

  Vector7d dq_max_reach(dq_max_);
  Vector7d t_f = Vector7d::Zero();
  Vector7d delta_t_2 = Vector7d::Zero();
  Vector7d t_1 = Vector7d::Zero();
  Vector7d delta_t_2_sync = Vector7d::Zero();
  Vector7i sign_delta_q;
  sign_delta_q << delta_q_.cwiseSign().cast<int>();

dq_max_: 最大速度（或最大关节速度）的向量（7维），表示每个关节的最大速度。
q: 目标角度变化量（7维），即每个关节从当前角度到目标角度的变化量。
ddq_max_start_ 和 ddq_max_goal_: 启动阶段和目标阶段的最大加速度（7维向量）。
kDeltaQMotionFinished: 用于判断是否已完成运动的小阈值。
dq_max_reach: 计算的"最大速度"值，代表在一定的加速度限制下，某个关节的最大可达到速度。
t_f, t_1,_t_2, t_1_sync,_t_2_sync, t_f_sync: 这些是与时间相关的参数，用来控制运动的时间分配，t_f 代表运动结束时的总时间，t_1 和_t_2 分别表示加速阶段和减速阶段的时间，t_f_sync 是同步版本的总时间，t_2_sync 和 q_1_ 与这些时间相关联，控制不同阶段的运动。
sign_q : 存储每个关节的目标运动方向，使用 cwiseSign() 计算每个角度变化的符号

3.4.2 计算最大速度 `dq_max_reach` 和总时间 `t_f`

cpp 复制代码

for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
      }
      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
    }
  }

该段代码首先检查每个关节的运动变化量（_q_），如果运动足够大（大于一个预定的阈值 kDeltaQMotionFinished），则计算该关节的最大速度 dq_max_reach。这个计算基于启动和目标阶段的最大加速度（ddq_max_start_ 和 ddq_max_goal_）。
t_1 和 _t_2 分别是加速阶段和减速阶段所需的时间。这里的 1.5 是与加速度和速度关系的系数，通常是为了保证运动平滑。
运动的总时间 t_f 是加速阶段、减速阶段和匀速阶段的时间的总和。

3.4.3 计算同步时间 `t_f_sync` 和同步速度 `dq_max_sync`

cpp 复制代码

  double max_t_f = t_f.maxCoeff();
  for (size_t i = 0; i < 7; i++) {
    if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
      double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
      double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
      double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
      double delta = b * b - 4.0 * a * c;
      if (delta < 0.0) {
        delta = 0.0;
      }
      dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
      t_f_sync_[i] =
          (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
      t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
      q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
    }
  }

计算出最大总时间 max_t_f，然后为每个关节计算同步的最大速度 dq_max_sync_ 和同步的时间参数。这里通过解一个二次方程来计算同步速度 dq_max_sync_，目的是使得所有关节的运动在相同的总时间内完成，保持运动的协调性。
之后，计算加速阶段（t_1_sync_）、减速阶段（_t_2_sync）的同步时间，以及整个同步运动的总时间 t_f_sync_。
最后，q_1_ 计算的是在加速阶段某一时刻的位移，用来确保在同步过程中每个关节的运动量是协调的。

3.5 `operator()` 重载

这段代码的核心是根据机器人的当前状态和目标位置，计算出一个目标关节位置，并返回给调用者。它会：

跟踪时间，更新机器人的运动状态。

在首次调用时记录初始状态并计算目标位置的变化量。

根据时间计算期望的关节位置，并返回计算出的目标关节位置。

使用 motion_finished 标志指示是否完成运动。

**operator()**的功能是动态地生成机器人运动轨迹，以便机器人每次调用时都能得到适当的目标关节位置，进而进行连续的运动控制。

3.5.1 更新当前时间 `time_`

cpp 复制代码

time_ += period.toSec();

3.5.2 初始化开始的关节位置 `q_start_` 和计算关节变化量 `_q_`

cpp 复制代码

if (time_ == 0.0) {
    q_start_ = Vector7d(robot_state.q_d.data());
   _q_ = q_goal_ - q_start_;
    calculateSynchronizedValues();
}

如果 time_ == 0.0（意味着是第一次调用或新的运动开始），首先记录机器人的当前关节位置（robot_state.q_d 是机器人当前的关节位置），并将其存储在 q_start_ 中，q_start_ 是一个 Vector7d 类型的向量，表示机器人每个关节的初始位置（7个自由度）。
q_goal_ 是目标位置，它是在其他地方定义的目标关节位置。_q_ 计算为目标位置与当前起始位置的差值，用来表示目标关节位置的变化量。
然后调用 calculateSynchronizedValues() 来计算同步的速度、加速度等参数，以确保在多个关节间进行协调运动。

3.5.3 计算期望的关节变化量 `_q_d`

cpp 复制代码

Vector7d_q_d;
bool motion_finished = calculateDesiredValues(time_, &_q_d);

calculateDesiredValues(time_, &_q_d) 用于计算当前时间 time_ 对应的期望关节变化量 _q_d。_q_d 是机器人从当前关节位置到目标关节位置的期望变化量。
calculateDesiredValues 可能会基于当前时间、加速度、速度限制等参数计算期望的运动轨迹，并将期望的变化量存储在 _q_d 中。返回值 motion_finished 是一个布尔值，表示运动是否完成，通常这个值是根据是否已经接近目标位置来判断的。

3.5.4 更新关节位置并创建 `franka::JointPositions` 对象

cpp 复制代码

std::array<double, 7> joint_positions;
Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ +_q_d);
franka::JointPositions output(joint_positions);

joint_positions 是一个存储7个关节位置的 std::array<double, 7> 数组，用来保存计算后的目标关节位置。
Eigen::VectorXd::Map(&joint_positions[0], 7) 将 Eigen::VectorXd 类型的向量 q_start_ +_q_d 映射到 joint_positions 数组中。q_start_ +_q_d 是当前起始关节位置加上期望的变化量，表示期望的关节位置。
franka::JointPositions output(joint_positions) 创建了一个 franka::JointPositions 对象 output，它包含了目标的关节位置。

3.5.5 设置 `motion_finished` 标志

cpp 复制代码

output.motion_finished = motion_finished;

将 motion_finished 标志（表示运动是否完成）赋值给 output 对象。motion_finished 是由 calculateDesiredValues 函数返回的布尔值，指示是否完成了整个运动。

Franka例程学习——examples_common

一、源代码

二、.h代码解读

1.setDefaultBehavior 函数：

2. MotionGenerator 类

2.1 构造函数

2.2 operator() 运算符重载

2.3 私有成员

2.4 计算运动轨迹的辅助函数

2.5 常量和默认值

三、.c代码解读

3.1 setDefaultBehavior 函数

3.2 MotionGenerator 类的构造函数

3.3 calculateDesiredValues 函数

3.3.1 初始化和符号处理

3.3.2 逐关节计算期望位移（_q_d）

3.3.3 返回运动是否完成

3.4 calculateSynchronizedValues 函数

3.4.1 变量概述

3.4.2 计算最大速度 dq_max_reach 和总时间 t_f

3.4.3 计算同步时间 t_f_sync 和同步速度 dq_max_sync

3.5 operator() 重载

3.5.1 更新当前时间 time_

3.5.2 初始化开始的关节位置 q_start_ 和计算关节变化量 _q_

3.5.3 计算期望的关节变化量 _q_d

3.5.4 更新关节位置并创建 franka::JointPositions 对象

3.5.5 设置 motion_finished 标志

1.`setDefaultBehavior` 函数：

3.1 `setDefaultBehavior` 函数

3.2 `MotionGenerator` 类的构造函数

3.3 `calculateDesiredValues` 函数

3.4 `calculateSynchronizedValues` 函数

3.4.2 计算最大速度 `dq_max_reach` 和总时间 `t_f`

3.4.3 计算同步时间 `t_f_sync` 和同步速度 `dq_max_sync`

3.5 `operator()` 重载

3.5.1 更新当前时间 `time_`

3.5.2 初始化开始的关节位置 `q_start_` 和计算关节变化量 `_q_`

3.5.3 计算期望的关节变化量 `_q_d`

3.5.4 更新关节位置并创建 `franka::JointPositions` 对象

3.5.5 设置 `motion_finished` 标志