这是本文档旧的修订版！
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
其中，franka_interface/src/franka_interface/arm.py 相关的代码，主要涉及到位姿的设置，看到可以支持不同的位姿模式
https://github.com/frankarobotics/franka_ros 镜像地址： https://gitee.com/ctbots/mirror-franka_ros
一个franka的fci 跟 ros 中间的桥接项目；
https://github.com/eric-heiden/panda-force 镜像地址： https://gitee.com/ctbots/mirror-panda-force
- experiments/echo_robot_state.cpp 可以看到对数据的保存，但是这里是对基本所有的字段的保存：experiments/state_recorder.hpp 可以看到用json几乎存储了所有数据
- experiments/zmq_control_server.cpp 文件可以看到支持的各种命令，录制和重放的模式；可以看到 也是提供2种重放功能：根据点位的(follow_qs)，根据加速度的(follow_cartesian_vel)。
| 命令 | 功能描述 | 参数 |
|---|---|---|
| connect | 连接到Franka机器人，建立与指定IP地址Franka机器人的连接，并加载机器人模型 | 机器人IP地址 |
| disconnect | 断开机器人连接，重置机器人连接指针，释放资源 | 无 |
| error_recovery | 尝试从机器人错误状态中恢复 | 无 |
| set_knife | 设置末端执行器（刀具）属性，根据配置文件设置不同刀具的末端执行器属性 | 刀具类型（“edc”或“slicing”） |
| get_knife | 获取当前选中的刀具，返回当前已设置的刀具类型 | 无 |
| set_joint_impedance | 配置机器人各关节的阻抗控制参数 | 7个关节的阻抗值数组 |
| set_collision_behavior | 配置机器人在碰撞情况下的响应行为 | 扭矩阈值和力阈值数组 |
| record | 以指定间隔记录指定数量的机器人状态数据，保存为JSON文件 | 步数、采样间隔(毫秒) |
| retrieve | 读取指定路径的JSON日志文件内容并返回 | 日志文件路径 |
| move_to_q | 控制机器人在指定时间内平滑移动到目标关节位置，可选记录运动过程 | 目标关节角度数组、移动时长、确认标志、记录标志、记录频率 |
| interpolate | 对给定的关节位置点进行插值，生成新的更密集或更稀疏的路点序列 | 路点数、关节角度数据、源时间步长、目标时间步长 |
| follow_qs | 根据给定的关节位置点序列和时间步长，控制机器人按照插值后的平滑轨迹运动 | 路点数、关节角度数据、源时间步长、记录频率 |
| follow_cartesian_vel | 根据给定的笛卡尔速度序列和时间步长，控制机器人执行相应的笛卡尔空间运动 | 路点数、笛卡尔速度数据、源时间步长、记录频率 |
https://github.com/nbfigueroa/easy-kinesthetic-recording 镜像地址： https://gitee.com/ctbots/mirror-easy-kinesthetic-recording
Motion的类型和具体源代码
我们后续，可能会主要考虑 使用 Joint Velocity 和 Cartesian Velocity 的方式进行录制和回放 ；
但是考虑到使用openvla进行配置，那么应该最推荐 Cartesian Velocity 的方式，毕竟 openvla 自己输出的就是 末端执行器的偏移速度，那么比较合适，不用再运动学计算。
| Motion Generator类型 | 文件位置 | 类名 | 应用场景 | 优势 | 劣势 |
|---|---|---|---|---|---|
| Joint Position | include/franka/control_types.h | JointPositions | 固定点位任务（搬运、分拣、装配） 精度要求高（精准定位） | 精度最高：可精确到达每个点 实现最简单：只需记录 7 个关节角度 数据量最小：每帧 7 个数据 计算快速：直接位置映射，无复杂计算 稳定可靠：误差界清晰 | 轨迹离散：点与点之间需要插值 运动不连贯：容易出现突变和震动 速度无法控制：重放时节奏不受控 环境适应性差：遇到障碍无法实时调整 不自然：不适合柔性操作 |
| Joint Velocity | include/franka/control_types.h | JointVelocities | 示教学习 工业工艺流程（喷涂、焊接） 需要平滑轨迹的场景 | 轨迹连续平滑：通过速度驱动，运动自然流畅 时间信息保留：速度包含了节奏信息 环境适应性好：可实时调整速度应对变化 能量效率高：减少急停急走 易于加速/减速：可以倍速或慢速重放 实现相对简单：只需记录 7 个速度值 数据量小：每帧 7 个数据 重放稳定性好：基于速度的连续控制更鲁棒 | 精度不如位置控制：可能有积累误差 对控制算法敏感：差的实现会产生偏差 需要校准：速度与实际轨迹的映射需要校准 长时间运行可能漂移：位置误差可能累积 |
| Cartesian Pose | include/franka/control_types.h | CartesianPose | 末端在空间中有严格约束的任务 工业装配、精密操作 | 末端精度高：可精确控制末端位置和姿态 直观：考虑末端的实际位置而非关节角度 易于规划：在末端空间规划轨迹 适合工业应用：许多工业任务在末端空间定义 | 数据量大：每帧 16 个数据（4×4 矩阵）实现复杂：需要正逆向运动学计算 计算效率低：需要求解逆向运动学 可能无解：某些末端位置可能无解 轨迹离散：点与点之间仍需插值 运行缓慢：计算负担大 |
| Cartesian Velocity | include/franka/control_types.h | CartesianVelocities | 末端轨迹录制/重放 连续的末端运动 | 连续平滑的末端运动 可实时调整末端速度 直观的末端轨迹控制 环境适应性好 | 需要理解 Twist（速度+角速度） 实现复杂性中等 可能需要额外的奇异点处理 |
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;
};
评论