devbox
盐析白兔
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Franka机械臂代码学习笔记1_机械臂外力估计

作者：盐析白兔 | 2024-06-22 15:40:55

赞

踩

机械臂外力估计

关节空间运动生成

  1. std::array<double, 7> initial_position;  
  2. // 定义一个7维的初始关节角矩阵
  3. double time = 0.0;
  4. // 定义时间变量
  5. robot.control([&initial_position, &time](const franka::RobotState& robot_state,
  6.                                              franka::Duration period) -> franka::JointPositions 
  7. {
  8.       time += period.toSec();
  9.       // 回调开始时的更新时间
  10.       if (time == 0.0) 
  11.       {
  12.         initial_position = robot_state.q_d;
  13.         // 给初始关节角矩阵赋值.这里q_d表示期望(desired)角度
  14.       }
  15.       double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
  16.       // 这里表示δθ=π/8*(1-cos(2πt/5))
  17.       franka::JointPositions output = {{initial_position[0], initial_position[1],
  18.                                         initial_position[2], initial_position[3] + delta_angle,
  19.                                         initial_position[4] + delta_angle, initial_position[5],
  20.                                         initial_position[6] + delta_angle}};
  21.       // 更新Franka的七个关节角为[θ0，θ1，θ2，θ3+δθ，θ4+δθ，θ5，θ6+δθ]
  22.       if (time >= 5.0) 
  23.       {
  24.         std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
  25.         // 如果t>5s，则程序结束
  26.         return franka::MotionFinished(output);
  27.       }
  28.   return output;
  29. });

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

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

笛卡尔空间运动生成

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

  • constexpr double：常量表达式（const expression）是指值不会改变并且在编译过程就能得到计算结果的表达式参考资料。
  • setCollisionBehavior(): 参考资料
  1. // Copyright (c) 2017 Franka Emika GmbH
  2. // Use of this source code is governed by the Apache-2.0 license, see LICENSE
  3. #include <cmath>
  4. #include <iostream>
  5. #include <franka/exception.h>
  6. #include <franka/robot.h>
  7. #include "examples_common.h"
  8. int main(int argc, char** argv) {
  9.   if (argc != 2) {
  10.     std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
  11.     return -1;
  12.   }
  13.   try {
  14.     franka::Robot robot(argv[1]);
  15.     setDefaultBehavior(robot);
  16.     // First move the robot to a suitable joint configuration
  17.     std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
  18.     MotionGenerator motion_generator(0.5, q_goal);
  19.     std::cout << "WARNING: This example will move the robot! "
  20.               << "Please make sure to have the user stop button at hand!" << std::endl
  21.               << "Press Enter to continue..." << std::endl;
  22.     std::cin.ignore();
  23.     robot.control(motion_generator);
  24.     std::cout << "Finished moving to initial joint configuration." << std::endl;
  25.     // Set additional parameters always before the control loop, NEVER in the control loop!
  26.     // Set collision behavior.
  27.     robot.setCollisionBehavior(
  28.         {{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}},
  29.         {{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}},
  30.         {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
  31.         {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});
  32.     std::array<double, 16> initial_pose;
  33.     double time = 0.0;
  34.     robot.control([&time, &initial_pose](const franka::RobotState& robot_state,
  35.                                          franka::Duration period) -> franka::CartesianPose {
  36.       time += period.toSec();
  37.       if (time == 0.0) {
  38.         initial_pose = robot_state.O_T_EE_c;
  39.       }
  40.       constexpr double kRadius = 0.3;
  41.       double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time));
  42.       double delta_x = kRadius * std::sin(angle);
  43.       double delta_z = kRadius * (std::cos(angle) - 1);
  44.       std::array<double, 16> new_pose = initial_pose;
  45.       new_pose[12] += delta_x;
  46.       new_pose[14] += delta_z;
  47.       if (time >= 10.0) {
  48.         std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
  49.         return franka::MotionFinished(new_pose);
  50.       }
  51.       return new_pose;
  52.     });
  53.   } catch (const franka::Exception& e) {
  54.     std::cout << e.what() << std::endl;
  55.     return -1;
  56.   }
  57.   return 0;
  58. }

