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, 16> transform_current;
    robot.readOnce(transform_current);
    std::cout << "Current end-effector pose: ";
    for (double t : transform_current) {
      std::cout << t << " ";
    }
    std::cout << std::endl;

    // 设置目标末端执行器位置
    std::array<double, 16> transform_target = {
        1, 0, 0, 0.5,
        0, 1, 0, 0.2,
        0, 0, 1, 0.8,
        0, 0, 0, 1
    };

    // 设置最大线速度和角速度
    double max_translational_velocity = 0.1;  // m/s
    double max_rotational_velocity = 0.5; // rad/s

    // 生成笛卡尔空间轨迹
    franka::Duration duration(2.0); // 轨迹持续时间2秒
    franka::CartesianVelocities cartesian_velocities = robot.generateCartesianVelocities(
        transform_current, transform_target, duration,
        max_translational_velocity, max_rotational_velocity);

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

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

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