Franka Robot 打印机器人关节位姿(print_joint_poses.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 <iterator>

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

/**
 * @example print_joint_poses.cpp
 * An example showing how to use the model library that prints the transformation
 * matrix of each joint with respect to the base frame.
 */

/**
 * @example print_joint_poses.cpp
 * 一个示例，展示如何使用模型库打印每个关节相对于基础坐标系的变换矩阵。
 */

// 定义了一个模板函数 operator<<，用于输出 std::array 类型的对象到输出流 std::ostream 中
template <class T, size_t N>
std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) {
  ostream << "[";
  //使用 std::copy 算法将数组的第一个元素到倒数第二个元素输出到流 ostream 中，每个元素之间用逗号分隔
  std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
  //将数组的最后一个元素输出到流 ostream 中，不再跟随逗号
  std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
  ostream << "]";
  return ostream;
}

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

  try {
    franka::Robot robot(argv[1]);// 连接机器人

    franka::RobotState state = robot.readOnce();// 读取机器人状态

    franka::Model model(robot.loadModel()); // 加载机器人模型库
    for (franka::Frame frame = franka::Frame::kJoint1; frame <= franka::Frame::kEndEffector;// 迭代打印
         frame++) {
      std::cout << model.pose(frame, state) << std::endl;// 通过坐标系和机器人状态，获取到相应状态的机器人位姿数组
    }
  } catch (franka::Exception const& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }

  return 0;
}

• 功能：

  • 命令行参数检查：检查参数个数是否为 2，若不是则输出正确的使用方式并返回 -1。
  • 机器人连接和状态读取：使用指定的机器人主机名连接机器人，并通过 robot.readOnce() 读取当前机器人的状态。
  • 模型加载和姿态获取：通过 robot.loadModel() 加载机器人的模型，并使用 model.pose(frame, state) 获取每个关节相对于基础坐标系的变换矩阵。
  • 循环打印：使用 for 循环迭代打印从关节 kJoint1 到末端执行器 kEndEffector 的每个关节的姿态信息。

• 异常处理：使用 try-catch 块捕获可能抛出的 franka::Exception 异常，并输出异常信息。

这段代码演示了如何利用 Franka Robotics 的库加载机器人模型，并获取打印每个关节相对于基础坐标系的变换矩阵的功能。它结合了模板函数以及机器人模型和状态的操作，为机器人控制和姿态分析提供了一个基础示例。