**这是本文档旧的修订版！**
libfranka的状态信息解析
以下部分都是个人的见解，如果有错误的地方，欢迎指正
franka的状态信息主要分为2块：机械臂本体的状态 和 末端执行器的状态。
Franka机械臂本体的状态解析
Franka 的机械臂相关原始代码
打开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 robotstate.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> OTEE{}; NOLINT(readability-identifier-naming)
/
* \f${^OT{EE}}{d}\f$
* 运动生成最后一次期望的末端执行器位姿，在 @ref o-frame “基坐标系” 中。
* 位姿以列主序的 4×4 矩阵表示。
/
std::array<double, 16> OTEEd{}; NOLINT(readability-identifier-naming)
/
* \f$^{F}T{EE}\f$
* 末端执行器坐标系在法兰坐标系中的位姿。
* 位姿以列主序的 4×4 矩阵表示。
* @see FTNE
* @see NETEE
* @see Robot 了解 F、NE 和 EE 坐标系的说明。
*/
std::array<double, 16> FTEE{}; NOLINT(readability-identifier-naming)
/
* \f$^{F}T{NE}\f$
* 标称末端执行器坐标系在法兰坐标系中的位姿。
* 位姿以列主序的 4×4 矩阵表示。
* @see FTEE
* @see NETEE
* @see Robot 了解 F、NE 和 EE 坐标系的说明。
*/
std::array<double, 16> FTNE{}; NOLINT(readability-identifier-naming)
/
* \f$^{NE}T{EE}\f$
* 末端执行器坐标系在标称末端执行器坐标系中的位姿。
* 位姿以列主序的 4×4 矩阵表示。
* @see Robot::setEE 用于更改此坐标系。
* @see FTEE
* @see FTNE
* @see Robot 了解 F、NE 和 EE 坐标系的说明。
*/
std::array<double, 16> NETEE{}; NOLINT(readability-identifier-naming)
/
* \f$^{EE}T{K}\f$
* 刚度坐标系在末端执行器坐标系中的位姿。
* 位姿以列主序的 4×4 矩阵表示。
* 另见 @ref k-frame “K 坐标系”。
*/
std::array<double, 16> EETK{}; NOLINT(readability-identifier-naming)
/
* \f$m{EE}\f$
* 配置的末端执行器质量。
/
double mee{};
/
* \f$I{EE}\f$
* 配置的末端执行器负载相对于质心的转动惯量矩阵。
/
std::array<double, 9> Iee{}; NOLINT(readability-identifier-naming)
/
* \f$^{F}x{C{EE}}\f$
* 配置的末端执行器负载质心相对于法兰坐标系的位置。
/
std::array<double, 3> FxCee{}; NOLINT(readability-identifier-naming)
/
* \f$m{load}\f$
* 配置的外部负载质量。
/
double mload{};
/
* \f$I{load}\f$
* 配置的外部负载相对于质心的转动惯量矩阵。
/
std::array<double, 9> Iload{}; NOLINT(readability-identifier-naming)
/
* \f$^{F}x{C{load}}\f$
* 配置的外部负载质心相对于法兰坐标系的位置。
/
std::array<double, 3> FxCload{}; NOLINT(readability-identifier-naming)
/
* \f$m{total}\f$
* 末端执行器与外部负载质量之和。
/
double mtotal{};
/
* \f$I{total}\f$
* 末端执行器负载与外部负载相对于质心的组合转动惯量矩阵。
/
std::array<double, 9> Itotal{}; NOLINT(readability-identifier-naming)
/
* \f$^{F}x{C{total}}\f$
* 末端执行器负载与外部负载相对于法兰坐标系的组合质心。
/
std::array<double, 3> FxCtotal{}; NOLINT(readability-identifier-naming)
/
* 肘部配置。
* 数组值的含义：
* - elbow[0]: 第3关节的位置，单位 \f$[rad]\f$。
* - elbow[1]: 肘部（第4关节）的翻转方向：
* - +1 如果 \f$q4 > q{elbow-flip}\f$
* - 0 如果 \f$q4 == q{elbow-flip} \f$
* - -1 如果 \f$q4 < q{elbow-flip} \f$
* 。
* 其中 \f$q{elbow-flip}\f$ 在 FCI 文档的机器人接口规范页面中定义。
/
std::array<double, 2> elbow{};
/
* 期望的肘部配置。
* 数组值的含义：
* - elbowd[0]: 第3关节的位置，单位 \f$[rad]\f$。
* - elbowd[1]: 肘部（第4关节）的翻转方向：
* - +1 如果 \f$q4 > q{elbow-flip}\f$
* - 0 如果 \f$q4 == q{elbow-flip} \f$
* - -1 如果 \f$q4 < q{elbow-flip} \f$
* 。
* 其中 \f$q{elbow-flip}\f$ 在 FCI 文档的机器人接口规范页面中定义。
/
std::array<double, 2> elbowd{};
/
* 命令的肘部配置。
* 数组值的含义：
* - elbowc[0]: 第3关节的位置，单位 \f$[rad]\f$。
* - elbowc[1]: 肘部（第4关节）的翻转方向：
* - +1 如果 \f$q4 > q{elbow-flip}\f$
* - 0 如果 \f$q4 == q{elbow-flip} \f$
* - -1 如果 \f$q4 < q{elbow-flip} \f$
* 。
* 其中 \f$q{elbow-flip}\f$ 在 FCI 文档的机器人接口规范页面中定义。
/
std::array<double, 2> elbowc{};
/
* 命令的肘部速度。
* 数组值的含义：
* - delbowc[0]: 第3关节的速度，单位 \f$\frac{rad}{s}\f$
* - delbowc[1]: 始终为 0。
*/
std::array<double, 2> delbowc{};
/
* 命令的肘部加速度。
* 数组值的含义：
* - ddelbowc[0]: 第3关节的加速度，单位 \f$\frac{rad}{s^2}\f$
* - ddelbowc[1]: 始终为 0。
*/
std::array<double, 2> ddelbowc{};
/
* \f$\tau{J}\f$
* 测量的关节侧力矩传感器信号。单位：\f$[Nm]\f$
/
std::array<double, 7> tauJ{}; NOLINT(readability-identifier-naming)
/
* \f${\tauJ}d\f$
* 期望的关节侧力矩传感器信号（不含重力）。单位：\f$[Nm]\f$
/
std::array<double, 7> tauJd{}; NOLINT(readability-identifier-naming)
/
* \f$\dot{\tau{J}}\f$
* 测量的关节侧力矩传感器信号的导数。单位：\f$[\frac{Nm}{s}]\f$
/
std::array<double, 7> dtauJ{}; NOLINT(readability-identifier-naming)
/
* \f$q\f$
* 测量的关节位置。单位：\f$[rad]\f$
/
std::array<double, 7> q{};
/
* \f$qd\f$
* 期望的关节位置。单位：\f$[rad]\f$
/
std::array<double, 7> qd{};
/
* \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> dqd{};
/
* \f$\ddot{q}d\f$
* 期望的关节加速度。单位：\f$[\frac{rad}{s^2}]\f$
/
std::array<double, 7> ddqd{};
/
* 表示每个关节激活的接触等级。接触消失后值变为零。
* @see Robot::setCollisionBehavior 用于设置灵敏度值。
*/
std::array<double, 7> jointcontact{};
/
* 表示笛卡尔维度 \f$(x,y,z,R,P,Y)\f$ 中激活的接触等级。接触消失后值变为零。
* @see Robot::setCollisionBehavior 用于设置灵敏度值。
*/
std::array<double, 6> cartesiancontact{};
/
* 表示每个关节激活的接触等级。接触消失后值保持不变，直到发送复位命令。
* @see Robot::setCollisionBehavior 用于设置灵敏度值。
* @see Robot::automaticErrorRecovery 用于在碰撞后执行复位。
*/
std::array<double, 7> jointcollision{};
/
* 表示笛卡尔维度 \f$(x,y,z,R,P,Y)\f$ 中激活的接触等级。接触消失后值保持不变，直到发送复位命令。
* @see Robot::setCollisionBehavior 用于设置灵敏度值。
* @see Robot::automaticErrorRecovery 用于在碰撞后执行复位。
*/
std::array<double, 6> cartesiancollision{};
/
* \f$\hat{\tau}{\text{ext}}\f$
* 低通滤波后的外部力在关节上产生的力矩。不包括配置的末端执行器和负载以及机器人的质量和动力学。
* tauexthatfiltered 是 tauJ 与机器人模型预期力矩之间的误差。单位：\f$[Nm]\f$。
/
std::array<double, 7> tauexthatfiltered{};
/
* \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> OFexthatK{}; 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> KFexthatK{}; 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> OdPEEd{}; NOLINT(readability-identifier-naming)
/
* \f${^OddP}O\f$
* 机器人基座加速度的线性分量，表达在与 @ref o-frame “基坐标系” 平行的坐标系中，即基座的平移加速度。
* 如果基座静止，则显示重力矢量方向。目前硬编码为 {0, 0, -9.81}。
/
std::array<double, 3> OddPO{}; NOLINT(readability-identifier-naming)
/
* \f${^OT{EE}}{c}\f$
* 运动生成最后一次命令的末端执行器位姿，在 @ref o-frame “基坐标系” 中。
* 位姿以列主序的 4×4 矩阵表示。
/
std::array<double, 16> OTEEc{}; 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> OdPEEc{}; 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> OddPEEc{}; 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 currenterrors{};
/
* 包含导致上一次运动中止的错误。
/
Errors lastmotionerrors{};
/
* 最近 100 个控制命令被机器人成功接收的百分比。
* 如果当前没有运行控制或运动生成循环，则显示为零。
*
* 范围：\f$[0, 1]\f$。
*/
double controlcommandsuccessrate{};
/
* 当前机器人模式。
/
RobotMode robotmode = RobotMode::kUserStopped;
/
* 自机器人启动以来严格单调递增的时间戳。
* 在控制循环内部，可以使用 @ref callback-docs “Robot::control 的 timestep 参数” 代替。
/
Duration time{};
};
/
* 将机器人状态以 JSON 对象形式流输出：{“fieldname1”: [0,0,0,0,0,0,0], “fieldname2”:
* [0,0,0,0,0,0], …}
* @param[in] ostream Ostream 实例
* @param[in] robotstate 要输出的 RobotState 实例
* @return Ostream 实例
*/
std::ostream& operator«(std::ostream& ostream, const franka::RobotState& robotstate);
/**
* 以人类可读形式输出 RobotMode
* @param[in] ostream Ostream 实例
* @param[in] robotmode 要输出的 RobotMode
*
* @return Ostream 实例
*/
std::ostream& operator«(std::ostream& ostream, RobotMode robot_mode);
} namespace franka
code
评论