{{htmlmetatags>metatag-robots=(index, follow) metatag-keywords=(libfranka, 机器人控制, Motion Model, Control Model, Franka机械臂, 关节位置控制, 关节速度控制, 笛卡尔姿态控制, 笛卡尔速度控制, openvla, 机器人运动仿真) metatag-description=(了解如何使用libfranka进行机器人控制数据的捕获和重放，包括不同类型的Motion Model和Control Model，及其应用场景和技术细节。) metatag-media-og:image=(:wiki:libfranka-motion-control.jpg) metatag-og:description=(探索libfranka库中实现的Motion Model和Control Model，掌握如何通过不同的控制模式录制和回放Franka机械臂的动作。) metatag-og:any=(获取深入的技术指南和开源项目链接，加速您在机器人编程领域的学习和发展。) }} ====== 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 * 依赖于https://github.com/nbfigueroa/franka_interactive_controllers，在ROS上发布了franka的节点的信息 * launch/franka_record_demonstrations.launch 录制并保存到本地 可视化 * launch/franka_replay_bag_demonstrations.launch 实现了录制数据的重放 ===== 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 q - 7 个关节的目标位置（弧度） 因为这种运动模式就是直接设定7个关节的最终位置 ++++ 展开代码| /** * 存储用于关节位置运动生成的值。 */ class JointPositions : public Finishable { public: /** * 创建一个新的JointPositions实例。 * * @param[in] joint_positions 期望的关节角度，单位为[rad]。 */ JointPositions(const std::array& joint_positions) noexcept; /** * 创建一个新的JointPositions实例。 * * @param[in] joint_positions 期望的关节角度，单位为[rad]。 * * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。 */ JointPositions(std::initializer_list joint_positions); /** * 期望的关节角度，单位为[rad]。 */ std::array q{}; }; ++++ ==== Joint Velocity ==== std::array dq - 7 个关节的目标速度（弧度/秒） ++++折叠代码| /** * 存储用于关节速度运动生成的值。 */ class JointVelocities : public Finishable { public: /** * 创建一个新的JointVelocities实例。 * * @param[in] joint_velocities 期望的关节速度，单位为[rad/s]。 * */ JointVelocities(const std::array& joint_velocities) noexcept; /** * 创建一个新的JointVelocities实例。 * * @param[in] joint_velocities 期望的关节速度，单位为[rad/s]。 * * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。 */ JointVelocities(std::initializer_list joint_velocities); /** * 期望的关节速度，单位为[rad/s]。 */ std::array dq{}; }; ++++ ==== Cartesian Pose ==== std::array O_T_EE - 末端执行器位置和姿态（4×4 齐次矩阵，列主序） 【实际上，可以用位置+欧拉角 直接推算出来】 std::array elbow - 肘部配置（利用 7 DOF 冗余性） ++++ 折叠代码| ```cpp /** * 存储用于笛卡尔姿态运动生成的值。 */ class CartesianPose : public Finishable { public: /** * 创建一个新的CartesianPose实例。 * * @param[in] cartesian_pose 期望的向量化齐次变换矩阵， * 列优先，从末端执行器坐标系转换到基座坐标系。 * 同样，它也是在基座坐标系中期望的末端执行器姿态。 */ CartesianPose(const std::array& cartesian_pose) noexcept; /** * 创建一个新的CartesianPose实例。 * * @param[in] cartesian_pose 期望的向量化齐次变换矩阵。 * @param[in] elbow 肘部配置（详见@ref elbow成员以获取更多细节）。 */ CartesianPose(const std::array& cartesian_pose, const std::array& elbow) noexcept; /** * 创建一个新的CartesianPose实例。 * * @param[in] cartesian_pose 期望的向量化齐次变换矩阵。 * * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。 */ CartesianPose(std::initializer_list cartesian_pose); /** * 创建一个新的CartesianPose实例。 * * @param[in] cartesian_pose 期望的向量化齐次变换矩阵。 * @param[in] elbow 肘部配置。 * * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。 */ CartesianPose(std::initializer_list cartesian_pose, std::initializer_list elbow); /** * 期望的末端执行器姿态（4x4齐次矩阵，列优先）。 */ std::array O_T_EE{}; /** * 肘部配置，用于冗余解析。 */ std::array elbow{}; /** * 检查是否指定了肘部配置。 */ bool hasElbow() const noexcept; }; ``` ++++ ==== Cartesian Velocity ==== std::array O_dP_EE - 笛卡尔速度（twist）：[v_x, v_y, v_z, ω_x, ω_y, ω_z] std::array 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& cartesian_velocities) noexcept; /** * 创建一个新的CartesianVelocities实例。 * * @param[in] cartesian_velocities 期望的笛卡尔速度。 * @param[in] elbow 肘部配置（详见@ref elbow成员以获取更多细节）。 */ CartesianVelocities(const std::array& cartesian_velocities, const std::array& elbow) noexcept; /** * 创建一个新的CartesianVelocities实例。 * * @param[in] cartesian_velocities 期望的笛卡尔速度。 * * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。 */ CartesianVelocities(std::initializer_list cartesian_velocities); /** * 创建一个新的CartesianVelocities实例。 * * @param[in] cartesian_velocities 期望的笛卡尔速度。 * @param[in] elbow 肘部配置。 * * @throw std::invalid_argument 如果给定的初始化列表参数数量无效，则抛出此异常。 */ CartesianVelocities(std::initializer_list cartesian_velocities, std::initializer_list elbow); /** * 基于基座坐标系O的笛卡尔速度，包含(v_x, v_y, v_z)以[m/s]为单位和(ω_x, ω_y, ω_z)以[rad/s]为单位。 */ std::array O_dP_EE{}; /** * 肘部配置。 */ std::array elbow{}; /** * 检查是否指定了肘部配置。 */ bool hasElbow() const noexcept; }; ++++