Franka Robot demo 外部控制循环下的笛卡尔速度运动(generate_cartesian_velocity_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_cartesian_velocity_motion_external_control_loop.cpp
 * An example showing how to generate a Cartesian velocity motion with an external control loop.
 *
 * @warning Before executing this example, make sure there is enough space in front of the robot.
 */
/**
 * @example generate_cartesian_velocity_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 the joint impedance.
    robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});

    // Set the collision behavior.
    std::array<double, 7> lower_torque_thresholds_nominal{
        {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
    std::array<double, 7> upper_torque_thresholds_nominal{
        {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
    std::array<double, 7> lower_torque_thresholds_acceleration{
        {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
    std::array<double, 7> upper_torque_thresholds_acceleration{
        {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
    std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
    std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
    std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
    std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
    robot.setCollisionBehavior(
        lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,
        lower_torque_thresholds_nominal, upper_torque_thresholds_nominal,
        lower_force_thresholds_acceleration, upper_force_thresholds_acceleration,
        lower_force_thresholds_nominal, upper_force_thresholds_nominal);

    double time_max = 4.0;      // 最长时间
    double v_max = 0.1;         // 最大速度
    double angle = M_PI / 4.0;  // 角度
    double time = 0.0;          // 累加时长

    auto callback_control = [=, &time](const franka::RobotState&,// 规划运动速度
                                       franka::Duration period) -> franka::CartesianVelocities {
      time += period.toSec();

      double cycle = std::floor(pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); // 周期
      double v = cycle * v_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); // 速度
      double v_x = std::cos(angle) * v;  // x轴速度
      double v_z = -std::sin(angle) * v; // y轴速度 

      franka::CartesianVelocities output = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}}; // 迪卡尔速度运动构建命令对象
      if (time >= 2 * time_max) {
        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.startCartesianVelocityControl(
        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 cartesian_velocities = callback_control(robot_state, duration); // 回调计算迪卡尔速度
      motion_finished = cartesian_velocities.motion_finished;
      active_control->writeOnce(cartesian_velocities); // 发布迪卡尔速度控制命令
    }

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

  return 0;
}

 

 

代码逻辑概述

1. 初始化和连接：

   • 检查命令行参数，确保提供了机器人的主机名。
   • 通过 franka::Robot 类连接到机器人，并调用 setDefaultBehavior 设置默认行为。

2. 初始位置设置：

   • 定义目标关节配置 q_goal，包含七个关节角度。
   • 创建 MotionGenerator 对象，用于生成移动到目标位置的关节运动。
   • 提示用户按下回车键以继续，并将机器人移动到初始关节配置。

3. 设置关节阻抗和碰撞行为：

   • 调用 setJointImpedance 设置每个关节的阻抗值。
   • 调用 setCollisionBehavior 设置各种碰撞行为参数，包括加速和正常工作条件下的扭矩和力的阈值。

4. 生成笛卡尔速度运动：

   • 定义运动参数，如最大时间 time_max、最大速度 v_max 和角度 angle。
   • 使用回调函数 callback_control 定义一个控制循环，在该循环中生成笛卡尔速度。
   • 在控制循环中，通过时间周期计算出当前速度 v，并将其分解为 x 和 z 方向的速度分量 v_x 和 v_z。
   • 创建并返回 franka::CartesianVelocities 对象，包含当前的速度分量。
   • 如果总时间超过两倍的最大时间，则结束运动并返回 MotionFinished。

5. 外部控制循环：

   • 使用 startCartesianVelocityControl 启动笛卡尔速度控制模式。
   • 在循环中读取机器人状态，并调用回调函数计算笛卡尔速度，然后写入控制命令。
   • 检查运动是否完成，完成后退出循环。

6. 异常处理：

   • 捕获并处理 franka::Exception，输出错误信息并返回-1。

总结来说，该代码通过外部控制循环生成笛卡尔速度运动，确保机器人按照预定义的轨迹安全地运动。