libfranka

在继续本章之前，请为 Linux 或 Windows 安装或编译 libfranka。最新版本的 libfranka 的 API 文档可从 https://frankaemika.github.io/libfranka 获得。

_images/libfranka-architecture.png

示意图概述

libfranka 是 FCI 客户端的 C++ 实现。它处理与 Control 的网络通信并提供接口以易化

  • 执行 非实时命令 来控制抓手和配置臂参数。

  • 执行 实时命令 以运行 1kHz 控制回路。

  • 读取 机器人状态 以获取 1kHz 的传感器数据。

  • 访问 模型库 以计算期望的运动学和动力学参数。

在操作过程中，可能还会遇到一些 错误，我们在本节末尾详细介绍了这些错误。

非实时命令

非实时命令是阻塞的、基于 TCP/IP 的，并且总是 在任何实时控制循环之外 执行。它们包含所有的抓手命令和一些与臂 的配置相关的命令。

_images/fci-architecture-non-realtime.png

臂和抓手的非实时命令。

与抓手最相关的是

  • homing 校准抓手的最大抓取宽度。

  • move, grasp 和 stop, 用抓手移动或抓取。

  • readOnce，读取抓手状态。

关于臂，一些有用的非实时命令是：

  • setCollisionBehavior 它设置接触和碰撞检测阈值。

  • setCartesianImpedance 和 setJointImpedance 为内部笛卡尔阻抗和内部关节阻抗控制器设置阻抗参数。

  • setEE 设置从名义末端执行器到末端执行器坐标系的变换 NE_T_EE。从法兰坐标系到末端执行器坐标系的变换 F_T_EE 被分为两个变换： F_T_NE 和 NE_T_EE。从法兰坐标系到名义末端执行器坐标系的变换 F_T_NE 只能通过管理员界面进行设置。

  • setK 设置从末端执行器坐标系到刚度坐标系的变换 EE_T_K。

  • setLoad 设置负载的动力学参数。

  • automaticErrorRecovery 清除之前在机器人中发生的任何命令或控制异常。

有关完整且详细指定的列表，请查看 Arm 或 Hand 的 API 文档 。

臂或抓手上的所有操作（非实时或实时）分别通过 franka::Robot 和 franka::Gripper 对象执行 。创建对象时，将建立与臂/抓手的连接：

#include <franka/robot.h>
#include <franka/gripper.h>

...

franka::Gripper gripper("<fci-ip>");
franka::Robot robot("<fci-ip>");

该地址可以作为主机名或 IP 地址传递。如果出现任何错误，无论是由于网络还是库版本冲突，franka::Exception 类型的异常都会被抛出。同时使用多个机器人时，只需创建多个具有适当 IP 地址的对象即可。

要运行特定命令，只需调用相应的方法，例如

gripper.homing();
robot.automaticErrorRecovery();

实时命令

实时命令基于 UDP，需要 1kHz 连接到 Control。有两种类型的实时接口：

  • 运动生成器，它定义了关节或笛卡尔空间中的机器人运动。

  • 控制器，定义要发送到机器人关节的扭矩。

有 4 种不同类型的外部运动生成器和 3 种不同类型的控制器（1 个外部和 2 个内部），如下图所示：

_images/rt-interfaces.png

实时接口：运动生成器和控制器。

可以使用单个接口或组合两种不同的类型。具体来说，可以给出命令：

  • 只有一个运动生成器，因此使用两个内部控制器之一来跟随命令的运动。

  • 只有一个外部控制器，而忽略任何运动生成器信号，即仅转矩控制。

  • 一个运动生成器和一个外部控制器，可在外部控制器中使用 Control 的逆运动学。

所有实时循环（运动生成器或控制器）都由一个回调函数定义，该函数接收机器人状态和最后一个循环的持续时间（除非发生数据包丢失，否则为 1ms） 并返回接口的特定类型。然后 franka::Robot 类的 control 方法将通过以 1kHz 的频率执行回调函数来运行控制循环，如本例所示

