libfranka的状态信息解析

以下部分都是个人的见解，如果有错误的地方，欢迎指正

franka的状态信息主要分为2块：机械臂本体的状态 和 末端执行器的状态。

打开libfranka项目，的 src/robot.cpp 文件，主要内容如下：

code_lang_cpp RobotState Robot::readOnce() {

std::unique_lock<std::mutex> control_lock(control_mutex_, std::try_to_lock);
assertOwningLock(control_lock);

return impl_->readOnce();

}

# 返回的结构体中：

Copyright © 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 “基坐标系” 中。
• 位姿以列主序的 4×4 矩阵表示。
• /

std::array<double, 16> O_T_EE{}; NOLINT(readability-identifier-naming) / * \f${^OT_{EE}}_{d}\f$ * 运动生成最后一次期望的末端执行器位姿，在 @ref o-frame “基坐标系” 中。 * 位姿以列主序的 4×4 矩阵表示。 */ std::array<double, 16> O_T_EE_d{}; NOLINT(readability-identifier-naming) / * \f$^{F}T_{EE}\f$ * 末端执行器坐标系在法兰坐标系中的位姿。 * 位姿以列主序的 4×4 矩阵表示。 * * @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$ * 标称末端执行器坐标系在法兰坐标系中的位姿。 * 位姿以列主序的 4×4 矩阵表示。 * * @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$ * 末端执行器坐标系在标称末端执行器坐标系中的位姿。 * 位姿以列主序的 4×4 矩阵表示。 * * @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$ * 刚度坐标系在末端执行器坐标系中的位姿。 * 位姿以列主序的 4×4 矩阵表示。 * * 另见 @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 “基坐标系” 中。 * 位姿以列主序的 4×4 矩阵表示。 */ 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