Franka libfranka 基于关节空间的速度控制

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

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

int main(int argc, char** argv) {
  try {
    // 连接到机器人
    franka::Robot robot("172.16.0.2");

    // 设置安全参数
    robot.setCollisionBehavior(
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}});

    // 获取当前关节角速度
    std::array<double, 7> dq_current;
    robot.readOnce(dq_current);
    std::cout << "Current joint velocities: ";
    for (double dq : dq_current) {
      std::cout << dq << " ";
    }
    std::cout << std::endl;

    // 设置目标关节角速度
    std::array<double, 7> dq_target = {0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5};

    // 设置最大关节角加速度
    double max_acceleration = 5.0; // rad/s^2

    // 生成关节空间速度轨迹
    franka::Duration duration(2.0); // 轨迹持续时间2秒
    franka::JointVelocities joint_velocities = robot.generateJointVelocities(
        dq_current, dq_target, duration, max_acceleration);

    // 控制机器人沿轨迹运动
    robot.control([&](const franka::RobotState& robot_state,
                     franka::Duration period) {
      return joint_velocities;
    });

    std::cout << "Trajectory completed." << std::endl;

    return 0;
  } catch (const franka::Exception& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }
}