这是本文档旧的修订版！

1.libfranka的捕获和重放的技术路线

我们想要录制和重放，必须事先对基本概念有理解，不然可能是在乱搞。

libfranka的控制数据的基本概念

如下都是我个人的理解，如果有错误的表述，请评论更正。

我们先用白话的方式介绍 libfranka的控制链。

外部的操作 → FCI接口 → Motion Model → Control Model → 机械臂本体运动

其中 Motion Model 表示：我们的目标是什么？【实现：人与Franka的控制器互联】

Control Model 表示：我们应该让机械臂如何运动。【实现：Franka的控制器到具体关节的互联】

那么 Motion Model 主要有如下几种模式：

Motion Generator类型	作用
Joint Position	franka::JointPositions 关节位置轨迹
Joint Velocity	franka::JointVelocities 关节速度轨迹
Cartesian Position	franka::CartesianPositions 末端位置轨迹
Cartesian Velocity	franka::CartesianVelocities 末端速度轨迹

Motion我们可以看到，主要是联系Franka和人的操作意图。从操作目标上，就2类：不是控制末端就是控制关节；从操作方式上：不是通过加速度控制，就是通过位置直接控制。

Control 控制方式主要分为如下几种：

Control模式类型	描述
Joint Velocity Control	关节速度模式
Joint Position Control	关节位置模式
Joint Impedance Control	关节阻抗模式
Cartesian Impedance Control	笛卡尔/末端阻抗模式
Torque Control	扭矩控制模式

从 Motion到 Control的转换，我们能直接控制的部分是Motion部分，所以主要把注意力集中在这里；这块转换代码如果要看，主要集中在libfranka的 convertMotion函数

别人的项目是如何完成libfranka的位姿读取和重放控制的？

我们参考了一些开源的项目，主要清单如下【如果不好访问github，提供了加速地址】：

https://github.com/justagist/franka_ros_interface 镜像地址： https://gitee.com/ctbots/mirror-franka_ros_interface

https://github.com/frankarobotics/franka_ros 镜像地址： https://gitee.com/ctbots/mirror-franka_ros

https://github.com/eric-heiden/panda-force 镜像地址： https://gitee.com/ctbots/mirror-panda-force

https://github.com/nbfigueroa/easy-kinesthetic-recording 镜像地址： https://gitee.com/ctbots/mirror-easy-kinesthetic-recording

Motion的类型和具体源代码

Motion Generator类型	文件位置	类名
Joint Position	include/franka/control_types.h	JointPositions
Joint Velocity	include/franka/control_types.h	JointVelocities
Cartesian Pose	include/franka/control_types.h	CartesianPose
Cartesian Velocity	include/franka/control_types.h	CartesianVelocities

Joint Position

我们可以看到，主要关注 std::array<double, 7> q - 7 个关节的目标位置（弧度）

因为这种运动模式就是直接设定7个关节的最终位置

展开代码

/**
 * 存储用于关节位置运动生成的值。
 */
class JointPositions : public Finishable {
 public:
  /**
   * 创建一个新的JointPositions实例。
   *
   * @param[in] joint_positions 期望的关节角度，单位为[rad]。
   */
  JointPositions(const std::array<double, 7>& joint_positions) noexcept;

  /**
   * 创建一个新的JointPositions实例。
   *
   * @param[in] joint_positions 期望的关节角度，单位为[rad]。
   *
   * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。
   */
  JointPositions(std::initializer_list<double> joint_positions);

  /**
   * 期望的关节角度，单位为[rad]。
   */
  std::array<double, 7> q{};
};

Joint Velocity

std::array<double, 7> dq - 7 个关节的目标速度（弧度/秒）

折叠代码

/**
 * 存储用于关节速度运动生成的值。
 */
class JointVelocities : public Finishable {
 public:
  /**
   * 创建一个新的JointVelocities实例。
   *
   * @param[in] joint_velocities 期望的关节速度，单位为[rad/s]。
   *
   */
  JointVelocities(const std::array<double, 7>& joint_velocities) noexcept;

  /**
   * 创建一个新的JointVelocities实例。
   *
   * @param[in] joint_velocities 期望的关节速度，单位为[rad/s]。
   *
   * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。
   */
  JointVelocities(std::initializer_list<double> joint_velocities);

