Franka机械臂代码学习笔记1

关节空间运动生成

• franka::Robot::control方法参考control()

std::array initial_position;  
// 定义一个7维的初始关节角矩阵
double time = 0.0;
// 定义时间变量
robot.control([&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;
        // 给初始关节角矩阵赋值.这里q_d表示期望(desired)角度
      }
      double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
      // 这里表示δθ=π/8*(1-cos(2πt/5))
      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}};
      // 更新Franka的七个关节角为[θ0，θ1，θ2，θ3+δθ，θ4+δθ，θ5，θ6+δθ]
      if (time >= 5.0) 
      {
        std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
        // 如果t>5s，则程序结束
        return franka::MotionFinished(output);
      }
  return output;
});

        这是一个S形曲线运动规划。使用正余弦函数可以保证轨迹在任何位置无穷阶可微，即保证了光滑性。此时只要限制振幅就可以确保速度、加速度、加加速度不超限。

        一个有用的技巧是：不要直接给定阶跃控制目标。平滑过渡对于实际操作十分重要。原因或许是缺少一个内部插值机制，偏差过大会导致控制器输出达到峰值。

笛卡尔空间运动生成

控制机器人末端在x-z平面执行圆周运动，参考官方源代码

• constexpr double：常量表达式（const expression）是指值不会改变并且在编译过程就能得到计算结果的表达式参考资料。
• setCollisionBehavior(): 参考资料

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include 
#include 
#include 
#include 
#include "examples_common.h"
int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " " << std::endl;
    return -1;
  }
  try {
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // First move the robot to a suitable joint configuration
    std::array 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 initial_pose;
    double time = 0.0;
    robot.control([&time, &initial_pose](const franka::RobotState& robot_state,
                                         franka::Duration period) -> franka::CartesianPose {
      time += period.toSec();
      if (time == 0.0) {
        initial_pose = robot_state.O_T_EE_c;
      }
      constexpr double kRadius = 0.3;
      double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time));
      double delta_x = kRadius * std::sin(angle);
      double delta_z = kRadius * (std::cos(angle) - 1);
      std::array new_pose = initial_pose;
      new_pose[12] += delta_x;
      new_pose[14] += delta_z;
      if (time >= 10.0) {
        std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
        return franka::MotionFinished(new_pose);
      }
      return new_pose;
    });
  } catch (const franka::Exception& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }
  return 0;
}

阻抗控制与力控

Franka机器人的一大优势就是直接实时控制关节力矩，这让用户可以自由设计复杂控制策略。

以力矩为输入的运动控制核心代码：

  // Set and initialize trajectory parameters.
  const double radius = 0.05;
  const double vel_max = 0.25;
  const double acceleration_time = 2.0;
  const double run_time = 20.0;

  double vel_current = 0.0;
  double angle = 0.0;
  double time = 0.0;
    // Define callback function to send Cartesian pose goals to get inverse kinematics solved.
    auto cartesian_pose_callback = [=, &time, &vel_current, &running, &angle, &initial_pose](
                                       const franka::RobotState& robot_state,
                                       franka::Duration period) -> franka::CartesianPose {
      time += period.toSec();
      if (time == 0.0) {
        // Read the initial pose to start the motion from in the first time step.
        initial_pose = robot_state.O_T_EE_c;
      }
      // Compute Cartesian velocity.
      if (vel_current < vel_max && time < run_time) {
        vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);
      }
      if (vel_current > 0.0 && time > run_time) {
        vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
      }
      vel_current = std::fmax(vel_current, 0.0);
      vel_current = std::fmin(vel_current, vel_max);
      // Compute new angle for our circular trajectory.
      angle += period.toSec() * vel_current / std::fabs(radius);
      if (angle > 2 * M_PI) {
        angle -= 2 * M_PI;
      }
      // Compute relative y and z positions of desired pose.
      double delta_y = radius * (1 - std::cos(angle));
      double delta_z = radius * std::sin(angle);
      franka::CartesianPose pose_desired = initial_pose;
      pose_desired.O_T_EE[13] += delta_y;
      pose_desired.O_T_EE[14] += delta_z;
      // Send desired pose.
      if (time >= run_time + acceleration_time) {
        running = false;
        return franka::MotionFinished(pose_desired);
      }
      return pose_desired;
    };
    // Set gains for the joint impedance control.
    // Stiffness
    const std::array k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}};
    // Damping
    const std::array d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}};
    // Define callback for the joint torque control loop.
    std::function
        impedance_control_callback =
            [&print_data, &model, k_gains, d_gains](
                const franka::RobotState& state, franka::Duration /*period*/) -> franka::Torques {
      // Read current coriolis terms from model.
      std::array coriolis = model.coriolis(state);
      // Compute torque command from joint impedance control law.
      // Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one
      // time step delay.
      std::array tau_d_calculated;
      for (size_t i = 0; i < 7; i++) {
        tau_d_calculated[i] =
            k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
      }
      // Send torque command.
      return tau_d_rate_limited;
    };
    // Start real-time control loop.
    robot.control(impedance_control_callback, cartesian_pose_callback);