阻抗控制与力控

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

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

  1.   // Set and initialize trajectory parameters.
  2.   const double radius = 0.05;
  3.   const double vel_max = 0.25;
  4.   const double acceleration_time = 2.0;
  5.   const double run_time = 20.0;
  6.  
  7.   double vel_current = 0.0;
  8.   double angle = 0.0;
  9.   double time = 0.0;
  10.     // Define callback function to send Cartesian pose goals to get inverse kinematics solved.
  11.     auto cartesian_pose_callback = [=, &time, &vel_current, &running, &angle, &initial_pose](
  12.                                        const franka::RobotState& robot_state,
  13.                                        franka::Duration period) -> franka::CartesianPose {
  14.       time += period.toSec();
  15.       if (time == 0.0) {
  16.         // Read the initial pose to start the motion from in the first time step.
  17.         initial_pose = robot_state.O_T_EE_c;
  18.       }
  19.       // Compute Cartesian velocity.
  20.       if (vel_current < vel_max && time < run_time) {
  21.         vel_current += period.toSec() * std::fabs(vel_max / acceleration_time);
  22.       }
  23.       if (vel_current > 0.0 && time > run_time) {
  24.         vel_current -= period.toSec() * std::fabs(vel_max / acceleration_time);
  25.       }
  26.       vel_current = std::fmax(vel_current, 0.0);
  27.       vel_current = std::fmin(vel_current, vel_max);
  28.       // Compute new angle for our circular trajectory.
  29.       angle += period.toSec() * vel_current / std::fabs(radius);
  30.       if (angle > 2 * M_PI) {
  31.         angle -= 2 * M_PI;
  32.       }
  33.       // Compute relative y and z positions of desired pose.
  34.       double delta_y = radius * (1 - std::cos(angle));
  35.       double delta_z = radius * std::sin(angle);
  36.       franka::CartesianPose pose_desired = initial_pose;
  37.       pose_desired.O_T_EE[13] += delta_y;
  38.       pose_desired.O_T_EE[14] += delta_z;
  39.       // Send desired pose.
  40.       if (time >= run_time + acceleration_time) {
  41.         running = false;
  42.         return franka::MotionFinished(pose_desired);
  43.       }
  44.       return pose_desired;
  45.     };
  46.     // Set gains for the joint impedance control.
  47.     // Stiffness
  48.     const std::array<double, 7> k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}};
  49.     // Damping
  50.     const std::array<double, 7> d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}};
  51.     // Define callback for the joint torque control loop.
  52.     std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
  53.         impedance_control_callback =
  54.             [&print_data, &model, k_gains, d_gains](
  55.                 const franka::RobotState& state, franka::Duration /*period*/) -> franka::Torques {
  56.       // Read current coriolis terms from model.
  57.       std::array<double, 7> coriolis = model.coriolis(state);
  58.       // Compute torque command from joint impedance control law.
  59.       // Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one
  60.       // time step delay.
  61.       std::array<double, 7> tau_d_calculated;
  62.       for (size_t i = 0; i < 7; i++) {
  63.         tau_d_calculated[i] =
  64.             k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i];
  65.       }
  66.       // Send torque command.
  67.       return tau_d_rate_limited;
  68.     };
  69.     // Start real-time control loop.
  70.     robot.control(impedance_control_callback, cartesian_pose_callback);

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

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

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

  1.     Eigen::VectorXd initial_tau_ext(7), tau_error_integral(7);
  2.     // Bias torque sensor
  3.     std::array<double, 7> gravity_array = model.gravity(initial_state);
  4.     Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_tau_measured(initial_state.tau_J.data());
  5.     Eigen::Map<Eigen::Matrix<double, 7, 1>> initial_gravity(gravity_array.data());
  6.     initial_tau_ext = initial_tau_measured - initial_gravity;
  7.     // init integrator
  8.     tau_error_integral.setZero();
  9.     // define callback for the torque control loop
  10.     Eigen::Vector3d initial_position;
  11.     double time = 0.0;
  12.     auto get_position = [](const franka::RobotState& robot_state) {
  13.       return Eigen::Vector3d(robot_state.O_T_EE[12], robot_state.O_T_EE[13],
  14.                              robot_state.O_T_EE[14]);
  15.     };
  16.     auto force_control_callback = [&](const franka::RobotState& robot_state,
  17.                                       franka::Duration period) -> franka::Torques {
  18.       time += period.toSec();
  19.       if (time == 0.0) {
  20.         initial_position = get_position(robot_state);
  21.       }
  22.       if (time > 0 && (get_position(robot_state) - initial_position).norm() > 0.01) {
  23.         throw std::runtime_error("Aborting; too far away from starting pose!");
  24.       }
  25.       // get state variables
  26.       std::array<double, 42> jacobian_array =
  27.           model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
  28.       Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
  29.       Eigen::Map<const Eigen::Matrix<double, 7, 1>> tau_measured(robot_state.tau_J.data());
  30.       Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
  31.       Eigen::VectorXd tau_d(7), desired_force_torque(6), tau_cmd(7), tau_ext(7);
  32.       desired_force_torque.setZero();
  33.       desired_force_torque(2) = desired_mass * -9.81;
  34.       tau_ext << tau_measured - gravity - initial_tau_ext;
  35.       tau_d << jacobian.transpose() * desired_force_torque;
  36.       tau_error_integral += period.toSec() * (tau_d - tau_ext);
  37.       // FF + PI control
  38.       tau_cmd << tau_d + k_p * (tau_d - tau_ext) + k_i * tau_error_integral;
  39.       // Smoothly update the mass to reach the desired target value
  40.       desired_mass = filter_gain * target_mass + (1 - filter_gain) * desired_mass;
  41.       std::array<double, 7> tau_d_array{};
  42.       Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_cmd;
  43.       return tau_d_array;
  44.     };

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

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