  /**
   * 期望的关节速度，单位为[rad/s]。
   */
  std::array<double, 7> dq{};
};

Cartesian Pose

std::array<double, 16> O_T_EE - 末端执行器位置和姿态（4×4 齐次矩阵，列主序）

std::array<double, 2> elbow - 肘部配置（利用 7 DOF 冗余性）

折叠代码

```cpp
/**
 * 存储用于笛卡尔姿态运动生成的值。
 */
class CartesianPose : public Finishable {
 public:
  /**
   * 创建一个新的CartesianPose实例。
   *
   * @param[in] cartesian_pose 期望的向量化齐次变换矩阵，
   * 列优先，从末端执行器坐标系转换到基座坐标系。
   * 同样，它也是在基座坐标系中期望的末端执行器姿态。
   */
  CartesianPose(const std::array<double, 16>& cartesian_pose) noexcept;

  /**
   * 创建一个新的CartesianPose实例。
   *
   * @param[in] cartesian_pose 期望的向量化齐次变换矩阵。
   * @param[in] elbow 肘部配置（详见@ref elbow成员以获取更多细节）。
   */
  CartesianPose(const std::array<double, 16>& cartesian_pose,
                const std::array<double, 2>& elbow) noexcept;

  /**
   * 创建一个新的CartesianPose实例。
   *
   * @param[in] cartesian_pose 期望的向量化齐次变换矩阵。
   *
   * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。
   */
  CartesianPose(std::initializer_list<double> cartesian_pose);

  /**
   * 创建一个新的CartesianPose实例。
   *
   * @param[in] cartesian_pose 期望的向量化齐次变换矩阵。
   * @param[in] elbow 肘部配置。
   *
   * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。
   */
  CartesianPose(std::initializer_list<double> cartesian_pose,
                std::initializer_list<double> elbow);

  /**
   * 期望的末端执行器姿态（4x4齐次矩阵，列优先）。
   */
  std::array<double, 16> O_T_EE{};

  /**
   * 肘部配置，用于冗余解析。
   */
  std::array<double, 2> elbow{};

  /**
   * 检查是否指定了肘部配置。
   */
  bool hasElbow() const noexcept;
};
```

Cartesian Velocity

std::array<double, 6> O_dP_EE - 笛卡尔速度（twist）：[v_x, v_y, v_z, ω_x, ω_y, ω_z]

std::array<double, 2> elbow - 肘部配置（可选）

折叠代码

/**
 * 存储用于笛卡尔速度运动生成的值。
 */
class CartesianVelocities : public Finishable {
 public:
  /**
   * 创建一个新的CartesianVelocities实例。
   *
   * @param[in] cartesian_velocities 基于基座坐标系O的期望笛卡尔速度，包含(v_x, v_y, v_z)以[m/s]为单位和(ω_x, ω_y, ω_z)以[rad/s]为单位。
   */
  CartesianVelocities(const std::array<double, 6>& cartesian_velocities) noexcept;

  /**
   * 创建一个新的CartesianVelocities实例。
   *
   * @param[in] cartesian_velocities 期望的笛卡尔速度。
   * @param[in] elbow 肘部配置（详见@ref elbow成员以获取更多细节）。
   */
  CartesianVelocities(const std::array<double, 6>& cartesian_velocities,
                      const std::array<double, 2>& elbow) noexcept;

  /**
   * 创建一个新的CartesianVelocities实例。
   *
   * @param[in] cartesian_velocities 期望的笛卡尔速度。
   *
   * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。
   */
  CartesianVelocities(std::initializer_list<double> cartesian_velocities);

  /**
   * 创建一个新的CartesianVelocities实例。
   *
   * @param[in] cartesian_velocities 期望的笛卡尔速度。
   * @param[in] elbow 肘部配置。
   *
   * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。
   */
  CartesianVelocities(std::initializer_list<double> cartesian_velocities,
                      std::initializer_list<double> elbow);

  /**
   * 基于基座坐标系O的笛卡尔速度，包含(v_x, v_y, v_z)以[m/s]为单位和(ω_x, ω_y, ω_z)以[rad/s]为单位。
   */
  std::array<double, 6> O_dP_EE{};

  /**
   * 肘部配置。
   */
  std::array<double, 2> elbow{};

  /**
   * 检查是否指定了肘部配置。
   */
  bool hasElbow() const noexcept;
};