std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
   my_external_controller_callback;
// Define my_external_controller_callback
...

std::function<franka::JointVelocities(const franka::RobotState&, franka::Duration)>
    my_external_motion_generator_callback;
// Define my_external_motion_generator_callback
...

try {
  franka::Robot robot("<fci-ip>");
  // only a motion generator
  robot.control(my_external_motion_generator_callback);
  // only an external controller
  robot.control(my_external_controller_callback);
  // a motion generator and an external controller
  robot.control(my_external_motion_generator_callback, my_external_controller_callback);
} catch (franka::Exception const& e) {
  std::cout << e.what() << std::endl;
  return -1;
}
  return 0;
}

一旦 motion_finished 实时命令的标志设置为 true，表示所有控制循环就完成了。此处显示了 libfranka 示例 中包含的 generate_joint_velocity_motion 示例的摘录

robot.control(
     [=, &time](const franka::RobotState&, franka::Duration period) -> franka::JointVelocities {
       time += period.toSec();

       double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max));
       double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time));

       franka::JointVelocities velocities = {{0.0, 0.0, 0.0, omega, omega, omega, omega}};

       if (time >= 2 * time_max) {
         std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
         return franka::MotionFinished(velocities);
       }
       return velocities;
     });

在这种情况下，回调函数直接在 robot.control( ... ) 函数的调用中定义。它使用关节速度运动生成器接口，因为它返回一个 franka::JointVeloities 对象。它命令最后四个关节的关节速度并将它们移动大约 +/-12 度。在 2 * time_max 秒后，回调函数将返回一个 motion_finished 标志，通过 franka::MotionFinished 方法将它设置为 true ，则控制循环将停止。

请注意，如果仅使用运动生成器，则默认控制器是内部关节阻抗控制器。但是，可以通过设置控制函数的可选参数来使用内部笛卡尔阻抗控制器，例如

// Set joint impedance (optional)
robot.setJointImpedance({{3000, 3000, 3000, 3000, 3000, 3000, 3000}});
// Runs my_external_motion_generator_callback with the default joint impedance controller
robot.control(my_external_motion_generator_callback);
// Identical to the previous line (default franka::ControllerMode::kJointImpedance)
robot.control(my_external_motion_generator_callback, franka::ControllerMode::kJointImpedance);

// Set Cartesian impedance (optional)
robot.setCartesianImpedance({{2000, 2000, 2000, 100, 100, 100}});
// Runs my_external_motion_generator_callback with the Cartesian impedance controller
robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance);

为了编写控制器，还使用了 franka::Robot::control 函数。以下示例显示了一个简单的控制器，它为每个关节发出零扭矩命令。重力项已由机器人补偿。

