Franka Emika
本文介绍Franka机器人C++代码库——libfranka的使用方法。包含笔者在使用过程中的一点心得体会。笔者基于libfranka 0.8.0 版本进行开发调试,仅限于 Linux 系统。相关介绍也可以参考此文


这里假设读者已具备以下基础知识:


  • C++ 程序设计基础
  • Linux 操作系统基础
  • CMake 工具操作基础




官方资料汇总



安装libfranka


环境要求:


  • 一台Franka Panda机器人(确定已购买科研包)。
  • PC一台作为Workstation,需要以太网口,及网线一根。
  • Linux操作系统,笔者使用的是Ubuntu 16.04(64位)。

Franka机器人的核心功能及开发工具安装流程请参考此文


libfranka基本操作


源码编译


这里首先介绍下libfranka的源码安装编译方法,注意_如果已经采用apt-get方法安装ros-kinetic-libfranka,那么此步骤可以跳过_,libfranka已经被默认安装在ROS目录下(/opt/ros/kinetic/)。但是,如果想要安装最新版本的libfranka,必须采用源码编译,且在此之前必须删除之前安装的libfranka和franka_ros以防冲突。


sudo apt remove “_libfranka_“
  • 1

第1步,删除完成后,首先安装依赖库:


sudo apt install build-essential cmake git libpoco-dev libeigen3-dev
  • 1

第2步,找到一个合适的位置,从Github下载源码,并进入libfranka源文件夹:


git clone —recursive https://github.com/frankaemika/libfranka
cd libfranka
  • 1
  • 2

第3步,在源文件夹下创造一个名为build的路径,并在其中启动CMake编译:


mkdir build
cd build
cmake -DCMAKE_BUILD_TYPE=Release ..
cmake —build .
  • 1
  • 2
  • 3
  • 4

HelloWorld


libfranka自带的例程对于理解libfrank非常有效,本节简单介绍下例程的使用方法。机器人上电后,首先通过浏览器进入Desk界面,此时机器人是锁定状态(黄色灯亮),在Desk界面接触锁定,进入准备运动模式(蓝色灯亮)。


以Linux系统(Ubuntu 16.04)为例,首先找到libfranka文件夹所在路径。如果读者是采用apt-get install安装的,那么去/opt/ros/kinetic/下找;如果读者采用源码编译,那么去保持源码的路径下找。


在libfranka路径下首先找到 examples 文件夹,该文件夹下保存了所有的示例C++代码。如要运行,在libfranka/build/examples/ 路径下已有生成的可执行文件。此时保持机器人控制柜与计算机工作站连接,且机器人处于准备运动模式(蓝色灯亮),执行如下指令:


