Franka例程学习——force_control

一、实现任务

这段代码实现了一个简单的PI（比例-积分）力控制器，用于控制机器人的末端执行器在Z轴方向上施加一个与目标质量（1 kg）相对应的重力力。该控制器计算施加在机器人末端执行器上的力，以使机器人在Z轴方向上模拟一个目标质量的重力影响。代码的主要部分包括机器人的连接、状态获取、力控制回调函数的实现以及实际的控制循环。

二、源代码

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

#include <Eigen/Core>

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

#include "examples_common.h"

/**
 * @example force_control.cpp
 * A simple PI force controller that renders in the Z axis the gravitational force corresponding
 * to a target mass of 1 kg.
 *
 * @warning: make sure that no endeffector is mounted and that the robot's last joint is in contact
 * with a horizontal rigid surface before starting.
 */

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;
  }
  // parameters
  double desired_mass{0.0};
  constexpr double target_mass{1.0};    // NOLINT(readability-identifier-naming)
  constexpr double k_p{1.0};            // NOLINT(readability-identifier-naming)
  constexpr double k_i{2.0};            // NOLINT(readability-identifier-naming)
  constexpr double filter_gain{0.001};  // NOLINT(readability-identifier-naming)

  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();

    // set collision behavior
    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

    franka::RobotState initial_state = robot.readOnce();

    Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7);
    // Bias torque sensor
    std::array<double, 7> gravity_array = model.gravity(initial_state);
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());
    Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());
    initial_tau_ext = initial_tau_measured - initial_gravity;

    // init integrator
    tau_error_integral.setZero();

    // define callback for the torque control loop
    Eigen::Vector3d initial_position;
    double time = 0.0;
    auto get_position = [](const franka::RobotState& robot_state) {
      return Eigen::Vector3d(robot_state.O_T_EE[12], robot_state.O_T_EE[13],
                             robot_state.O_T_EE[14]);
    };
    auto force_control_callback = [&](const franka::RobotState& robot_state,
                                      franka::Duration period) -> franka::Torques {
      time += period.toSec();

      if (time == 0.0) {
        initial_position = get_position(robot_state);
      }

      if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
        throw std::runtime_error("Aborting; too far away from starting pose!");
      }

      // get state variables
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);

      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());

      Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
      desired_force_torque.setZero();
      desired_force_torque(2) = desired_mass * -9.81;
      tau_ext << tau_measured - gravity - initial_tau_ext;
      tau_d << jacobian.transpose() * desired_force_torque;
      tau_error_integral += period.toSec() * (tau_d - tau_ext);
      // FF + PI control
      tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral;

      // Smoothly update the mass to reach the desired target value
      desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;

      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;
      return tau_d_array;
    };
    std::cout << "WARNING: Make sure sure that no endeffector is mounted and that the robot's last "
                 "joint is "
                 "in contact with a horizontal rigid surface before starting. Keep in mind that "
                 "collision thresholds are set to high values."
              << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    // start real-time control loop
    robot.control(force_control_callback);

  } catch (const std::exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }
  return 0;
}

三、代码解读

1. 引入头文件和命名空间

#include <array>
#include <iostream>
#include <Eigen/Core>
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>
#include "examples_common.h"

array, iostream, Eigen/Core 等库提供数组、输入输出、矩阵运算等功能。
franka/duration.h, franka/exception.h, franka/model.h, franka/robot.h 是 Franka Emika 机器人 SDK 的头文件，用于与机器人进行交互。
examples_common.h 是一个常见的头文件，可能包含一些通用的帮助函数或设置。

2. 命令行参数检查

if (argc != 2) {
  std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
  return -1;
}

检查是否传递了正确的命令行参数（机器人主机名）。如果没有提供主机名，则输出使用说明并返回错误。

3. 参数设置

double desired_mass{0.0};
constexpr double target_mass{1.0};
constexpr double k_p{1.0};
constexpr double k_i{2.0};
constexpr double filter_gain{0.001};

desired_mass 是目标质量，初始化为0。
target_mass 设定为 1.0 kg，作为目标质量。
k_p 和 k_i 分别是比例增益和积分增益，用于PI控制器。
filter_gain 用于平滑更新 desired_mass。

4. 连接到机器人并加载模型