robot.control([&](const franka::RobotState&, franka::Duration) -> franka::Torques {
      return {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
    });

可以在 libfranka examples 中找到所有接口及控制回路组合的示例。在运行示例之前，请验证机器人是否有足够的自由空间来移动而不会发生碰撞。然后，例如对于 generate_joint_velocity_motion 示例，从 libfranka 构建目录 build 执行以下命令：

./examples/generate_joint_velocity_motion <fci-ip>

警告

要编写自己的运动生成器或控制器，向机器人传递平滑信号至关重要。非平滑信号很容易产生不连续性错误，甚至使机器人不稳定。开始前需 检查 接口规范 。

信号处理

为了便于在非理想网络连接下控制机器人，libfranka 包括信号处理函数，这些函数将修改用户命令的值，使其符合 接口限制。所有实时控制回路中都包含两个 可选功能：

  • 一阶低通滤波器 low-pass filter，用于平滑用户命令信号。

  • 一个速率限制器 rate limiter，它使用户命令值的时间导数饱和。

  • 从版本 0.5.0 开始，libfranka 包括一个低通滤波器 low-pass filter，用于 默认运行 的所有实时接口，截止频率为 100Hz。滤波器平滑命令信号以提供更稳定的机器人运动，但不能防止违反 接口限制。

  • 从版本 0.4.0 开始，所有实时接口的 速率限制器 （rate limiters） 默认运行 。速率限制器 （rate limiters） ，也称为 安全控制器 （safe controllers），将限制用户发送的信号的变化率，以防止违反 接口限制。. 对于运动生成器，它将限制加速度和加加速度，而对于外部控制器，它将限制扭矩变化率。它们的主要目的是提高控制回路的稳健性。在数据包丢失的情况下，即使发送的信号符合接口限制，Control 也可能检测到违反速度、加速度或加加速度限制的情况。速率限制将调整命令以确保不会发生这种情况。有关更多详细信息，请查看 不合规错误部分 。

    警告

    速率限制将确保除了逆向​​运动学之后的关节限制之外没有任何限制被违反，其违反会产生以 cartesian_motion_generator_joint_* 开头的一系列错误。有关更多详细信息，请查看 不合规错误部分 。

    提示

    速率限制器中使用的限制在 franka/rate_limiting.h 定义并被设置到接口限制中。如果这会产生急动或不稳定的行为，可以将限制设置为较低的值，激活低通滤波器或降低其截止频率。

为了控制信号处理函数，所有 robot.control() 函数调用都有两个额外的可选参数。第一个是启用或停用速率限制器的标志，而第二个指定一阶低通滤波器的截止频率。如果达到截止频率 >=1000.0，滤波器将被停用。例

// Set Cartesian impedance (optional)
robot.setCartesianImpedance({{2000, 2000, 2000, 100, 100, 100}});
// Runs my_external_motion_generator_callback with the Cartesian impedance controller,
// rate limiters on and low-pass filter with 100 Hz cutoff
robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance);
// Identical to the previous line (default true, 100.0 Hz cutoff)
robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance, true, 100.0);
// Runs my_external_motion_generator_callback with the Cartesian impedance controller,
// rate limiters off and low-pass filter off
robot.control(my_external_motion_generator_callback, franka::ControllerMode::kCartesianImpedance, false, 1000.0);

或类似的外部控制器

// With rate limiting and filter
robot.control(my_external_controller);
// Identical to the previous line (default true, 100.0 Hz cutoff)
robot.control(my_external_controller, true, 100.0);
// Without rate limiting but with low-pass filter (100.0 Hz)
robot.control(my_external_controller, false);
// Without rate limiting and without low-pass filter
robot.control(my_external_controller, false, 1000.0);

危险

低通滤波器和速率限制器是用来针对数据包丢失的稳健性功能，在已经设计了平滑运动生成器或控制器 之后 使用。对于新控制回路的首次测试，我们强烈建议停用这些功能。过滤和限制非平滑信号的速率会产生不稳定性或意外行为。太多的数据包丢失也会产生不稳定的行为。通过监控 control_command_success_rate 机器人状态信号来检查通信质量。

内部逻辑

到目前为止，我们已经介绍了在客户端（即自己的工作站 PC）上运行的接口的详细信息。包括实时接口的控制端在内的完整控制回路的行为如下图所示

_images/rt-loop.png

实时循环：从控制命令到机器人期望的关节扭矩。

运动生成器 ：用户发送的所有运动生成器命令都有下标 c，代表 ‘commanded’。当发送运动生成器时，机器人运动学完备块 Robot Kinematics completion 将计算用户命令信号的正向/逆向运动学，产生 期望 信号，下标 d。如果使用内部控制器，它将产生必要的扭矩 \(\tau_{d}\) 跟踪相应的计算 d 信号（内部关节阻抗控制器将跟踪关节信号 \(q_{d}, \dot{q}_{d}\) 和内部笛卡尔阻抗控制器将跟踪末端笛卡尔信号 \({}^OT_{EE,d}, {}^O\dot{P}_{EE,d}\)）并将它们发送到机器人关节。图中控制侧的所有变量，即最后接收到的 c 值（在低通滤波器和由于数据包丢失导致的外插 extrapolation 之后，阅读下面的解释）、计算出的 d 值及其时间导数被作为机器人状态（的一部分）发送回用户。通过这种方式，可以在自己的外部控制器中利用逆运动学，同时，它将提供 完全透明的信息：用户将始终知道机器人在上一个采样中接收和跟踪的确切值和导数。

