Franka阻抗控制代码

下面给出的是franka机械臂的阻抗控制代码，设置了安全保护

(1)把碰撞阈值robot.setCollisionBehavior设置的比较小，在遇到较大的力时会停下。

(2)设置了机械臂末端远离初始点的最大距离，目前设置为0.04m，可以根据需要进行修改

// 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 

#include 
#include 
#include 
#include 

#include "my_dcbf/examples_common.h"
#include "my_dcbf/head.h"

/**
 * @example cartesian_impedance_control.cpp
 * An example showing a simple cartesian impedance controller without inertia shaping
 * that renders a spring damper system where the equilibrium is the initial configuration.
 * After starting the controller try to push the robot around and try different stiffness levels.
 *
 * @warning collision thresholds are set to high values. Make sure you have the user stop at hand!
 */

int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " " << std::endl;
    return -1;
  }

  // 刚度和阻尼矩阵设计，阻尼设计为c=2*sqrt(k)，系统处于临界阻尼状态，可以以最快速度回到平衡状态并且不震荡
  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);

  try {
    // 连接机械臂
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // 加载运动学和动力学模型
    franka::Model model = robot.loadModel();

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

    // 平衡点是初始位置
    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());

    // // 设置碰撞行为
    // 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}});

   // 设置碰撞行为
    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}});






    int dof = 7;                                                                                                            //机械臂自由度
    Eigen::MatrixXd q = Eigen::MatrixXd::Zero(dof, 1);                       //关节角   
    Eigen::MatrixXd dq = Eigen::MatrixXd::Zero(dof, 1);                     //关节角速度    
    Eigen::MatrixXd ddq = Eigen::MatrixXd::Zero(dof, 1);                  //关节角加速度    
    Eigen::MatrixXd tau = Eigen::MatrixXd::Zero(dof, 1);  

    // 定义关节角的安全阈值
    Eigen::MatrixXd qlim_min(7,1),qlim_max(7,1), dqlim(7,1), ddqlim(7,1), taulim(7,1);     
    double coeff_q = 0.9;
    double coeff_dq = 0.6;
    double coeff_ddq = 0.5;
    double coeff_tau = 0.9;

    qlim_min << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
    qlim_max << 2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973;
    dqlim << 2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100;
    ddqlim << 15, 7.5, 10, 12.5, 15, 20, 20;
    taulim << 60, 60, 60, 60, 8, 8, 8;        // 技术手册上的的taulim << 87, 87, 87, 87, 12, 12, 12;

    qlim_min = qlim_min*coeff_q;
    qlim_max = qlim_max*coeff_q;
    dqlim = dqlim*coeff_dq;
    ddqlim = ddqlim*coeff_ddq;
    taulim = taulim*coeff_tau;

    // ******************************************机器人循环控制**************************************************
    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;

  // -------------------------------------------------------------限制机械臂的q, dq, tau--------------------------------------------------------------------------------


    // 获取机器人状态q,dq,tau信息
    Eigen::Map>  tau(robot_state.tau_J.data());


    // 机器人关节角，角速度，力矩约束
    for (int i = 0; i <= 6; i++) {
        if (q(i)<= qlim_min(i)|| q(i)>= qlim_max(i)) {
            std::cerr << "q" << i << "=" << q(i)<< std::endl;
            throw std::runtime_error("Aborting; Joint q limited exceeded ! ! ! ! ! !");
        }

        if (std::fabs(dq(i)) >= dqlim(i)) {
            std::cerr << "dq" << i << "=" << dq(i)<< std::endl;
            throw std::runtime_error("Aborting; Joint dq limited exceeded ! ! ! ! ! !");
        }

        if (std::fabs(tau(i)) >= taulim(i)) {
            std::cerr << "tau" << i << "=" << tau(i)<< std::endl;
            throw std::runtime_error("Aborting; Joint tau limited exceeded ! ! ! ! ! !");
        }
 


      // -------------------------------------------------------------限制机械臂的控制力矩-------------------------------------------------------------------------------
        if (std::fabs(tau_d(i)) >= taulim(i)) {
            std::cerr << "DCBF_tau_in" << i << "=" << tau(i)<< std::endl;
            throw std::runtime_error("Aborting; DCBF Tau Design limited exceeded ! ! ! ! ! !");
        }
    }
    
   // 平衡点是初始位置
    Eigen::Affine3d real_transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
    Eigen::Vector3d position_real(real_transform.translation());
    Eigen::Quaterniond orientation_real(real_transform.linear());
    std::cout<<"position_real="< 0.04 
        || std::abs(position_real(1) -position_d(1)) > 0.04 
        || std::abs(position_real(2) - position_d(2)) > 0.04 
        || (position_real - position_d).norm() > 0.04 ) {
        throw std::runtime_error("Aborting; too far away from starting pose!");
    }


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

    // start real-time control loop
    std::cout << "WARNING: Collision thresholds are set to high values. "
              << "Make sure you have the user stop at hand!" << std::endl
              << "After starting try to push the robot and see how it reacts." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(impedance_control_callback);

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

  return 0;
}