阻抗控制的核心代码：

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

  1.   // Compliance parameters
  2.   const double translational_stiffness{150.0};
  3.   const double rotational_stiffness{10.0};
  4.   Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  5.   stiffness.setZero();
  6.   stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  7.   stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  8.   damping.setZero();
  9.   damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
  10.                                      Eigen::MatrixXd::Identity(3, 3);
  11.   damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
  12.                                          Eigen::MatrixXd::Identity(3, 3);
  13.     // connect to robot
  14.     franka::Robot robot(argv[1]);
  15.     setDefaultBehavior(robot);
  16.     // load the kinematics and dynamics model
  17.     franka::Model model = robot.loadModel();
  18.     franka::RobotState initial_state = robot.readOnce();
  19.     // equilibrium point is the initial position
  20.     Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
  21.     Eigen::Vector3d position_d(initial_transform.translation());
  22.     Eigen::Quaterniond orientation_d(initial_transform.linear());
  23.     // set collision behavior
  24.     robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
  25.                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
  26.                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
  27.                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
  28.     // define callback for the torque control loop
  29.     std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
  30.         impedance_control_callback = [&](const franka::RobotState& robot_state,
  31.                                          franka::Duration /*duration*/) -> franka::Torques {
  32.       // get state variables
  33.       std::array<double, 7> coriolis_array = model.coriolis(robot_state);
  34.       std::array<double, 42> jacobian_array =
  35.           model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
  36.       // convert to Eigen
  37.       Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
  38.       Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
  39.       Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
  40.       Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
  41.       Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
  42.       Eigen::Vector3d position(transform.translation());
  43.       Eigen::Quaterniond orientation(transform.linear());
  44.       // compute error to desired equilibrium pose
  45.       // position error
  46.       Eigen::Matrix<double, 6, 1> error;
  47.       error.head(3) << position - position_d;
  48.       // orientation error
  49.       // "difference" quaternion
  50.       if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
  51.         orientation.coeffs() << -orientation.coeffs();
  52.       }
  53.       // "difference" quaternion
  54.       Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
  55.       error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
  56.       // Transform to base frame
  57.       error.tail(3) << -transform.linear() * error.tail(3);
  58.       // compute control
  59.       Eigen::VectorXd tau_task(7), tau_d(7);
  60.       // Spring damper system with damping ratio=1
  61.       tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
  62.       tau_d << tau_task + coriolis;
  63.  
  64.       std::array<double, 7> tau_d_array{};
  65.       Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
  66.       return tau_d_array;
  67.     };

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

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

其他参考知识:

libfranka官方

FCI官方文档（franka）

Eign主页

非官方：Eigen库使用指南

非官方：Eigen与VS Code配置

非官方：Eigen快速入门

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/盐析白兔/article/detail/746936
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号