提示

当使用 关节 运动生成器时， 机器人运动学 完备块不会修改命令中的 关节 值，因此 \(q_d, \dot{q}_d, \ddot{q}_d\) 和 \(q_c,\dot{q}_c, \ddot{q}_c\) 是等价的。请注意，此时只会在机器人状态下找到（下标为） d 信号。如果使用 笛卡尔 运动生成器，机器人运动学完备块 可能会修改用户命令的值以避免奇异点，从而所需的信号 \({}^OT_{EE,d}, {}^O\dot{P}_{EE,d}\) 和命令的信号 \({}^OT_{EE,c}, {}^O\dot{P}_{EE,c}, {}^O\ddot{P}_{EE,c}\) 可能不同。此时会在机器人状态中找到（下标为） d 和 c 信号。

外部控制器：如果发送外部控制器，则用户命令的已经额外补偿重力与摩擦力的期望关节扭矩 \(\tau_{d}\) 将直接送入机器人关节。得到以下方程：

\(\tau_{c} = \tau_{d} + \tau_{f} + \tau_{g}\)

其中：

  • \(\tau_{d}\) 是 libfranka 用户输入的期望扭矩，

  • \(\tau_{c}\) 是有效控制关节的扭矩，

  • \(\tau_{f}\) 是补偿电机摩擦的扭矩，

  • \(\tau_{g}\) 是补偿整个运动学链重力所需的扭矩。

请注意，在控制端，有两件事可以修改信号：

  • 数据包丢失，如果：

    • 由于 PC 和 网卡的性能，没有很好的网络连接。

    • 控制回路计算时间太长（取决于网卡和 PC 配置，剩下大约 < 300 \(\mu s\) 的时间用于自己的控制回路）。

    在这种情况下，Control 假设一个恒定加速度模型或一个恒定扭矩来推断信号。如果 >=20 个数据包连续丢失，则控制循环将停止，并伴随 communication_constraints_violation 异常的抛出。

提示

如果不确定信号是否被过滤或外插，可以随时检查发送的最后一个命令值，并将它们与在下一个采样中在机器人状态中接收到的值进行比较。还可以在异常发生后在 franka::ControlException::log 成员中找到这些值 。

机器人状态

机器人状态以 1kHz 的速率提供机器人传感器读数和估计值。它提供：

  • 关节层级信号：电机和估计的关节角度及其导数、关节扭矩及其导数、估计的外部扭矩、关节碰撞/接触。

  • 笛卡尔层级信号：笛卡尔位姿、配置的末端执行器和负载参数、作用在末端执行器上的外部力旋量、笛卡尔碰撞

  • 接口信号：上一小节中解释的最后的命令和期望值及其导数值。

如需完整列表，请查看 此处 的 franka::RobotState API 。如上一小节所示，机器人状态始终是控制循环的所有回调函数的输入。但是，如果希望只读取机器人状态而不对其进行控制，则可以使用功能 read 或 readOnce 来收集它，例如用于记录或可视化目的。

通过合法连接，可以使用以下函数 readOnce 读取 机器人的单次采样状态：

franka::RobotState state = robot.readOnce();

下一个示例显示如何使用 read 函数和回调连续读取机器人状态。返回 false 会在回调中停止循环。下面显示了 echo_robot_state 示例的摘录：

size_t count = 0;
robot.read([&count](const franka::RobotState& robot_state) {
  // Printing to std::cout adds a delay. This is acceptable for a read loop such as this,
  // but should not be done in a control loop.
  std::cout << robot_state << std::endl;
  return count++ < 100;
});

Model 库