franka::Robot robot(argv[1]);
setDefaultBehavior(robot);
franka::Model model = robot.loadModel();

通过传递的机器人主机名 (argv[1]) 连接到机器人。
setDefaultBehavior(robot) 设置默认行为，通常是安全控制、碰撞处理等。
robot.loadModel() 加载机器人动力学和运动学模型，用于计算重力、雅可比矩阵等。

5. 设置碰撞行为

robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                           {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                           {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                           {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

设定碰撞行为。这里的数值可能表示碰撞时的力学阈值，确保机器人在遇到障碍物时不会受到损害。

6. 获取初始状态并初始化

franka::RobotState initial_state = robot.readOnce();
Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7);
std::array<double, 7> gravity_array = model.gravity(initial_state);
Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());
Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());
initial_tau_ext = initial_tau_measured - initial_gravity;
tau_error_integral.setZero();

获取机器人当前的状态（initial_state）。此状态包含机器人各关节的位置、速度、力矩等信息。
model.gravity(initial_state) 计算重力效应。
initial_tau_measured - initial_gravity 计算外部力矩（tau_ext），即从力矩传感器中测量到的力矩与重力引起的力矩的差。
初始化 tau_error_integral 为零，用于 PI 控制器的积分部分。

7. 定义位置获取函数

Eigen::Vector3d initial_position;
auto get_position = [](const franka::RobotState& robot_state) {
  return Eigen::Vector3d(robot_state.O_T_EE[12], robot_state.O_T_EE[13],
                         robot_state.O_T_EE[14]);
};

get_position 是一个 Lambda 函数，用于提取机器人末端执行器的位置。robot_state.O_T_EE 是一个 4x4 的变换矩阵，表示末端执行器相对于基座的位姿（位置和方向）。从矩阵中提取位置部分（即矩阵的第12、13、14个元素）得到末端执行器的 XYZ 坐标。

8. 定义力控制回调函数

auto force_control_callback = [&](const franka::RobotState& robot_state, franka::Duration period) -> franka::Torques {
  time += period.toSec();
  if (time == 0.0) {
    initial_position = get_position(robot_state);
  }

  if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
    throw std::runtime_error("Aborting; too far away from starting pose!");
  }

force_control_callback 是一个回调函数，每次调用时根据当前时间 period.toSec() 更新控制周期。回调的目标是控制机器人产生预定的力矩。
如果机器人的末端执行器位置偏离初始位置超过 0.01 米，抛出异常并终止程序。

9. 获取雅可比矩阵和力矩数据

std::array<double, 42> jacobian_array = model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());

model.zeroJacobian 计算末端执行器的雅可比矩阵，表示末端执行器的线性和角速度如何影响机器人的各个关节速度。
获取当前测量的力矩（tau_measured）和重力效应（gravity）。

10. 控制算法：PI控制器

Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
desired_force_torque.setZero();
desired_force_torque(2) = desired_mass * -9.81; // 设置 Z 轴上的目标力矩
tau_ext << tau_measured - gravity - initial_tau_ext;
tau_d << jacobian.transpose() * desired_force_torque;
tau_error_integral += period.toSec() * (tau_d - tau_ext); // 积分项
tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral; // PI控制

计算目标力矩（desired_force_torque），其中 Z 轴的力是目标质量与重力加速度的乘积（1 kg × 9.81 m/s²）。
使用雅可比矩阵计算所需的关节力矩（tau_d）。
更新积分项，使用 PI 控制公式计算最终的力矩命令（tau_cmd）。

11. 平滑更新目标质量

desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;

desired_mass 在每个周期内逐渐接近目标质量（1 kg），通过滤波器平滑过渡。

12. 返回力矩命令

std::array<double, 7> tau_d_array{};
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;
return tau_d_array;

将计算出的力矩命令 tau_cmd 转换为 std::array<double, 7> 类型，并返回。

13. 启动控制循环

robot.control(force_control_callback);

启动实时控制循环，robot.control 会不断调用 force_control_callback，实现持续的力控制。

14. 错误处理

} catch (const std::exception& ex) {
  std::cout << ex.what() << std::endl;
}

如果在控制过程中出现异常，捕获并输出异常信息

Franka例程学习——force_control

一、实现任务

二、源代码

三、代码解读

悦读