====== libfranka的状态信息解析 ====== 以下部分都是个人的见解，如果有错误的地方，欢迎指正 franka的状态信息主要分为2块：机械臂本体的状态 和 末端执行器的状态。 ===== Franka机械臂本体的状态解析 ===== ==== Franka 的机械臂相关原始代码 ==== 打开libfranka项目，的 src/robot.cpp 文件，主要内容如下： RobotState Robot::readOnce() { std::unique_lock control_lock(control_mutex_, std::try_to_lock); assertOwningLock(control_lock); return impl_->readOnce(); } 返回结果中，比较重要的几个属性： ^ 列名 ^ 单位 | 作用 | | q | 7自由度的double\\ 单位是弧度rad\\ 每个关节的弧度范围有限制 | 当前机械臂的关节实际测量得到的数据。\\ 【注意】这里代表的是实际位置 | | q_d | 7自由度的double\\ 单位是弧度rad\\ 每个关节的弧度范围有限制 | 期望应该到达的位置。\\ 【注意】这里是期望 | | dq | 数组，长度7\\ 单位是弧度每秒rad/s | 当前的机械臂的运行速度\\ 【注意】这里代表的是实际速度 | | dq_d | 数组，长度7\\ 单位是弧度每秒rad/s | 期望应该达到的运行速度\\ 【注意】这里是期望 | | tau_J | 数组，长度7\\ 单位是牛米Nm | 测量到的连杆侧关节力矩传感器信号\\ 【注意】这里代表的是实际力矩 | | tau_J_d | 数组，长度7\\ 单位是牛米Nm | 不含重力项的期望连杆侧关节力矩传感器信号\\ 【注意】这里是期望;\\ 控制系统计算得出的理想力矩参考值，但这个值已经去除了重力的影响，主要用于力矩控制算法中的参考信号和比较基准 | | time | 单位Duration | 递增的时间耗时，用来标定数据发生时刻 | ++++ 折叠robot.cpp代码| # 返回的结构体中： // Copyright (c) 2024 Franka Robotics GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE #pragma once #include #include #include #include /** * @file robot_state.h * 包含 franka::RobotState 类型定义。 */ namespace franka { /** * 描述机器人当前的模式。 */ enum class RobotMode { kOther, kIdle, kMove, kGuiding, kReflex, kUserStopped, kAutomaticErrorRecovery }; /** * 描述机器人的状态。 */ struct RobotState { /** * \f$^{O}T_{EE}\f$ * 测量的末端执行器位姿，在 @ref o-frame "基坐标系" 中。 * 位姿以列主序的 4x4 矩阵表示。 */ std::array O_T_EE{}; // NOLINT(readability-identifier-naming) /** * \f${^OT_{EE}}_{d}\f$ * 运动生成最后一次期望的末端执行器位姿，在 @ref o-frame "基坐标系" 中。 * 位姿以列主序的 4x4 矩阵表示。 */ std::array O_T_EE_d{}; // NOLINT(readability-identifier-naming) /** * \f$^{F}T_{EE}\f$ * 末端执行器坐标系在法兰坐标系中的位姿。 * 位姿以列主序的 4x4 矩阵表示。 * * @see F_T_NE * @see NE_T_EE * @see Robot 了解 F、NE 和 EE 坐标系的说明。 */ std::array F_T_EE{}; // NOLINT(readability-identifier-naming) /** * \f$^{F}T_{NE}\f$ * 标称末端执行器坐标系在法兰坐标系中的位姿。 * 位姿以列主序的 4x4 矩阵表示。 * * @see F_T_EE * @see NE_T_EE * @see Robot 了解 F、NE 和 EE 坐标系的说明。 */ std::array F_T_NE{}; // NOLINT(readability-identifier-naming) /** * \f$^{NE}T_{EE}\f$ * 末端执行器坐标系在标称末端执行器坐标系中的位姿。 * 位姿以列主序的 4x4 矩阵表示。 * * @see Robot::setEE 用于更改此坐标系。 * @see F_T_EE * @see F_T_NE * @see Robot 了解 F、NE 和 EE 坐标系的说明。 */ std::array NE_T_EE{}; // NOLINT(readability-identifier-naming) /** * \f$^{EE}T_{K}\f$ * 刚度坐标系在末端执行器坐标系中的位姿。 * 位姿以列主序的 4x4 矩阵表示。 * * 另见 @ref k-frame "K 坐标系"。 */ std::array EE_T_K{}; // NOLINT(readability-identifier-naming) /** * \f$m_{EE}\f$ * 配置的末端执行器质量。 */ double m_ee{}; /** * \f$I_{EE}\f$ * 配置的末端执行器负载相对于质心的转动惯量矩阵。 */ std::array I_ee{}; // NOLINT(readability-identifier-naming) /** * \f$^{F}x_{C_{EE}}\f$ * 配置的末端执行器负载质心相对于法兰坐标系的位置。 */ std::array F_x_Cee{}; // NOLINT(readability-identifier-naming) /** * \f$m_{load}\f$ * 配置的外部负载质量。 */ double m_load{}; /** * \f$I_{load}\f$ * 配置的外部负载相对于质心的转动惯量矩阵。 */ std::array I_load{}; // NOLINT(readability-identifier-naming) /** * \f$^{F}x_{C_{load}}\f$ * 配置的外部负载质心相对于法兰坐标系的位置。 */ std::array F_x_Cload{}; // NOLINT(readability-identifier-naming) /** * \f$m_{total}\f$ * 末端执行器与外部负载质量之和。 */ double m_total{}; /** * \f$I_{total}\f$ * 末端执行器负载与外部负载相对于质心的组合转动惯量矩阵。 */ std::array I_total{}; // NOLINT(readability-identifier-naming) /** * \f$^{F}x_{C_{total}}\f$ * 末端执行器负载与外部负载相对于法兰坐标系的组合质心。 */ std::array F_x_Ctotal{}; // NOLINT(readability-identifier-naming) /** * 肘部配置。 * * 数组值的含义： * - elbow[0]: 第3关节的位置，单位 \f$[rad]\f$。 * - elbow[1]: 肘部（第4关节）的翻转方向： * - +1 如果 \f$q_4 > q_{elbow-flip}\f$ * - 0 如果 \f$q_4 == q_{elbow-flip} \f$ * - -1 如果 \f$q_4 < q_{elbow-flip} \f$ * 。 * 其中 \f$q_{elbow-flip}\f$ 在 FCI 文档的机器人接口规范页面中定义。 */ std::array elbow{}; /** * 期望的肘部配置。 * * 数组值的含义： * - elbow_d[0]: 第3关节的位置，单位 \f$[rad]\f$。 * - elbow_d[1]: 肘部（第4关节）的翻转方向： * - +1 如果 \f$q_4 > q_{elbow-flip}\f$ * - 0 如果 \f$q_4 == q_{elbow-flip} \f$ * - -1 如果 \f$q_4 < q_{elbow-flip} \f$ * 。 * 其中 \f$q_{elbow-flip}\f$ 在 FCI 文档的机器人接口规范页面中定义。 */ std::array elbow_d{}; /** * 命令的肘部配置。 * * 数组值的含义： * - elbow_c[0]: 第3关节的位置，单位 \f$[rad]\f$。 * - elbow_c[1]: 肘部（第4关节）的翻转方向： * - +1 如果 \f$q_4 > q_{elbow-flip}\f$ * - 0 如果 \f$q_4 == q_{elbow-flip} \f$ * - -1 如果 \f$q_4 < q_{elbow-flip} \f$ * 。 * 其中 \f$q_{elbow-flip}\f$ 在 FCI 文档的机器人接口规范页面中定义。 */ std::array elbow_c{}; /** * 命令的肘部速度。 * * 数组值的含义： * - delbow_c[0]: 第3关节的速度，单位 \f$\frac{rad}{s}\f$ * - delbow_c[1]: 始终为 0。 */ std::array delbow_c{}; /** * 命令的肘部加速度。 * * 数组值的含义： * - ddelbow_c[0]: 第3关节的加速度，单位 \f$\frac{rad}{s^2}\f$ * - ddelbow_c[1]: 始终为 0。 */ std::array ddelbow_c{}; /** * \f$\tau_{J}\f$ * 测量的关节侧力矩传感器信号。单位：\f$[Nm]\f$ */ std::array tau_J{}; // NOLINT(readability-identifier-naming) /** * \f${\tau_J}_d\f$ * 期望的关节侧力矩传感器信号（不含重力）。单位：\f$[Nm]\f$ */ std::array tau_J_d{}; // NOLINT(readability-identifier-naming) /** * \f$\dot{\tau_{J}}\f$ * 测量的关节侧力矩传感器信号的导数。单位：\f$[\frac{Nm}{s}]\f$ */ std::array dtau_J{}; // NOLINT(readability-identifier-naming) /** * \f$q\f$ * 测量的关节位置。单位：\f$[rad]\f$ */ std::array q{}; /** * \f$q_d\f$ * 期望的关节位置。单位：\f$[rad]\f$ */ std::array q_d{}; /** * \f$\dot{q}\f$ * 测量的关节速度。单位：\f$[\frac{rad}{s}]\f$ */ std::array dq{}; /** * \f$\dot{q}_d\f$ * 期望的关节速度。单位：\f$[\frac{rad}{s}]\f$ */ std::array dq_d{}; /** * \f$\ddot{q}_d\f$ * 期望的关节加速度。单位：\f$[\frac{rad}{s^2}]\f$ */ std::array ddq_d{}; /** * 表示每个关节激活的接触等级。接触消失后值变为零。 * * @see Robot::setCollisionBehavior 用于设置灵敏度值。 */ std::array joint_contact{}; /** * 表示笛卡尔维度 \f$(x,y,z,R,P,Y)\f$ 中激活的接触等级。接触消失后值变为零。 * * @see Robot::setCollisionBehavior 用于设置灵敏度值。 */ std::array cartesian_contact{}; /** * 表示每个关节激活的接触等级。接触消失后值保持不变，直到发送复位命令。 * * @see Robot::setCollisionBehavior 用于设置灵敏度值。 * @see Robot::automaticErrorRecovery 用于在碰撞后执行复位。 */ std::array joint_collision{}; /** * 表示笛卡尔维度 \f$(x,y,z,R,P,Y)\f$ 中激活的接触等级。接触消失后值保持不变，直到发送复位命令。 * * @see Robot::setCollisionBehavior 用于设置灵敏度值。 * @see Robot::automaticErrorRecovery 用于在碰撞后执行复位。 */ std::array cartesian_collision{}; /** * \f$\hat{\tau}_{\text{ext}}\f$ * 低通滤波后的外部力在关节上产生的力矩。不包括配置的末端执行器和负载以及机器人的质量和动力学。 * tau_ext_hat_filtered 是 tau_J 与机器人模型预期力矩之间的误差。单位：\f$[Nm]\f$。 */ std::array tau_ext_hat_filtered{}; /** * \f$^OF_{K,\text{ext}}\f$ * 估计的作用在刚度坐标系上的外部扳手（力、力矩），相对于 @ref o-frame "基坐标系" 表示。 * 机器人施加给环境的力为正，环境施加给机器人的力为负。 * 在接近或处于奇异点时变为 \f$[0,0,0,0,0,0]\f$。另见 @ref k-frame "刚度坐标系 K"。 * 单位：\f$[N,N,N,Nm,Nm,Nm]\f$。 */ std::array O_F_ext_hat_K{}; // NOLINT(readability-identifier-naming) /** * \f$^{K}F_{K,\text{ext}}\f$ * 估计的作用在刚度坐标系上的外部扳手（力、力矩），相对于刚度坐标系表示。 * 机器人施加给环境的力为正，环境施加给机器人的力为负。 * 在接近或处于奇异点时变为 \f$[0,0,0,0,0,0]\f$。另见 @ref k-frame "刚度坐标系 K"。 * 单位：\f$[N,N,N,Nm,Nm,Nm]\f$。 */ std::array K_F_ext_hat_K{}; // NOLINT(readability-identifier-naming) /** * \f${^OdP_{EE}}_{d}\f$ * 期望的末端执行器扭转，在 @ref o-frame "基坐标系" 中。 * 单位：\f$[\frac{m}{s},\frac{m}{s},\frac{m}{s},\frac{rad}{s},\frac{rad}{s},\frac{rad}{s}]\f$。 */ std::array O_dP_EE_d{}; // NOLINT(readability-identifier-naming) /** * \f${^OddP}_O\f$ * 机器人基座加速度的线性分量，表达在与 @ref o-frame "基坐标系" 平行的坐标系中，即基座的平移加速度。 * 如果基座静止，则显示重力矢量方向。目前硬编码为 `{0, 0, -9.81}`。 */ std::array O_ddP_O{}; // NOLINT(readability-identifier-naming) /** * \f${^OT_{EE}}_{c}\f$ * 运动生成最后一次命令的末端执行器位姿，在 @ref o-frame "基坐标系" 中。 * 位姿以列主序的 4x4 矩阵表示。 */ std::array O_T_EE_c{}; // NOLINT(readability-identifier-naming) /** * \f${^OdP_{EE}}_{c}\f$ * 最后一次命令的末端执行器扭转，在 @ref o-frame "基坐标系" 中。 * 单位：\f$[\frac{m}{s},\frac{m}{s},\frac{m}{s},\frac{rad}{s},\frac{rad}{s},\frac{rad}{s}]\f$。 */ std::array O_dP_EE_c{}; // NOLINT(readability-identifier-naming) /** * \f${^OddP_{EE}}_{c}\f$ * 最后一次命令的末端执行器加速度，在 @ref o-frame "基坐标系" 中。 * 单位： * \f$[\frac{m}{s^2},\frac{m}{s^2},\frac{m}{s^2},\frac{rad}{s^2},\frac{rad}{s^2},\frac{rad}{s^2}]\f$。 */ std::array O_ddP_EE_c{}; // NOLINT(readability-identifier-naming) /** * \f$\theta\f$ * 电机位置。单位：\f$[rad]\f$ */ std::array theta{}; /** * \f$\dot{\theta}\f$ * 电机速度。单位：\f$[\frac{rad}{s}]\f$ */ std::array dtheta{}; /** * 当前错误状态。 */ Errors current_errors{}; /** * 包含导致上一次运动中止的错误。 */ Errors last_motion_errors{}; /** * 最近 100 个控制命令被机器人成功接收的百分比。 * * 如果当前没有运行控制或运动生成循环，则显示为零。 * * 范围：\f$[0, 1]\f$。 */ double control_command_success_rate{}; /** * 当前机器人模式。 */ RobotMode robot_mode = RobotMode::kUserStopped; /** * 自机器人启动以来严格单调递增的时间戳。 * * 在控制循环内部，可以使用 @ref callback-docs "Robot::control 的 time_step 参数" 代替。 */ Duration time{}; }; /** * 将机器人状态以 JSON 对象形式流输出：{"field_name_1": [0,0,0,0,0,0,0], "field_name_2": * [0,0,0,0,0,0], ...} * * @param[in] ostream Ostream 实例 * @param[in] robot_state 要输出的 RobotState 实例 * * @return Ostream 实例 */ std::ostream& operator<<(std::ostream& ostream, const franka::RobotState& robot_state); /** * 以人类可读形式输出 RobotMode * @param[in] ostream Ostream 实例 * @param[in] robot_mode 要输出的 RobotMode * * @return Ostream 实例 */ std::ostream& operator<<(std::ostream& ostream, RobotMode robot_mode); } // namespace franka ++++