auto关键字可以参考auto知识点

逆解问题是 ill-posed problem。ill-posed problem不适定问题：经典的数学物理方程定解问题中，人们只研究适定问题。适定问题是指定解满足下面三个要求的问题：① 解是存在的；② 解是唯一的；③ 解连续依赖于定解条件，即解是稳定的。这三个要求中，只要有一个不满足，则称之为不适定问题。

外力估计与控制的核心代码：

    Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7);
    // Bias torque sensor
    std::array gravity_array = model.gravity(initial_state);
    Eigen::Map> initial_tau_measured(initial_state.tau_J.data());
    Eigen::Map> 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 jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
      Eigen::Map> jacobian(jacobian_array.data());
      Eigen::Map> tau_measured(robot_state.tau_J.data());
      Eigen::Map> 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 tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;
      return tau_d_array;
    };

Eigen是基于线性代数的C ++模板库，主要用于矩阵，向量，数值求解器和相关算法。

eigen中Maps是一个模板类，用于将顺序容器中的元素（或者说是一段连续内存）表达成eigen中矩阵类型如Matrix或者Vector，而不会造成任何内存和时间上的开销。其操作的对象是顺序容器、数组等能获得指向一段连续内存的指针。上文提到的例子中，vector是内存是连续的，所以可以直接应用Map映射成矩阵。

阻抗控制的核心代码：

 有关机器人阻抗控制的可以参考简述机器人阻抗控制 - 知乎

  // Compliance parameters
  const double translational_stiffness{150.0};
  const double rotational_stiffness{10.0};
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
                                     Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
                                         Eigen::MatrixXd::Identity(3, 3);
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();
    franka::RobotState initial_state = robot.readOnce();
    // equilibrium point is the initial position
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
    Eigen::Vector3d position_d(initial_transform.translation());
    Eigen::Quaterniond orientation_d(initial_transform.linear());
    // 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}});
    // define callback for the torque control loop
    std::function
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                         franka::Duration /*duration*/) -> franka::Torques {
      // get state variables
      std::array coriolis_array = model.coriolis(robot_state);
      std::array jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
      // convert to Eigen
      Eigen::Map> coriolis(coriolis_array.data());
      Eigen::Map> jacobian(jacobian_array.data());
      Eigen::Map> q(robot_state.q.data());
      Eigen::Map> dq(robot_state.dq.data());
      Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
      Eigen::Vector3d position(transform.translation());
      Eigen::Quaterniond orientation(transform.linear());
      // compute error to desired equilibrium pose
      // position error
      Eigen::Matrix error;
      error.head(3) << position - position_d;
      // orientation error
      // "difference" quaternion
      if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
        orientation.coeffs() << -orientation.coeffs();
      }
      // "difference" quaternion
      Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
      error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
      // Transform to base frame
      error.tail(3) << -transform.linear() * error.tail(3);
      // compute control
      Eigen::VectorXd tau_task(7), tau_d(7);
      // Spring damper system with damping ratio=1
      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
      tau_d << tau_task + coriolis;

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

---

参考博客深度解析Franka机器人的运动生成与控制——libfranka_止于至玄的博客-CSDN博客

还参考了机器人末端力/力矩控制实用简述——以Franka机器人为例_止于至玄的博客-CSDN博客_力矩控制

---

其他参考知识:

libfranka官方

FCI官方文档（franka）

Eign主页

非官方：Eigen库使用指南

非官方：Eigen与VS Code配置

非官方：Eigen快速入门