Franka Robot demo 关节阻抗控制(joint_impedance_control.cpp)
// Copyright (c) 2023 Franka Robotics GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE #include <array> #include <atomic> #include <cmath> #include <functional> #include <iostream> #include <iterator> #include <mutex> #include <thread> #include <franka/duration.h> #include <franka/exception.h> #include <franka/model.h> #include <franka/rate_limiting.h> #include <franka/robot.h> #include "examples_common.h" namespace { template <class T, size_t N> std::ostream& operator<<(std::ostream& ostream, const std::array<T, N>& array) { ostream << "["; std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ",")); std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream)); ostream << "]"; return ostream; } } // anonymous namespace /** * @example joint_impedance_control.cpp * An example showing a joint impedance type control that executes a Cartesian motion in the shape * of a circle. The example illustrates how to use the internal inverse kinematics to map a * Cartesian trajectory to joint space. The joint space target is tracked by an impedance control * that additionally compensates coriolis terms using the libfranka model library. This example * also serves to compare commanded vs. measured torques. The results are printed from a separate * thread to avoid blocking print functions in the real-time loop. */ /** * @example joint_impedance_control.cpp * 一个示例，展示了一种关节阻抗类型控制，执行一个圆形的笛卡尔运动。 * 该示例演示了如何使用内部逆运动学将笛卡尔轨迹映射到关节空间。 * 关节空间目标由阻抗控制跟踪，同时使用 libfranka 模型库补偿科氏力项。 * 该示例还用于比较命令力矩和测量力矩。结果会从单独的线程打印，以避免在实时循环中阻塞打印函数。 */ 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; } // Set and initialize trajectory parameters. 设置和初始化轨迹参数 const double radius = 0.05; // 半径 const double vel_max = 0.25; // 最大速度 const double acceleration_time = 2.0; //加速时间 s const double run_time = 20.0; // 运行时间 s // Set print rate for comparing commanded vs. measured torques. 设置打印命令力距和测量力距的速率 const double print_rate = 10.0; double vel_current = 0.0; // 当前速度 double angle = 0.0; // 角度 double time = 0.0; // 耗时 // Initialize data fields for the print thread. 初始化打印线程的数据字段 struct { std::mutex mutex; // 互斥锁 bool has_data; // 是否有数据 std::array<double, 7> tau_d_last; // 最新期望力距 franka::RobotState robot_state; // 机器人状态 std::array<double, 7> gravity; // 重力 } print_data{}; std::atomic_bool running{true}; // 原子布尔类型 运行标志 // Start print thread. std::thread print_thread([print_rate, &print_data, &running]() { // 打印线程 while (running) { // Sleep to achieve the desired print rate. std::this_thread::sleep_for( std::chrono::milliseconds(static_cast<int>((1.0 / print_rate * 1000.0)))); // 延迟指定时间 // Try to lock data to avoid read write collisions. if (print_data.mutex.try_lock()) {// 尝试获取锁 if (print_data.has_data) { std::array<double, 7> tau_error{};// 力距误差 double error_rms(0.0); std::array<double, 7> tau_d_actual{};// 实际期望关节力距 for (size_t i = 0; i < 7; ++i) { tau_d_actual[i] = print_data.tau_d_last[i] + print_data.gravity[i]; // 实际期望关节力距 = 上一个周期计算出的期望关节扭矩 + 重力补偿项 tau_error[i] = tau_d_actual[i] - print_data.robot_state.tau_J[i]; // 力距误差 = 实际期望关节力距 - 当前状态下测量到的关节力矩 error_rms += std::pow(tau_error[i], 2.0) / tau_error.size(); // 是均方根误差的平方根，表示了 tau_error 数组中所有关节力距误差的平均值 } error_rms = std::sqrt(error_rms); // 均方根误差 // Print data to console std::cout << "tau_error [Nm]: " << tau_error << std::endl << "tau_commanded [Nm]: " << tau_d_actual << std::endl << "tau_measured [Nm]: " << print_data.robot_state.tau_J << std::endl << "root mean square of tau_error [Nm]: " << error_rms << std::endl << "-----------------------" << std::endl; print_data.has_data = false; } print_data.mutex.unlock(); } } }); try { // Connect to robot. franka::Robot robot(argv[1]); // 连接机器人 setDefaultBehavior(robot); // 初始化机器人默认行为 // First move the robot to a suitable joint configuration 将机器人移动到一个合适的关节配置 std::array<double, 7> 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}}); // Load the kinematics and dynamics model. 加载运动学和动力学模型 franka::Model model = robot.loadModel(); std::array<double, 16> initial_pose; // 初始化位姿 // 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 { // Update time. 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;// 末端执行器（End-Effector）相对于基坐标系（Base）的当前位姿 } // 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);// 速度应 >= 0 vel_current = std::fmin(vel_current, vel_max); // 速度应 <= vel_max // Compute new angle for our circular trajectory. angle += period.toSec() * vel_current / std::fabs(radius); // 累加角度 角度 = t * (v / r) if (angle > 2 * M_PI) {// 将角度控制在 0-2PI之间 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; // 只修改y和z 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<double, 7> k_gains = {{600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}}; // k_gains 数组包含了每个关节的刚度值，单位为Nm/rad 刚度参数决定了关节对位移的反应程度 // Damping const std::array<double, 7> d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}}; // 数组包含了每个关节的阻尼值，单位为Nm/(rad/s)。 阻尼参数则决定了关节对速度的反应程度 // Define callback for the joint torque control loop. 为关节力距控制循环定义回调函数 std::function<franka::Torques(const franka::RobotState&, franka::Duration)> 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<double, 7> 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. // 从关节阻抗控制法则计算扭矩命令。 // 注意：我们的笛卡尔姿态逆运动学的答案始终在 state.q_d 中，有一个时间步骤的延迟。 std::array<double, 7> 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]; } // The following line is only necessary for printing the rate limited torque. As we activated // rate limiting for the control loop (activated by default), the torque would anyway be // adjusted! std::array<double, 7> tau_d_rate_limited = franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, state.tau_J_d);// 速率限制 // Update data to print. if (print_data.mutex.try_lock()) { print_data.has_data = true; print_data.robot_state = state; print_data.tau_d_last = tau_d_rate_limited; print_data.gravity = model.gravity(state); print_data.mutex.unlock(); } // Send torque command. return tau_d_rate_limited; }; // Start real-time control loop. 开始实时控制循环 robot.control(impedance_control_callback, cartesian_pose_callback); } catch (const franka::Exception& ex) { running = false; std::cerr << ex.what() << std::endl; } if (print_thread.joinable()) { print_thread.join(); } return 0; }
关键点
-
初始化和参数检查:
- 检查是否提供了所需的参数（机器人主机名）。
- 初始化轨迹参数。
-
打印线程:
- 一个单独的线程以指定的速率打印力矩误差和其他数据。
-
机器人连接和配置:
- 使用提供的主机名连接到机器人。
- 设置机器人的默认行为并将其移动到初始关节配置。
-
碰撞行为和模型加载:
- 配置机器人的碰撞行为。
- 加载机器人的运动学和动力学模型。
-
笛卡尔位置回调:
- 计算圆形轨迹的期望笛卡尔位置。
-
阻抗控制回调:
- 使用关节阻抗控制律计算力矩命令。
-
执行:
- 使用定义的回调函数启动实时控制循环。
- 处理异常并确保打印线程正确结束。
结论
这段代码展示了如何在Franka Emika Panda机器人上进行高级控制，包括轨迹生成、逆运动学、阻抗控制和实时反馈。它是使用libfranka库实现复杂机器人行为的一个很好的示例。
