差别

这里会显示出您选择的修订版和当前版本之间的差别。

到此差别页面的链接

后一修订版		前一修订版
机器人:franka:libfranka:libfranka的状态信息解析 [2025/11/10 05:44] – 创建 ctbots机器人:franka:libfranka:libfranka的状态信息解析 [2025/11/10 07:23] (当前版本) – [表] ctbots
行 1: 行 1:
-todo+====== libfranka的状态信息解析 ====== 
 + 
 +以下部分都是个人的见解，如果有错误的地方，欢迎指正 
 + 
 +franka的状态信息主要分为2块：机械臂本体的状态 和 末端执行器的状态。 
 + 
 +===== Franka机械臂本体的状态解析 ===== 
 + 
 +==== Franka 的机械臂相关原始代码 ==== 
 + 
 + 
 +打开libfranka项目，的 src/robot.cpp 文件，主要内容如下： 
 + 
 +<code c++  robot.h [enable_line_numbers="true"]> 
 +RobotState Robot::readOnce() { 
 +  std::unique_lock<std::mutex> control_lock(control_mutex_, std::try_to_lock); 
 +  assertOwningLock(control_lock); 
 + 
 +  return impl_->readOnce(); 
 +} 
 +</code> 
 + 
 + 
 +返回结果中，比较重要的几个属性： 
 +^ 列名     ^ 单位                                                        | 作用                                                                                                                                                                         | 
 +| 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代码| 
 +<code c++ robot.cpp [enable_line_numbers="true"]> 
 +# 返回的结构体中： 
 + 
 + 
 + 
 +// Copyright (c) 2024 Franka Robotics GmbH 
 +// Use of this source code is governed by the Apache-2.0 license, see LICENSE 
 +#pragma once 
 +#include <array> 
 +#include <ostream> 
 +#include <franka/duration.h> 
 +#include <franka/errors.h> 
 +/** 
 + * @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<double, 16> O_T_EE{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f${^OT_{EE}}_{d}\f$ 
 +   * 运动生成最后一次期望的末端执行器位姿，在 @ref o-frame "基坐标系" 中。 
 +   * 位姿以列主序的 4x4 矩阵表示。 
 +   */ 
 +  std::array<double, 16> 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<double, 16> 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<double, 16> 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<double, 16> NE_T_EE{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$^{EE}T_{K}\f$ 
 +   * 刚度坐标系在末端执行器坐标系中的位姿。 
 +   * 位姿以列主序的 4x4 矩阵表示。 
 +   * 
 +   * 另见 @ref k-frame "K 坐标系"。 
 +   */ 
 +  std::array<double, 16> EE_T_K{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$m_{EE}\f$ 
 +   * 配置的末端执行器质量。 
 +   */ 
 +  double m_ee{}; 
 +  /** 
 +   * \f$I_{EE}\f$ 
 +   * 配置的末端执行器负载相对于质心的转动惯量矩阵。 
 +   */ 
 +  std::array<double, 9> I_ee{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$^{F}x_{C_{EE}}\f$ 
 +   * 配置的末端执行器负载质心相对于法兰坐标系的位置。 
 +   */ 
 +  std::array<double, 3> F_x_Cee{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$m_{load}\f$ 
 +   * 配置的外部负载质量。 
 +   */ 
 +  double m_load{}; 
 +  /** 
 +   * \f$I_{load}\f$ 
 +   * 配置的外部负载相对于质心的转动惯量矩阵。 
 +   */ 
 +  std::array<double, 9> I_load{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$^{F}x_{C_{load}}\f$ 
 +   * 配置的外部负载质心相对于法兰坐标系的位置。 
 +   */ 
 +  std::array<double, 3> F_x_Cload{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$m_{total}\f$ 
 +   * 末端执行器与外部负载质量之和。 
 +   */ 
 +  double m_total{}; 
 +  /** 
 +   * \f$I_{total}\f$ 
 +   * 末端执行器负载与外部负载相对于质心的组合转动惯量矩阵。 
 +   */ 
 +  std::array<double, 9> I_total{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$^{F}x_{C_{total}}\f$ 
 +   * 末端执行器负载与外部负载相对于法兰坐标系的组合质心。 
 +   */ 
 +  std::array<double, 3> 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<double, 2> 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<double, 2> 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<double, 2> elbow_c{}; 
 +  /** 
 +   * 命令的肘部速度。 
 +   * 
 +   * 数组值的含义： 
 +   * - delbow_c[0]: 第3关节的速度，单位 \f$\frac{rad}{s}\f$ 
 +   * - delbow_c[1]: 始终为 0。 
 +   */ 
 +  std::array<double, 2> delbow_c{}; 
 +  /** 
 +   * 命令的肘部加速度。 
 +   * 
 +   * 数组值的含义： 
 +   * - ddelbow_c[0]: 第3关节的加速度，单位 \f$\frac{rad}{s^2}\f$ 
 +   * - ddelbow_c[1]: 始终为 0。 
 +   */ 
 +  std::array<double, 2> ddelbow_c{}; 
 +  /** 
 +   * \f$\tau_{J}\f$ 
 +   * 测量的关节侧力矩传感器信号。单位：\f$[Nm]\f$ 
 +   */ 
 +  std::array<double, 7> tau_J{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f${\tau_J}_d\f$ 
 +   * 期望的关节侧力矩传感器信号（不含重力）。单位：\f$[Nm]\f$ 
 +   */ 
 +  std::array<double, 7> tau_J_d{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$\dot{\tau_{J}}\f$ 
 +   * 测量的关节侧力矩传感器信号的导数。单位：\f$[\frac{Nm}{s}]\f$ 
 +   */ 
 +  std::array<double, 7> dtau_J{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$q\f$ 
 +   * 测量的关节位置。单位：\f$[rad]\f$ 
 +   */ 
 +  std::array<double, 7> q{}; 
 +  /** 
 +   * \f$q_d\f$ 
 +   * 期望的关节位置。单位：\f$[rad]\f$ 
 +   */ 
 +  std::array<double, 7> q_d{}; 
 +  /** 
 +   * \f$\dot{q}\f$ 
 +   * 测量的关节速度。单位：\f$[\frac{rad}{s}]\f$ 
 +   */ 
 +  std::array<double, 7> dq{}; 
 +  /** 
 +   * \f$\dot{q}_d\f$ 
 +   * 期望的关节速度。单位：\f$[\frac{rad}{s}]\f$ 
 +   */ 
 +  std::array<double, 7> dq_d{}; 
 +  /** 
 +   * \f$\ddot{q}_d\f$ 
 +   * 期望的关节加速度。单位：\f$[\frac{rad}{s^2}]\f$ 
 +   */ 
 +  std::array<double, 7> ddq_d{}; 
 +  /** 
 +   * 表示每个关节激活的接触等级。接触消失后值变为零。 
 +   * 
 +   * @see Robot::setCollisionBehavior 用于设置灵敏度值。 
 +   */ 
 +  std::array<double, 7> joint_contact{}; 
 +  /** 
 +   * 表示笛卡尔维度 \f$(x,y,z,R,P,Y)\f$ 中激活的接触等级。接触消失后值变为零。 
 +   * 
 +   * @see Robot::setCollisionBehavior 用于设置灵敏度值。 
 +   */ 
 +  std::array<double, 6> cartesian_contact{}; 
 +  /** 
 +   * 表示每个关节激活的接触等级。接触消失后值保持不变，直到发送复位命令。 
 +   * 
 +   * @see Robot::setCollisionBehavior 用于设置灵敏度值。 
 +   * @see Robot::automaticErrorRecovery 用于在碰撞后执行复位。 
 +   */ 
 +  std::array<double, 7> joint_collision{}; 
 +  /** 
 +   * 表示笛卡尔维度 \f$(x,y,z,R,P,Y)\f$ 中激活的接触等级。接触消失后值保持不变，直到发送复位命令。 
 +   * 
 +   * @see Robot::setCollisionBehavior 用于设置灵敏度值。 
 +   * @see Robot::automaticErrorRecovery 用于在碰撞后执行复位。 
 +   */ 
 +  std::array<double, 6> cartesian_collision{}; 
 +  /** 
 +   * \f$\hat{\tau}_{\text{ext}}\f$ 
 +   * 低通滤波后的外部力在关节上产生的力矩。不包括配置的末端执行器和负载以及机器人的质量和动力学。 
 +   * tau_ext_hat_filtered 是 tau_J 与机器人模型预期力矩之间的误差。单位：\f$[Nm]\f$。 
 +   */ 
 +  std::array<double, 7> 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<double, 6> 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<double, 6> 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<double, 6> O_dP_EE_d{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f${^OddP}_O\f$ 
 +   * 机器人基座加速度的线性分量，表达在与 @ref o-frame "基坐标系" 平行的坐标系中，即基座的平移加速度。 
 +   * 如果基座静止，则显示重力矢量方向。目前硬编码为 `{0, 0, -9.81}`。 
 +   */ 
 +  std::array<double, 3> O_ddP_O{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f${^OT_{EE}}_{c}\f$ 
 +   * 运动生成最后一次命令的末端执行器位姿，在 @ref o-frame "基坐标系" 中。 
 +   * 位姿以列主序的 4x4 矩阵表示。 
 +   */ 
 +  std::array<double, 16> 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<double, 6> 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<double, 6> O_ddP_EE_c{}; // NOLINT(readability-identifier-naming) 
 +  /** 
 +   * \f$\theta\f$ 
 +   * 电机位置。单位：\f$[rad]\f$ 
 +   */ 
 +  std::array<double, 7> theta{}; 
 +  /** 
 +   * \f$\dot{\theta}\f$ 
 +   * 电机速度。单位：\f$[\frac{rad}{s}]\f$ 
 +   */ 
 +  std::array<double, 7> 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 
 + 
 +</code> 
 +++++