机器人模型库提供

  • 所有机器人关节的正向运动学。

  • 所有机器人关节的体雅可比矩阵和零雅可比矩阵。

  • 动力学参数：惯性矩阵、科式力和离心力矢量和重力矢量。

请注意，加载模型库后，可以计算任意机器人状态的运动学和动力学参数，而不仅仅是当前状态。还可以以非实时方式使用模型库，例如在优化循环中。libfranka 示例包括示例代码 打印关节位姿 或者 计算雅可比和动力学参数 。

错误

使用 FCI 时，可能会遇到一些错误，这些错误可能是由于用户发送的命令不合规、通信问题或机器人行为造成的。以下小节详细介绍了最相关的内容。如需完整列表，请查看 API 文档。”

提示

请注意，在发生错误后，可以自动清除它并使用 franka::Robot::automaticErrorRecovery() 命令继续运行程序，而无需用户干预。在进一步操作前检查异常字符串，以确保错误不是严重错误。

也可以通过切换外部激活设备或使用 Desk 中的错误恢复按钮手动清除某些错误。

由于不合规的命令值而导致的错误

如果用户发送的 指令值 不符合 接口要求 ，则会出现以下错误之一：

  • 由于 运动生成器的错误初始值（wrong initial values of a motion generator） 而导致的错误：

  • joint_motion_generator_start_pose_invalid

  • cartesian_position_motion_generator_start_pose_invalid

  • cartesian_motion_generator_start_elbow_invalid

  • cartesian_motion_generator_elbow_sign_inconsistent

这些错误表明当前机器人值与用户发送的初始值之间存在差异。要修复这些错误，请确保控制回路从机器人状态中观察到的最后一个命令值开始。例如，对于关节位置接口

