Franka Robot demo 基于插补运动的关节空间运动生成及控制

#include <array>
#include <cmath>
#include <iostream>

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

int main(int argc, char** argv) {
  // 检查是否提供了机器人IP地址
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

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

    // 设置默认的行为参数
    robot.setJointImpedance({{3000.0, 3000.0, 3000.0, 3000.0, 250.0, 250.0, 250.0}});
    robot.setCartesianImpedance({{3000.0, 3000.0, 3000.0, 300.0, 300.0, 300.0}});

    // 定义目标关节位置
    std::array<double, 7> q_goal = {{0.0, -M_PI / 4, 0.0, -3 * M_PI / 4, 0.0, M_PI / 2, M_PI / 4}};

    // 控制器回调函数
    auto motion_finished = [&q_goal](const franka::RobotState& robot_state,
                                     franka::Duration period) -> franka::JointPositions {
      static double time = 0.0;
      time += period.toSec();

      std::array<double, 7> current_q = robot_state.q_d;

      // 计算关节位置误差
      std::array<double, 7> delta_q;
      for (size_t i = 0; i < q_goal.size(); i++) {
        delta_q[i] = q_goal[i] - current_q[i];
      }

      // 如果误差足够小，停止运动
      bool finished = true;
      for (double dq : delta_q) {
        if (std::abs(dq) > 0.01) {
          finished = false;
          break;
        }
      }

      if (finished) {
        return franka::MotionFinished(franka::JointPositions(q_goal));
      }

      // 简单的线性插值
      std::array<double, 7> new_q;
      for (size_t i = 0; i < q_goal.size(); i++) {
        new_q[i] = current_q[i] + 0.1 * delta_q[i];
      }

      return franka::JointPositions(new_q);
    };

    // 运行控制循环
    robot.control(motion_finished);

  } catch (const franka::Exception& e) {
    // 处理异常
    std::cerr << e.what() << std::endl;
    return -1;
  }

  return 0;
}

1. 连接到机器人：使用franka::Robot对象连接到机器人。
2. 设置行为参数：设置关节和笛卡尔空间的阻抗。
3. 定义目标关节位置：定义一个目标关节位置数组q_goal。
4. 控制器回调函数：在回调函数中计算当前关节位置与目标位置的差异，并通过线性插值逐步逼近目标位置。如果误差足够小，则停止运动。
5. 运行控制循环：调用robot.control函数运行控制循环。