libfranka的状态信息解析

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

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

Franka机械臂本体的状态解析

Franka 的机械臂相关原始代码

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

RobotState Robot::readOnce() {
  std::unique_lock<std::mutex> 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 <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

目录

libfranka的状态信息解析

Franka机械臂本体的状态解析

Franka 的机械臂相关原始代码

评论