Franak 给定目标关节空间位置， 规划并控制机器人运动

#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> q_current;
    robot.readOnce(q_current);
    std::cout << "Current joint positions: ";
    for (double q : q_current) {
      std::cout << q << " ";
    }
    std::cout << std::endl;

    // 设置目标关节角度
    std::array<double, 7> q_target = {-0.785, -0.785, -0.785, -0.785, -0.785, -0.785, 0.0};

    // 计算关节角度变化
    std::array<double, 7> dq = q_target - q_current;

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

    // 生成关节空间轨迹
    franka::Duration duration(2.0); // 轨迹持续时间2秒
    franka::JointVelocities joint_velocities = robot.generateJointVelocities(
        q_current, dq, duration, max_velocity, 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;
  }
}

这个示例代码包括以下步骤:

1. 连接到Franka Emika机器人。
2. 设置机器人的碰撞行为参数。
3. 获取当前关节角度。
4. 设置目标关节角度。
5. 计算关节角度变化。
6. 设置最大关节角速度和加速度。
7. 生成关节空间轨迹。
8. 控制机器人沿轨迹运动。

整个过程使用了libfranka提供的各种API函数,包括readOnce()、generateJointVelocities()和control()等。这样可以精确地控制机器人的关节角变化,实现所需的运动轨迹。