Franka Robot demo 当前关节位置平滑地移动到目标关节位置

// 为了从当前关节位置平滑地移动到目标关节位置，我们可以使用Franka Emika提供的运动生成器（MotionGenerator）来实现。
// 以下是一个简单的示例代码，展示了如何从当前关节位置移动到目标关节位置：
// 版权所有 (c) 2023 Franka Robotics GmbH
// 本源代码的使用遵循 Apache-2.0 许可，参见 LICENSE
#include <cmath>
#include <iostream>

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

#include "examples_common.h"

/**
 * @example move_to_joint_position.cpp
 * 一个演示如何从当前关节位置移动到目标关节位置的示例。
 *
 * @warning 在执行此示例之前，请确保机器人前方有足够的空间。
 */

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "使用方法: " << argv[0] << " <机器人主机名>" << std::endl;
    return -1;
  }

  try {
    franka::Robot robot(argv[1]); // 连接机器人
    setDefaultBehavior(robot); // 设置默认行为

    // 获取当前关节位置
    franka::RobotState initial_state = robot.readOnce();
    std::array<double, 7> initial_position = initial_state.q;

    // 设置目标关节位置
    std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};

    // 创建运动生成器，从当前关节位置移动到目标位置
    MotionGenerator motion_generator(0.5, q_goal);

    std::cout << "警告: 这个示例将移动机器人! "
              << "请确保用户停止按钮在手边!" << std::endl
              << "按 Enter 键继续..." << std::endl;
    std::cin.ignore();

    // 控制机器人运动到目标位置
    robot.control(motion_generator);
    std::cout << "完成移动到目标关节位置。" << std::endl;
  } catch (const franka::Exception& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }

  return 0;
}

代码解释

• 初始化和连接：通过命令行参数连接到机器人，并设置默认行为。
• 获取当前关节位置：使用robot.readOnce()获取当前的关节位置。
• 设置目标关节位置：定义一个目标关节位置数组。
• 创建运动生成器：使用MotionGenerator从当前关节位置移动到目标位置。
• 运动控制：使用robot.control(motion_generator)控制机器人运动到目标位置。
• 异常处理：捕获并处理可能出现的异常，确保程序能够正确终止。

注意事项

• 确保安全：在执行此示例之前，请确保机器人前方有足够的空间，以避免碰撞。
• 用户停止按钮：在执行过程中，确保用户停止按钮在手边，以便在紧急情况下可以立即停止机器人。

这个示例展示了如何从当前关节位置平滑地移动到指定的目标关节位置，确保运动的安全性和稳定性。