Franka机械臂代码学习——echo_robot_state.cpp例程

原创已于 2023-05-09 14:12:59 修改 · 935 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#学习 #人工智能

于 2023-03-31 22:06:56 首次发布

franka代码学习专栏收录该内容

8 篇文章

订阅专栏

本文主要解读了Franka机械臂的echo_robot_state.cpp例程，该例程用于读取机械臂状态。介绍了franka::RobotState结构体提供的关节空间、笛卡尔空间和接口信号三类状态信息，还提及franka::Robot::readOnce和franka::Robot::read函数，指出读取依靠该结构体，且控制环中也可读取数据。

在第一次运行Franka机械臂的时候，FCI手册上让我们跑的就是echo_robot_state.cpp这个例程。这个例程主要目的是读取机械臂的各个状态，比如关节角之类的。下面我们就来解读一下这个例程：

#include <iostream>
// C++中的输入输出流库
#include <franka/exception.h>
// 包含用于异常处理的类
#include <franka/robot.h>

/**
 * @example echo_robot_state.cpp
 * 一个连续读取机械臂状态的例程
 */

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

  try {   
  // try模块中存放可能出现异常的代码，后面通常有一个或多个catch块
    franka::Robot robot(argv[1]);
    
    // 回调函数
    size_t count = 0;       // size_t类型表示C/C++中任何对象所能达到的最大长度，无符号整数
    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;
    });

    std::cout << "Done." << std::endl;
  } catch (franka::Exception const& e) {    // catch跟在try后面捕获异常
    std::cout << e.what() << std::endl;     // 抛出异常
    return -1;                              // return -1表示有异常信息，函数失败
  }

  return 0;
}

franka::RobotState 结构体提供了机械臂的状态信息，主要包括三类：

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

对于一个简单的读取操作，可以使用 franka::Robot::readOnce 函数：
franka::RobotState state = robot.readOnce();

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