Franka Robot demo 外部控制循环结合关节位置生成运动(generate_joint_position_motion_external_control_loop.cpp)

// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <cmath>
#include <iostream>

#include <franka/active_control.h>
#include <franka/active_motion_generator.h>
#include <franka/exception.h>
#include <franka/robot.h>
#include "examples_common.h"
/**
 * @example generate_joint_position_motion_external_control_loop.cpp
 * An example showing how to generate a joint position motion with an external control loop..
 *
 * @warning Before executing this example, make sure there is enough space in front of the robot.
 */
/**
 * @example generate_joint_position_motion_external_control_loop.cpp
 * 一个演示如何通过外部控制循环生成关节位置运动的示例。
 *
 * @warning 在执行此示例之前，请确保机器人前方有足够的空间。
 */
int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

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

    // First move the robot to a suitable joint configuration
    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 << "WARNING: This example will move the robot! "
              << "Please make sure to have the user stop button at hand!" << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(motion_generator);
    std::cout << "Finished moving to initial joint configuration." << std::endl;

    // Set additional parameters always before the control loop, NEVER in the control loop!
    // Set collision behavior.
    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}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});

    std::array<double, 7> initial_position{{0, 0, 0, 0, 0, 0, 0}}; // 初始化位置
    double time = 0.0; //运行时间
    auto control_callback = [&initial_position, &time](
                                const franka::RobotState& robot_state,
                                franka::Duration period) -> franka::JointPositions {
      time += period.toSec();

      if (time == 0.0) {
        initial_position = robot_state.q_d;
      }

      double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));// 使用一个调整后的余弦函数来生成一个随时间变化的角度偏移量，从而可以用来控制关节的运动

      franka::JointPositions output = {{initial_position[0], initial_position[1],
                                        initial_position[2], initial_position[3] + delta_angle,
                                        initial_position[4] + delta_angle, initial_position[5],
                                        initial_position[6] + delta_angle}};

      if (time >= 5.0) {// 5秒后停止
        std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
        return franka::MotionFinished(output);
      }
      return output;
    };

    bool motion_finished = false;
    auto active_control = robot.startJointPositionControl(
        research_interface::robot::Move::ControllerMode::kJointImpedance);
    while (!motion_finished) {
      auto read_once_return = active_control->readOnce();
      auto robot_state = read_once_return.first;
      auto duration = read_once_return.second;
      auto joint_positions = control_callback(robot_state, duration);// 获取机器人位置
      motion_finished = joint_positions.motion_finished;
      active_control->writeOnce(joint_positions); // 写入机器人位置
    }

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

  return 0;
}

• 初始化和连接：通过命令行参数连接到机器人，并设置默认行为。
• 运动到初始位置：将机器人移动到一个预定义的关节配置，以确保其处于合适的初始位置。
• 设置碰撞行为：在控制循环之前设置碰撞行为参数，以确保机器人在运动过程中能够正确处理碰撞。
• 关节位置控制循环：通过调整后的余弦函数生成关节位置，并控制关节运动。在控制循环中，机器人关节会按照一个正弦波的模式进行运动，直到运动完成。
• 异常处理：捕获并处理可能出现的异常，确保程序能够正确终止。

这个示例展示了如何通过外部控制循环生成机器人关节位置运动，并确保安全和稳定的操作。