// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <chrono>
#include <iostream>
#include <thread>
#include <franka/active_control.h>
#include <franka/active_torque_control.h>
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/robot.h>
#include "examples_common.h"
/**
* @example communication_test.cpp
* An example indicating the network performance. 测试网络性能的示例
*
* @warning Before executing this example, make sure there is enough space in front of the robot.
*/
int main(int argc, char** argv) {
if (argc != 2) {
std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
return -1;
}
uint64_t counter = 0; // 通信包计数，每到100的倍数统计一次
double avg_success_rate = 0.0; //平均通信成功率
double min_success_rate = 1.0; // 最小通信成功率
double max_success_rate = 0.0; // 最大通信成功率
uint64_t time = 0; //运行时间
std::cout.precision(2); // 输出小数位数为2为
std::cout << std::fixed;// 输出为固定小数点，不采用科学计数法
try {
franka::Robot robot(argv[1]);//使用ip初始化机器人模型
// void setDefaultBehavior(franka::Robot& robot) {// 非Franka官方函数，定义默认参数
// robot.setCollisionBehavior(
// {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
// {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
// {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
// {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
// robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
// robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
// }
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 << std::endl;
std::cout << "Starting communication test." << std::endl;
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}});
franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // 设置机器人零关节力距
//让机器人进入扭矩控制模式。在这种模式下,机器人可以直接对各关节施加扭矩,而不需要考虑关节角度或速度等状态量。
//这种控制模式非常适合需要精细控制力矩的应用场景,如力反馈控制、碰撞检测等。
auto rw_interface = robot.startTorqueControl();
// 机器人状态
franka::RobotState robot_state;
// franka::Duration是Franka Emika机器人库中用于表示时间间隔的数据类型。
// 它是一个包装类,内部使用std::chrono::duration来存储时间值。
franka::Duration period;
while (!zero_torques.motion_finished) {
std::tie(robot_state, period) = rw_interface->readOnce();//模拟python 将状态值和运行时间取出
time += period.toMSec(); // 累加运行时间 ms
if (time == 0.0) {
rw_interface->writeOnce(zero_torques); // 使用零力据初始化
continue;
}
counter++;
if (counter % 100 == 0) {
std::cout << "#" << counter
<< " Current success rate: " << robot_state.control_command_success_rate
<< std::endl;
}
std::this_thread::sleep_for(std::chrono::microseconds(100));// 延时100ms
avg_success_rate += robot_state.control_command_success_rate;//累加成功率
if (robot_state.control_command_success_rate > max_success_rate) {//记录最大成功率
max_success_rate = robot_state.control_command_success_rate;
}
if (robot_state.control_command_success_rate < min_success_rate) {//记录最小成功率
min_success_rate = robot_state.control_command_success_rate;
}
if (time >= 10000) {// 最大运行10s
std::cout << std::endl << "Finished test, shutting down example" << std::endl;
zero_torques.motion_finished = true; //标志运动停止
}
// Sending zero torques - if EE is configured correctly, robot should not move
rw_interface->writeOnce(zero_torques); // 写零力距
}
} catch (const franka::Exception& e) {
std::cout << e.what() << std::endl;
return -1;
}
avg_success_rate = avg_success_rate / counter; //计算平均成功率
std::cout << std::endl
<< std::endl
<< "#######################################################" << std::endl;
uint64_t lost_robot_states = time - counter; // 计算丢失通信次数
if (lost_robot_states > 0) {
std::cout << "The control loop did not get executed " << lost_robot_states << " times in the"
<< std::endl
<< "last " << time << " milliseconds! (lost " << lost_robot_states << " robot states)"
<< std::endl
<< std::endl;
}
std::cout << "Control command success rate of " << counter << " samples: " << std::endl;
std::cout << "Max: " << max_success_rate << std::endl;
std::cout << "Avg: " << avg_success_rate << std::endl;
std::cout << "Min: " << min_success_rate << std::endl;
if (avg_success_rate < 0.90) { // 通信成功率小于0.9, 警告：此设置可能不足以支持 FCI
std::cout << std::endl
<< "WARNING: THIS SETUP IS PROBABLY NOT SUFFICIENT FOR FCI!" << std::endl;
std::cout << "PLEASE TRY OUT A DIFFERENT PC / NIC" << std::endl;
} else if (avg_success_rate < 0.95) {// 通信成功率小于0.95 警告 。 请检查您的设置并遵循建议
std::cout << std::endl << "WARNING: MANY PACKETS GOT LOST!" << std::endl;
std::cout << "PLEASE INSPECT YOUR SETUP AND FOLLOW ADVICE ON" << std::endl
<< "https://frankaemika.github.io/docs/troubleshooting.html" << std::endl;
}
std::cout << "#######################################################" << std::endl << std::endl;
return 0;
}