double time{0.0};
robot.control(
 [=, &time](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointPositions {
   time += period.toSec();
   if (time == 0) {
     // Send the last commanded q_c as the initial value
     return franka::JointPositions(robot_state.q_c);
   } else {
     // The rest of your control loop
     ...
   }
 });

  • 由于使用关节位置/速度运动生成器违反 位置限制 而导致的错误，这将产生 joint_motion_generator_position_limits_violation 报错. 解决此错误应该很简单：确保发送的值在 限制 范围内。笛卡尔接口对逆运动学计算之后得到的关节信号也有限制：如果 Control 的逆运动学求解器产生超出限制的某个关节配置，cartesian_motion_generator_joint_position_limits_violation 将被触发。

  • 由于违反 速度（velocity） 的限制产生的错误和不连续的错误，其指的违反 加速度（acceleration） 和/或 加加速度 （jerk） 的限制。如果使用关节运动生成器，则可能的错误是

  • joint_motion_generator_velocity_limits_violation

  • joint_motion_generator_velocity_discontinuity （违反加速度限制）

  • joint_motion_generator_acceleration_discontinuity （违反加加速度限制）

如果使用的是笛卡尔运动生成器，错误可能是

  • 笛卡尔限制

    • cartesian_motion_generator_velocity_limits_violation

    • cartesian_motion_generator_velocity_discontinuity （违反加速度限制）

    • cartesian_motion_generator_acceleration_discontinuity （违反加加速度限制）

  • 逆运动学解算后的关节限制

    • cartesian_motion_generator_joint_velocity_limits_violation

    • cartesian_motion_generator_joint_velocity_discontinuity （违反加速度限制）

    • cartesian_motion_generator_joint_acceleration_discontinuity （违反加加速度限制）

要减少速度不合规或不连续性错误，请确保命令的信号不违反 限制 。对于每个运动生成器，Control 都会使用反向欧拉法区分用户发送的信号。例如，如果使用关节位置运动生成器，在时间 \(k\) 用户发送命令 \(q_{c,k}\)，由此产生的速度、加速度和加加速度将是

  • 速度 \(\dot{q}_{c,k} = \frac{q_{c,k} - q_{c,k-1}}{\Delta t}\)

  • 加速度 \(\ddot{q}_{c,k} = \frac{\dot{q}_{c,k} - \dot{q}_{c,k-1}}{\Delta t}\)

  • 加加速度/急动度 \(\dddot{q}_{c,k} = \frac{\ddot{q}_{c,k} - \ddot{q}_{c,k-1}}{\Delta t}\)

这里 \(\Delta t = 0.001\). 注意 \(q_{c,k-1}, \dot{q}_{c,k-1}\) 和 \(\dot{q}_{c,k-1}\) 总是作为机器人状态（的一部分）发送回用户作为 \(q_{d}, \dot{q}_{d}\) 和 \(\ddot{q}_{d}\)。因此，即使在数据包丢失的情况下，也可以提前计算结果的导数。有关更多详细信息，请查看 关于控制端接口的细节部分 。

最后，对于扭矩接口，违反 扭矩速率限制 torque rate 会触发错误

  • controller_torque_discontinuity

Control 还使用反向欧拉法（backwards Euler）计算扭矩变化率，即 \(\dot{\tau}_{d,k} = \frac{\tau_{d,k} - \tau_{d,k-1}}{\Delta t}\). 用户先前期望的扭矩也在机器人状态下作为 \(\tau_d\) 发回。因此，即使在数据包丢失的情况下，也可以提前计算产生的扭矩率。

提示

libfranka 版本中包含的速率限制器从 0.4.0 版本起修改用户发送的信号，使它们符合所有这些限制，除了逆向运动学之后的关节限制。可以查看 include/franka/rate_limiting.h 和 src/rate_limiting.cpp 以获取有关如何计算由此限制的所有接口的速度、加速度和加加速度的示例代码。我们再次强调，在不连续信号上使用速率限制很容易产生不稳定行为，因此在启用此 稳健性 robustness 功能之前，请确保信号足够平滑。

通讯问题导致的错误

如果在实时循环中 Control 在 20 个周期（即 20ms）内没有收到任何数据包，将收到 communication_constraints_violation 错误消息。请注意，如果连接间歇性丢包，它可能不会停止，但即使源信号符合接口规范，它也可能触发不连续性错误。在这种情况下，请查看我们的 故障排除部分 并考虑启用 信号处理功能 以提高控制回路的稳健性。

行为错误

警告

这些监控功能并不符合任何安全规范，也不保证对用户的任何安全。他们的目的只是帮助研究人员开发和测试他们的控制算法。

  • 反射性错误 Reflex errors。如果估计的外部扭矩 \(\hat{\tau}_{ext}\) 或力 \({}^O\hat{F}_{ext}\) 超过配置的阈值，将分别触发 cartesian_reflex 或 joint_reflex 错误。可以使用 franka::Robot::setCollisionBehavior 非实时命令配置阈值。

    提示

    如果希望机器人与环境接触，必须将碰撞阈值设置为更高的值。否则，一旦抓住一个物体或推向一个表面，就会触发反射错误。此外，如果阈值较低，在没有接触的情况下非常快速或突然的运动可能会触发反射错误；外部扭矩和力只是 估计值，根据机器人配置它们可能不准确，尤其是在高加速阶段。可以监视它们的值通过观察机器人状态中的 \(\hat{\tau}_{ext}\) 和 \({}^O\hat{F}_{ext}\)。

  • 自碰撞避免 Self-collision avoidance。如果机器人达到接近自碰撞的配置，则会触发 self_collision_avoidance_violation 错误。

    警告

    此错误并不能保证机器人在任何配置和速度下都能防止自碰撞。如果使用扭矩接口以全速驱动机器人，机器人可能会发生自我碰撞。

  • 如果达到 扭矩传感器限制，tau_j_range_violation 错误将被触发。这并不能保证传感器在任何高扭矩相互作用或运动后不会损坏，但旨在防止其中一些。

  • 如果达到 最大允许功率 ，power_limit_violation 错误将触发。它将阻止机器人移动并阻止控制循环继续。

  • 如果达到关节或笛卡尔极限，将分别得到一个 joint_velocity_violation 或一个 cartesian_velocity_violation 错误。