Franka libfranka 运动控制介绍

 

libfranka是 FCI 客户端的 C++ 实现。它处理与 Control 的网络通信，并提供接口以轻松实现以下功能：

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

• 执行实时命令来运行您自己的 1 kHz 控制循环。

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

• 访问模型库来计算所需的运动学和动态参数。

非实时命令

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

 

与 Hand 最相关的是

• homing它校准了手的最大抓握宽度。

• move、grasp和stop、用手移动或抓握。

• readOnce，读取手部状态。

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

• 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 上的所有操作（非实时或实时）分别通过 franka::Robot和franka::Gripper对象执行。创建对象时将建立与 Arm/Hand 的连接：

#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，需要 1 kHz 的控制连接。实时接口有两种类型：

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

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

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

 

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

• 只有一个运动发生器，因此使用两个内部控制器中的一个来遵循命令的运动。

• 仅外部控制器并忽略任何运动发生器信号，即仅扭矩控制。

• 一个运动发生器和一个外部控制器，以使用外部控制器中的控制逆运动学。

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

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 ，所有控制循环都将完成。此处显示了libfranka 示例中true的示例摘录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;
     });

 在这种情况下，回调函数直接在函数调用中定义 。它使用关节速度运动生成器接口，因为它返回一个对象。它命令最后四个关节的关节速度并将它们移动约 +/-12 度。几秒钟后，它将通过使用该方法将其设置为 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}};
    });

在运行示例之前，请验证机器人是否有足够的自由空间来移动而不会发生碰撞。然后，例如，对于示例，generate_joint_velocity_motion从构建目录执行以下命令libfranka：

./examples/generate_joint_velocity_motion <fci-ip>

！注意：要编写自己的运动发生器或控制器，向机器人提供平滑信号至关重要。不平滑的信号很容易产生不连续性错误，甚至使机器人不稳定。开始之前，请检查接口规格。

信号处理

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

• 一阶低通滤波器用于平滑用户命令的信号。

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

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

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

速率限制将确保不会违反任何限制，除了逆运动学后的关节限制，违反该限制会产生以 开头的一系列错误 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）上运行的接口的详细信息。下图显示了包括实时接口控制端在内的完整控制回路的行为：

•  运动发生器：用户发送的所有运动发生器命令都带有下标c ，代表“命令”。发送运动发生器时，机器人运动学完成 块将计算用户命令信号的正向/反向运动学，产生“所需”信号，下标d。如果使用内部控制器，它将产生必要的扭矩𝜏𝑑跟踪相应的计算d信号（内部联合阻抗控制器将跟踪联合信号𝑞𝑑,𝑞˙𝑑以及内部笛卡尔阻抗控制器 𝑂𝑇𝐸𝐸,𝑑,𝑂𝑃˙𝐸𝐸,𝑑) 并将它们发送到机器人关节。图中控制侧的所有变量，即最后收到的c值（经过低通滤波器和由于数据包丢失而进行的外推，请参阅下文了解解释）、计算出的d值及其时间导数都将在机器人状态下发送回用户。这样，您就可以利用您自己的外部控制器中的逆运动学，同时它将为您提供完全的透明度：您将始终知道机器人在最后一个样本中接收和跟踪的确切值和导数。

当你使用关节运动发生器时，机器人运动学完成块不会修改命令的关节值，因此𝑞𝑑,𝑞˙𝑑,𝑞¨𝑑和 𝑞𝑐,𝑞˙𝑐,𝑞¨𝑐是等效的。请注意，您只会 在机器人状态中找到d信号。如果您使用笛卡尔运动生成器，机器人运动学完成块可能会修改用户命令的值以避免奇异性，从而避免所需的信号𝑂𝑇𝐸𝐸,𝑑,𝑂𝑃˙𝐸𝐸,𝑑和指令信号𝑂𝑇𝐸𝐸,𝑐,𝑂𝑃˙𝐸𝐸,𝑐,𝑂𝑃¨𝐸𝐸,𝑐可能会有所不同。您将在机器人状态中发现d和c信号。

外部控制器：如果发送了外部控制器，则用户命令的所需关节扭矩𝜏𝑑直接馈送到机器人关节，并附加重力和电机摩擦的补偿，得出以下方程：

 在哪里：

• 𝜏𝑑是 libfranka 用户输入的所需扭矩，

• 𝜏𝑐是有效控制关节的扭矩，

• 𝜏𝑓是补偿电机摩擦的扭矩，

• 𝜏𝑔是补偿整个运动链重力所需的扭矩。

请注意，在控制方面，有两件事可能会修改您的信号：

数据包丢失，可能发生在以下情况：

• 由于您的 PC + 网卡的性能，连接效果不是很好。

• 你的控制循环计算时间太长（取决于你的网卡和 PC 配置，大约 < 300𝜇𝑠用于您自己的控制回路）。

在这种情况下，控制采用恒定加速度模型或恒定扭矩来推断您的信号。如果>=20数据包连续丢失，则控制环路将停止，但会出现异常 communication_constraints_violation。

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