Franka Robot 夹爪控制示例(grasp_object.cpp)

// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <iostream>
#include <sstream>
#include <string>
#include <thread>

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

/**
 * @example grasp_object.cpp
 * An example showing how to control FRANKA's gripper.
 */
/**
 * @example grasp_object.cpp
 * 控制Franka夹爪的示例
 * ./grasp_object gripper123 true 0.05
 * gripper123 是抓取器的主机名。
 × true 表示程序将在抓取前执行归位操作。
 × 0.05 表示目标物体的宽度为 5 厘米。
 */
int main(int argc, char** argv) {
  if (argc != 4) {
    std::cerr << "Usage: ./grasp_object <gripper-hostname> <homing> <object-width>" << std::endl;
    return -1;
  }

  try {
    franka::Gripper gripper(argv[1]); // 使用IP初始化夹爪 
    double grasping_width = std::stod(argv[3]);// 获取夹爪宽度  目标物体的宽度 0.05 表示目标物体的宽度为 5 厘米

    std::stringstream ss(argv[2]);
    bool homing;
    if (!(ss >> homing)) {// 动作前 是否归位
      std::cerr << "<homing> can be 0 or 1." << std::endl;
      return -1;
    }

    if (homing) {// 动作前， 进行归位操作。
      // Do a homing in order to estimate the maximum grasping width with the current fingers.
      gripper.homing();
    }

    // Check for the maximum grasping width.
    franka::GripperState gripper_state = gripper.readOnce();// 读取夹爪状态
    if (gripper_state.max_width < grasping_width) {// 如果抓取器的最大开口宽度小于目标抓取宽度， 则报错
      std::cout << "Object is too large for the current fingers on the gripper." << std::endl;
      return -1;
    }

    // Grasp the object.
    if (!gripper.grasp(grasping_width, 0.1, 60)) {// 使用0.1 代表抓取速度， 60代表抓取力度
      std::cout << "Failed to grasp object." << std::endl;
      return -1;
    }

    // Wait 3s and check afterwards, if the object is still grasped.
    std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(3000));// 延迟3s

    gripper_state = gripper.readOnce();//读取一次夹爪状态
    if (!gripper_state.is_grasped) {// 判断是否仍然抓取物体 丢失则报错
      std::cout << "Object lost." << std::endl;
      return -1;
    }

    std::cout << "Grasped object, will release it now." << std::endl;
    gripper.stop(); // 停止抓取动作， 撤销施加的力
  } catch (franka::Exception const& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }

  return 0;
}

这段代码是一个使用 Franka Robotics GmbH 的夹爪控制库（franka::Gripper）的示例程序。让我来解释一下主要的部分：

1. 命令行参数解析：

   • argv[1]：表示夹爪的主机名或者 IP 地址，用于初始化 franka::Gripper 对象。
   • argv[2]：表示是否进行归位操作的标志，可以是 true 或 false，在代码中通过 std::stod 转换为 bool 类型的 homing 变量。
   • argv[3]：表示目标物体的宽度，这里使用 std::stod 转换为 double 类型的 grasping_width 变量，单位可能是米或者厘米，这取决于你的具体设置。

2. 归位操作：

   • 如果 homing 为 true，则调用 gripper.homing() 进行夹爪的归位操作，以确保准备好进行抓取。

3. 检查夹爪能力：

   • 使用 gripper.readOnce() 读取夹爪的状态，其中 gripper_state.max_width 表示夹爪的最大开口宽度。如果目标物体的宽度 grasping_width 大于夹爪的最大开口宽度，程序会输出错误信息并返回 -1。

4. 抓取操作：

   • 调用 gripper.grasp(grasping_width, 0.1, 60) 进行抓取操作，其中 0.1 可能代表抓取的速度，60 可能代表抓取时施加的力度或者其他参数，具体取决于你的夹爪和库的实现。

5. 等待和检查抓取：

   • 使用 std::this_thread::sleep_for 延迟 3 秒，然后再次调用 gripper.readOnce() 检查是否仍然成功抓取物体。

6. 停止抓取：

   • 最后调用 gripper.stop() 停止抓取动作，释放施加的力，表明抓取操作结束。

整体来说，这个程序展示了如何使用 Franka Robotics 的夹爪库来实现抓取物体的功能，并包含了一些基本的错误处理和状态检查。