./communication_test “172.16.0.2”

    这里以通信测试为例,其它类似,注意第一个参数是机器人的IP地址。官方推荐使用机器人控制柜与计算机工作站连接,当然连接机器人底座亦可,只不过IP地址要换成hostname,笔者不曾尝试过。开始运行时例程会提示保持急停按钮在操作者手中,并按Enter继续。可以看到机器人运动到一个位姿,终端输出通信测试结果。


    运动生成与机器人控制


    libfranka
    Franka机器人控制必须以实时(Real-Time)方式进行,如上图所示。libfranka中,定义了一个名为”franka”的命名空间,其中最重要的是“franka::Robot”类,该类用于与机器人通信(I/O)。libfranka提供多种控制信号输入模式,笛卡尔空间位置、速度,关节空间位置、速度,关节力矩。这些均由一个多态的“franka::Robot::control”方法实现。


    控制算法主要通过回调函数实现,官方例程如下,这是一个lambda闭包表达式


    double time = 0.0;
    auto control_callback = [&time](const franka::RobotState& robot_state,
    franka::Duration time_step) -> franka::JointPositions {
    time += time_step.toSec(); // Update time at the beginning of the callback.
    franka::JointPositions output = getJointPositions(time);
    if (time >= max_time) {
    // Return MotionFinished at the end of the trajectory.
    return franka::MotionFinished(output);
    }
    return output;
    }

      注意Franka机器人工作频率是1kHz,也就是说回调函数必须在1ms之内执行完成。回调函数有两个参数



      通过 franka::Duration 类对象的 toSec() 方法返回时间信息(double,通常是0.001)可以实现插值。上述例程的控制模式为 franka::JointPositions 。结束这个回调函数循环使用 franka::MotionFinished 该函数的输入参数是当前控制模式下最后一个控制指令(如例程中式 franka::JointPositions 类型变量),返回值也是也是该类型指令对象,只不过该指令对象的 motion_finished 属性被赋值为 true。


      无论是运动生成还是控制,信号都必须满足关节空间和笛卡尔空间的约束条件。约束条件在FCI文档中给出。如果任一约束条件未能满足,程序将抛出一个错误并终止运动。例如如果用户给定的初始位置与机器人实际当前位置差太远,系统将抛出 start_pose_invalid 错误并终止运动。


      运动生成


      libfranka提供4种类型的运动生成器,如下图所示:
      Control
      下面给出一个简单的关节位置运动生成示例,其它类似:


      #include <cmath>
      #include <iostream>
      #include <franka/exception.h>
      #include <franka/robot.h>

      int main(int argc, char__ argv) {
      if (argc != 2) {
      std::cerr << “Usage: “ << argv[0] << “IP-Address” << std::endl;
      return -1;
      }
      try
      {
      franka::Robot robot(argv[1]);
      robot.setCollisionBehavior(
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
      robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
      robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
      std::cout << “Please make sure to have the user stop button at hand!” << std::endl
      << “Press Enter to continue…” << std::endl;
      std::cin.ignore();
      std::array<double, 7> initial_position;
      double time = 0.0;
      robot.control([&initial_position, &time](const franka::RobotState& robot_state,
      franka::Duration period) -> franka::JointPositions {
      time += period.toSec();
      if (time == 0.0) {
      initial_position = robot_state.q_d;
      }
      double delta_angle = M_PI / 8.0 _ (1 - std::cos(M_PI / 2.5 _ time));
      franka::JointPositions output = {{initial_position[0], initial_position[1],
      initial_position[2], initial_position[3],
      initial_position[4], initial_position[5],
      initial_position[6] + delta_angle}};
      if (time >= 5) {
      std::cout << std::endl << “Finished motion, shutting down robot” << std::endl;
      return franka::MotionFinished(output);
      }
      return output;
      });
      }
      catch(const franka::Exception& e)
      {
      std::cerr << e.what() << std::endl;
      return -1;
      }
      return 0;
      }

      首先声明一个 franka::Robot 类对象,初始化参数为机器人IP地址。然后设定其初始化参数,注意参数设定必须在执行控制指令前,切不可在控制过程中。


      机器人运动生成与控制采用 franka::Robot::control 方法实现,该方法的输入参数是一个函数模板,即回调函数,如前所述。回调函数内完成1ms控制周期内的控制指令后,通过 franka::MotionFinished 函数结束控制过程。


      具体操作及其它功能可参考官方文档及例程


      机器人控制


      事实上,如果单纯使用运动生成器,那么 franka::Robot::control 会默认调用内部的阻抗控制器(kJointImpedance 和 kCartesianImpedance)。当然,也可以同时使用,即 franka::Robot::control 方法可以有两个参数,如下述官方模板:


      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;
      }

      可以看出,当发生错误时,libfranka会抛出“franka::Exception”异常。
      对于控制器设计,libfranka提供了关节力矩作为输入信号。一个简单的零力控制方法如下,这里机器人本身提供了重力补偿:


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

        注意无论是自己设计运动生成器还是控制器,都必须保证信号足够平滑,也就是说两个控制周期之间输入的变化不要太大。官方手册给出了franka机器人的一些约束条件。为了应对不太理想的网络连接,libfranka本身提供了信号处理机制来平滑化输入信号,减少其控制周期间的变化。这是一个比率限制(rate limiting)和一个低通滤波(low-pass filter)。在libfranka 0.5.0 版本之后,低通滤波就被基本放弃了。事实上 franka::Robot::control 方法还有两个额外的参数用于此功能,详情可参考官方手册相关章节。注意第一次测试自定义控制器时尽量不要使能这些平滑机制,因为可能会造成不稳定,方法如下:


        // 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);

          对于实时通信过程中较大量的丢包问题,这些平滑机制也于事无补。官方例程中给出了一个通信性能测试例程(communication_test),在libfranka/build/example中。franka::RobotState 结构体的属性 control_command_success_rate 可以返回实时通信成功率。


          libfranka运动生成器和控制器的作用机理可参考下图:
          RT
          控制机器人的信号实际上是带有下标d的信号(desired),用户可以实时获取该信号来检查信号被滤波或者是否有丢包现象。


          信息读取与模型库


          Franka 自带重点摩擦补偿机制。由于有关节力矩传感器,使得其零力控制性能相对于UR之类的机器人更加灵敏顺畅。打开示教模式(白色灯亮),用户可以通过机器人末端的Pilot实现拖动示教,并记录一些信息。libfranka中,机器人状态由 franka::RobotState 结构体管理,实时读取的频率也是1kHz,读取数据由 franka::Robot::read 函数管理。


          franka::RobotState 结构体提供了丰富的信息,主要包括三类:


          • 关节空间信息:如关节角度、角速度、关节力矩、估计出的外部力矩、碰撞/接触状态等。
          • 笛卡尔空间信息:末端参数、末端位置、速度、估计出的外部扭矩等。
          • 接口信号:最后一个控制信号和期望信号等。

          对于一个简单的读取操作,可以使用 franka::Robot::readOnce 函数来简化流程:


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

            持续的实时读取依然需要回调函数,官方例程如下:


            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;
            });

              注意,franka::Robot:;read 函数的定义如下:


              void franka::Robot::read(std::function< bool(const RobotState &)> read_callback)
              • 1

              可以看出,其参数是一个返回布尔型数据的回调函数,当返回 false 时,状态读取停止。注意在示教模式下(白色灯亮),不可以设置机器人的阻抗控制参数。


              实际上,读取依靠的是 franka::RobotState 结构体,即 franka::Robot::read 函数的输入参数。而该参数在 franka::Robot::control 函数中也有,用起来都是当作反映机器人当前状态信息的常量。因此,很多时候并不需要单独使用 franka::Robot::read 函数专门读取数据,在控制环中也可以读取。但是,如前所述,每个控制周期内留个程序执行的时间一般小于300微妙,对于上述例程中用count输出信息的操作在实际控制环路中要尽量避免,使用读取函数就可以接受。


              具体操作及其它功能可参考例程“echo_robot_state.cpp”。


              libfranka自带的模型库可以提供运动学与动力学参数,包括:


              • 前向运动学。
              • 雅可比矩阵。
              • 动力学参数:质量矩阵、科氏力和向心力向量、重力向量。

              模型库由franka::Model类管理,注意使用时需要加入头文件:


              #include <franka/Model.h>
              • 1

              注意初始化一个franka::Model对象通常采用franka::Robot对象的loadModel()方法,返回值是一个franka::Model对象:


              franka::Robot robot(argv[1]);
              franka::Model model = robot.loadModel();

                使用机器人模型库读者可以计算任意姿态下的参数,不必是机器人当前姿态下的参数。具体用法可参考例程


                非实时指令


                电爪控制


                Franka 自带的电爪可以做很多事情,使用非常方便。电爪的控制是非实时(blocking)的,如下图所示。
                Hand
                电爪的控制由 franka::Gripper 类管理。使用该类时注意加入头文件:


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

                ...

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

                  电爪的IP地址和机器人是一样的。


                  参数设置


                  一些机器人的参数不能在实时控制环路中修改,必须在环路外已非实时指令的方式设定。一些典型的指令是:


                  • franka::Robot::setCollisionBehavior:设置碰撞检测门限。
                  • franka::Robot::setCartesianImpedance:设置笛卡尔空间阻抗参数
                  • franka::Robot::setJointImpedance:设置关节空间阻抗参数
                  • franka::Robot::setEE:设置末端执行器位姿(Nominal Frame 到 End Effector Frame)
                  • franka::Robot::setK:设置末端执行器位姿(End Effector Frame 到 Stiffness Frame)
                  • franka::Robot::setLoad:设置末端负载参数
                  • franka::Robot::automaticErrorRecovery:清除所有已发生的异常

                  末端执行器也可以在Desk界面的Settings中设置,默认是自带的电爪。