Franka例程学习——joint_point_to_point_motion

一、实现任务

首先简单介绍一下这个代码的实现功能

输入：目标关节角，关节速度因子

输出：机械臂从初始位姿，以最大速度到达目标关节角

二、完整代码

cpp 复制代码

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

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

#include "examples_common.h"

/**
 * @example joint_point_to_point_motion.cpp
 * An example that moves the robot to a target position by commanding joint positions.
 *
 * @warning Before executing this example, make sure there is enough space in front of the robot.
 */

int main(int argc, char** argv) {
  if (argc != 10) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname> "
              << "<joint0> <joint1> <joint2> <joint3> <joint4> <joint5> <joint6> "
              << "<speed-factor>" << std::endl
              << "joint0 to joint6 are joint angles in [rad]." << std::endl
              << "speed-factor must be between zero and one." << std::endl;
    return -1;
  }
  try {
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);

    std::array<double, 7> q_goal;
    for (size_t i = 0; i < 7; i++) {
      q_goal[i] = std::stod(argv[i + 2]);
    }
    double speed_factor = std::stod(argv[9]);

    // Set additional parameters always before the control loop, NEVER in the control loop!
    // Set collision behavior.
    robot.setCollisionBehavior(
        {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
        {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
        {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
        {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});

    MotionGenerator motion_generator(speed_factor, 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 << "Motion finished" << std::endl;
  } catch (const franka::Exception& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }

  return 0;
}

三、代码解读

1、首先要验证输入的参数是否满足10个参数

cpp 复制代码

  if (argc != 10) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname> "
              << "<joint0> <joint1> <joint2> <joint3> <joint4> <joint5> <joint6> "
              << "<speed-factor>" << std::endl
              << "joint0 to joint6 are joint angles in [rad]." << std::endl
              << "speed-factor must be between zero and one." << std::endl;
    return -1;
  }

argc != 10：如果传入的参数数量不是 10，则会输出一个错误信息并退出程序。
错误提示信息显示：
cpp 复制代码
```
Usage: <program-name> <robot-hostname> <joint0> <joint1> <joint2> <joint3> <joint4> <joint5> <joint6> <speed-factor>
```
- robot-hostname：机器人主机名（可能是一个字符串，用于指定机器人的连接地址或名称）。
- joint0 到 joint6：表示机器人的七个关节角度，这些值的单位是弧度（rad），一共 7 个参数。
- speed-factor：这是一个速度因子，必须在 0 到 1 之间，用于调整机器人的运动速度。

2. 创建并初始化机器人对象：

cpp 复制代码

   franka::Robot robot(argv[1]);

这一行代码使用 argv $1$ 中传入的参数（即机器人的主机名或 IP 地址）来创建一个 franka::Robot 类型的对象 robot。这通常是用来连接并与机器人进行交互的。
3.设置默认行为：

cpp 复制代码

   setDefaultBehavior(robot);

这行调用了一个名为 setDefaultBehavior 的函数，该函数是用来配置机器人默认的行为或状态。它为机器人设置了一些初始化或默认设置。
4.读取目标关节角度并保存到 q_goal 数组：

cpp 复制代码

   std::array&lt;double, 7&gt; q_goal;
   for (size_t i = 0; i &lt; 7; i++) {
       q_goal[i] = std::stod(argv[i + 2]);
   }

这段代码声明了一个包含 7 个元素的数组 q_goal，用于存储目标关节角度。for 循环遍历了 argv 数组中的 argv $2$ 到 argv $8$ （即命令行输入的关节角度参数），并使用 std::stod() 函数将它们从字符串转换为 double 类型，赋值给 q_goal $i$ 。这相当于从命令行参数中读取机器人关节的目标角度。
5.读取速度因子并保存到 speed_factor：

cpp 复制代码

 double speed_factor = std::stod(argv[9]);

这一行代码将命令行中的最后一个参数 argv $9$ （即速度因子）转换为 double 类型，并将其赋值给 speed_factor。该速度因子将用于控制机器人的移动速度。

6. 设置碰撞行为：

cpp 复制代码

robot.setCollisionBehavior(
    {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
    {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
    {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
    {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});

setCollisionBehavior 方法配置了机器人的碰撞行为。机器人在运动时，可能会与外部环境发生碰撞或接触。这个方法通过设置各种碰撞的行为参数来定义机器人在碰撞情况下的反应。

这些参数通常是控制机器人的关节速度、力矩、加速度等。

第一组参数：机器人各个关节的碰撞行为（可能是阈值）设置为 20.0。

第二组参数：与第一组相同，可能代表机器人的力矩限制。

第三组和第四组：类似的设置，但值较小，分别为 10.0。

最后的两组设置也类似，可能代表不同的力矩或速度限制。

7. 初始化运动生成器（这行代码是核心）：

cpp 复制代码

MotionGenerator motion_generator(speed_factor, q_goal);

MotionGenerator 是一个用于生成机器人运动轨迹的类（假设在其他地方定义）。传递给它的参数是 speed_factor 和 q_goal，即：speed_factor：之前从命令行传入的速度因子，用于控制机器人的运动速度。q_goal：目标关节角度数组，表示机器人期望到达的位置。

8. 用户提示并等待输入：

cpp 复制代码

std::cout &lt;&lt; "WARNING: This example will move the robot! "
          &lt;&lt; "Please make sure to have the user stop button at hand!" &lt;&lt; std::endl
          &lt;&lt; "Press Enter to continue..." &lt;&lt; std::endl;
std::cin.ignore();

在实际控制机器人之前，给用户一个警告，说明这段代码将会移动机器人。并提示用户准备好紧急停止按钮以防万一。

程序等待用户按下回车键才能继续执行。std::cin.ignore() 让程序暂停，直到用户输入。

9. 开始控制机器人（让机器人动起来：

cpp 复制代码

robot.control(motion_generator);

这行代码启动了机器人的控制，传递了之前创建的 motion_generator。该生成器包含了机器人目标关节角度和速度因子，机器人将根据这些参数执行运动。

10. 完成运动后的输出：

cpp 复制代码

std::cout &lt;&lt; "Motion finished" &lt;&lt; std::endl;

运动完成后，输出 "Motion finished" 提示，表明机器人已经完成了运动。

11. 异常处理：

cpp 复制代码

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

如果在执行过程中发生任何 franka::Exception 异常（例如机器人通信失败或其他错误），程序将捕获并输出异常信息。如果捕获到异常，程序返回 -1，表示发生错误。