1035 lines
39 KiB
C++
1035 lines
39 KiB
C++
|
||
#include <array>
|
||
#include <cmath>
|
||
#include <fstream>
|
||
#include <iostream>
|
||
#include <sstream>
|
||
#include <string>
|
||
#include <cstdio>
|
||
#include <ctime>
|
||
#include <chrono>
|
||
#include <vector>
|
||
|
||
#include "coordinated_actions/coordinated_actions.h"
|
||
#include "inspire_hand/hand_actions.h"
|
||
#include "robot_action.h"
|
||
#include "tools/tools.h"
|
||
|
||
namespace coordinated_actions {
|
||
|
||
constexpr double PI = 3.14159265358979323846;
|
||
|
||
using tools::get_cwd;
|
||
using tools::clear_screen;
|
||
using tools::sleep_ms;
|
||
using tools::get_actions_record_path;
|
||
|
||
namespace {
|
||
|
||
// 记录当前“机械臂关节 + 灵巧手角度”到同一个记录文件：
|
||
// 格式: timestamp, j0..j5 (deg), h0..h5 (0~1000), "comment"
|
||
static bool record_coordinated_pose(RobotController &rc, const std::string &comment)
|
||
{
|
||
std::array<double, 6> joints_deg{};
|
||
if (!rc.getJointPositionsDeg(joints_deg)) {
|
||
std::cerr << "record_coordinated_pose: 获取关节位姿失败\n";
|
||
return false;
|
||
}
|
||
|
||
auto hand_angles = hand_actions::get_angle_act_service();
|
||
if (hand_angles.size() != 6) {
|
||
hand_angles.assign(6, 1000); // 如果读取失败，用全伸直作为占位
|
||
}
|
||
|
||
const std::string path = get_actions_record_path();
|
||
std::ofstream ofs(path.c_str(), std::ios::app);
|
||
if (!ofs) {
|
||
std::cerr << "record_coordinated_pose: 无法打开文件: " << path << std::endl;
|
||
return false;
|
||
}
|
||
|
||
auto t = std::time(nullptr);
|
||
ofs << t;
|
||
for (int i = 0; i < 6; ++i) ofs << "," << joints_deg[i];
|
||
for (int i = 0; i < 6; ++i) ofs << "," << static_cast<int>(hand_angles[i]);
|
||
ofs << "," << '"' << comment << '"' << std::endl;
|
||
ofs.close();
|
||
return true;
|
||
}
|
||
|
||
static bool delete_coordinated_file()
|
||
{
|
||
const std::string path = get_actions_record_path();
|
||
return std::remove(path.c_str()) == 0;
|
||
}
|
||
|
||
// 将联合记录文件清空为 0 字节（如果存在则截断，不存在则创建空文件）
|
||
static bool clear_coordinated_file()
|
||
{
|
||
const std::string path = get_actions_record_path();
|
||
std::ofstream ofs(path.c_str(), std::ios::trunc);
|
||
return static_cast<bool>(ofs);
|
||
}
|
||
|
||
// 手势菜单使用的力传感基准，避免在每次轮询时重复采集
|
||
std::vector<double> g_force_baseline(6, 0.0);
|
||
void set_force_baseline_ready()
|
||
{
|
||
// 在轮询外采集基准数据
|
||
std::cout << "采集力传感器基准数据中，请稍后..." << std::endl;
|
||
if (!hand_actions::capture_force_baseline(g_force_baseline, 8, 50)) {
|
||
std::cout << "采集基准失败，稍后重试..." << std::endl;
|
||
sleep_ms(200);
|
||
}
|
||
std::cout << "采集基准数据完毕" << std::endl;
|
||
}
|
||
|
||
bool run_console_menu(RobotController &rc, robot_action::RobotParams ¶ms)
|
||
{
|
||
clear_screen();
|
||
std::cout << "控制台菜单 (输入 q 返回手势控制):\n";
|
||
std::cout << " 1) 重新设置\n";
|
||
std::cout << " 2) 示教模式与记录\n";
|
||
|
||
while (true) {
|
||
std::cout << "请输入选项: ";
|
||
std::string input;
|
||
if (!std::getline(std::cin, input)) {
|
||
return false;
|
||
}
|
||
if (input == "1") {
|
||
std::cout << "请选择要设置的对象:0 = 机械臂速度, 1 = 灵巧手速度, 2 = 传感器校准: ";
|
||
std::string opt;
|
||
if (!std::getline(std::cin, opt)) {
|
||
return false;
|
||
}
|
||
if (opt == "0") {
|
||
robot_action::adjust_speeds(rc, params);
|
||
} else if (opt == "1") {
|
||
hand_actions::adjust_hand_speeds();
|
||
} else if (opt == "2") {
|
||
hand_actions::calibrate_hand_sensors();
|
||
sleep_ms(15000);
|
||
} else {
|
||
std::cout << "输入无效，保持当前设置。\n";
|
||
}
|
||
clear_screen();
|
||
std::cout << "控制台菜单 (输入 q 返回手势控制):\n";
|
||
std::cout << " 1) 重新设置速度\n";
|
||
std::cout << " 2) 示教模式与记录\n";
|
||
} else if (input == "2") {
|
||
run_teach_mode(rc);
|
||
clear_screen();
|
||
std::cout << "控制台菜单 (输入 q 返回手势控制):\n";
|
||
std::cout << " 1) 重新设置速度\n";
|
||
std::cout << " 2) 示教模式与记录\n";
|
||
} else if (input == "q" || input == "Q") {
|
||
clear_screen();
|
||
return true;
|
||
} else if (input.empty()) {
|
||
continue;
|
||
} else {
|
||
std::cout << "输入无效，请重新输入。\n";
|
||
}
|
||
}
|
||
}
|
||
|
||
} // namespace
|
||
|
||
std::string force_trigger_selection()
|
||
{
|
||
constexpr double trigger_threshold = 40.0;
|
||
constexpr int wave_index = 1;
|
||
constexpr int rps_index = 2;
|
||
constexpr int grasp_index = 3;
|
||
constexpr int console_index = 5;
|
||
constexpr int exit_index = 0;
|
||
|
||
auto forces = hand_actions::get_force_service();
|
||
if (forces.size() != 6) {
|
||
return std::string();
|
||
}
|
||
|
||
auto delta_at = [&](int index) -> double {
|
||
if (index < 0 || index >= 6) return 0.0;
|
||
return hand_actions::force_delta_abs(forces, g_force_baseline, index);
|
||
};
|
||
|
||
double exit_delta = delta_at(exit_index);
|
||
if (exit_delta >= trigger_threshold) {
|
||
return std::string("exit");
|
||
}
|
||
|
||
double wave_delta = delta_at(wave_index);
|
||
if (wave_delta >= trigger_threshold) {
|
||
return std::string("wave");
|
||
}
|
||
|
||
double rps_delta = delta_at(rps_index);
|
||
if (rps_delta >= trigger_threshold) {
|
||
return std::string("rps");
|
||
}
|
||
|
||
double grasp_delta = delta_at(grasp_index);
|
||
if (grasp_delta >= trigger_threshold) {
|
||
return std::string("grasp");
|
||
}
|
||
|
||
double console_delta = delta_at(console_index);
|
||
if (console_delta >= trigger_threshold) {
|
||
return std::string("console");
|
||
}
|
||
|
||
return std::string();
|
||
}
|
||
|
||
void wave_and_handshake(RobotController &rc)
|
||
{
|
||
clear_screen();
|
||
std::cout << "挥手与握手演示" << std::endl;
|
||
std::cout << "机械臂将摆动第4关节 ±25°，任意非小拇指受力触发伸手，10 秒内所有手指受力则执行灵巧手挥手，小拇指受力随时退出。" << std::endl;
|
||
|
||
std::array<double,6> preset_deg = {0.0, 90.0, -90.0, -15.0, 0.0, -90.0};
|
||
auto preset_rad = degrees_to_radians(preset_deg);
|
||
if (!rc.moveJoints(preset_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad())) {
|
||
std::cout << "机械臂移动失败，无法开始。1 秒后返回手势等待。" << std::endl;
|
||
sleep_ms(1000);
|
||
return;
|
||
}
|
||
|
||
hand_actions::set_speed_service(400,400,400,400,400,400);
|
||
hand_actions::set_force_service(300,300,300,300,300,300);
|
||
hand_actions::set_angle_service(1000,1000,1000,1000,1000,1000);
|
||
sleep_ms(1000);
|
||
|
||
std::vector<double> baseline;
|
||
if (!hand_actions::capture_force_baseline(baseline, 12, 50)) {
|
||
std::cout << "无法获取力传感基准，2 秒后返回手势等待。" << std::endl;
|
||
sleep_ms(2000);
|
||
return;
|
||
}
|
||
|
||
const int pinky_index = 0;
|
||
const std::array<int,5> handshake_indices = {1, 2, 3, 4, 5};
|
||
const double trigger_threshold = 40.0;
|
||
const double exit_threshold = 40.0;
|
||
const int switch_interval_ms = 600;
|
||
const int wait_all_ms = 8000; // 等待所有手指受力的最长时间
|
||
|
||
int open_angles[6] = {1000,1000,1000,1000,1000,1000};
|
||
int shake_angles[6] = {50,50,50,50,400,600};
|
||
|
||
bool swing_left = true;
|
||
auto last_switch = std::chrono::steady_clock::now();
|
||
std::array<double,6> base_rad = preset_rad;
|
||
double base_joint = base_rad[4];
|
||
double amplitude = 25.0 * PI / 180.0;
|
||
|
||
// 伸手关节位姿（用户提供的关节角度，单位度）
|
||
std::array<double,6> extend_deg = {-120.0, -15.0, -82.0, 69.0, -27.0, 53.0};
|
||
auto extend_rad = degrees_to_radians(extend_deg);
|
||
|
||
|
||
while (true){
|
||
auto forces = hand_actions::get_force_service();
|
||
if (forces.size() != 6) {
|
||
sleep_ms(80);
|
||
continue;
|
||
}
|
||
|
||
// 小拇指触发整个演示退出
|
||
double pinky_delta = hand_actions::force_delta_abs(forces, baseline, pinky_index);
|
||
if (pinky_delta >= exit_threshold){
|
||
std::cout << "检测到小拇指受力，结束挥手演示。" << std::endl;
|
||
break;
|
||
}
|
||
|
||
// 检测除小拇指以外任意手指的受力，触发伸手
|
||
double max_hand_delta = 0.0;
|
||
for (int idx : handshake_indices){
|
||
double delta = hand_actions::force_delta_abs(forces, baseline, idx);
|
||
if (delta > max_hand_delta) {
|
||
max_hand_delta = delta;
|
||
}
|
||
}
|
||
|
||
if (max_hand_delta >= trigger_threshold){
|
||
std::cout << "检测到非小拇指受力，机械臂伸出准备握手..." << std::endl;
|
||
rc.motionAbort();
|
||
if (!rc.moveJoints(extend_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad())) {
|
||
std::cout << "伸手位姿移动失败，结束挥手演示。" << std::endl;
|
||
break;
|
||
}
|
||
|
||
sleep_ms(1000);
|
||
//重新采集
|
||
std::vector<double> extend_baseline;
|
||
if (!hand_actions::capture_force_baseline(extend_baseline, 10, 50)) {
|
||
std::cout << "无法获取力传感基准，结束挥手演示。" << std::endl;
|
||
break;
|
||
}
|
||
|
||
auto start_wait = std::chrono::steady_clock::now();
|
||
bool all_fingers = false;
|
||
|
||
while (true) {
|
||
auto now = std::chrono::steady_clock::now();
|
||
auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - start_wait).count();
|
||
if (elapsed_ms >= wait_all_ms) {
|
||
break;
|
||
}
|
||
|
||
auto forces2 = hand_actions::get_force_service();
|
||
if (forces2.size() != 6) {
|
||
sleep_ms(80);
|
||
continue;
|
||
}
|
||
|
||
bool any_ok = false;
|
||
for (int idx : handshake_indices) {
|
||
double d = hand_actions::force_delta_abs(forces2, extend_baseline, idx);
|
||
if (d >= trigger_threshold) {
|
||
any_ok = true;
|
||
}
|
||
}
|
||
|
||
// 握手过程中，只要检测到至少一根手指受力，就去执行挥手
|
||
if (any_ok) {
|
||
all_fingers = true;
|
||
break;
|
||
}
|
||
sleep_ms(80);
|
||
}
|
||
|
||
if (all_fingers) {
|
||
std::cout << "检测到所有手指受力，执行灵巧手握手动作。" << std::endl;
|
||
hand_actions::set_angle_service(shake_angles[0],shake_angles[1],shake_angles[2],shake_angles[3],shake_angles[4],shake_angles[5]);
|
||
sleep_ms(4000);
|
||
hand_actions::set_angle_service(open_angles[0],open_angles[1],open_angles[2],open_angles[3],open_angles[4],open_angles[5]);
|
||
sleep_ms(2000);
|
||
break; // 完成一次示范后结束演示
|
||
} else {
|
||
std::cout << "未在 10 秒内检测到所有手指受力，返回挥手姿态继续挥手。" << std::endl;
|
||
rc.moveJoints(base_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
swing_left = true;
|
||
last_switch = std::chrono::steady_clock::now();
|
||
}
|
||
}
|
||
|
||
// 正常挥手
|
||
auto now = std::chrono::steady_clock::now();
|
||
if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_switch).count() >= switch_interval_ms){
|
||
std::array<double,6> target = base_rad;
|
||
target[4] = base_joint + (swing_left ? amplitude : -amplitude);
|
||
//非阻塞
|
||
rc.moveJoints(target, ABS, false, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
swing_left = !swing_left;
|
||
last_switch = now;
|
||
}
|
||
|
||
sleep_ms(80);
|
||
}
|
||
|
||
rc.motionAbort();
|
||
rc.moveJoints(base_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
hand_actions::set_angle_service(open_angles[0],open_angles[1],open_angles[2],open_angles[3],open_angles[4],open_angles[5]);
|
||
std::cout << "演示结束。" << std::endl;
|
||
sleep_ms(1000);
|
||
}
|
||
|
||
namespace {
|
||
|
||
static bool replay_coordinated_file(RobotController &rc, const std::string &path)
|
||
{
|
||
std::ifstream ifs(path.c_str());
|
||
if (!ifs) {
|
||
std::cout << "无法打开记录文件: " << path << std::endl;
|
||
return false;
|
||
}
|
||
std::string line;
|
||
int idx_line = 0;
|
||
while (std::getline(ifs, line)) {
|
||
++idx_line;
|
||
if (line.empty()) continue;
|
||
std::stringstream ss(line);
|
||
std::string token;
|
||
if (!std::getline(ss, token, ',')) continue; // timestamp 忽略
|
||
|
||
std::array<double, 6> joints_deg{};
|
||
bool ok_joint = true;
|
||
for (int i = 0; i < 6; ++i) {
|
||
if (!std::getline(ss, token, ',')) { ok_joint = false; break; }
|
||
try { joints_deg[i] = std::stod(token); }
|
||
catch (...) { ok_joint = false; break; }
|
||
}
|
||
if (!ok_joint) {
|
||
std::cerr << "解析第" << idx_line << "行关节数据失败\n";
|
||
continue;
|
||
}
|
||
|
||
int hand_vals[6] = {1000,1000,1000,1000,1000,1000};
|
||
bool ok_hand = true;
|
||
for (int i = 0; i < 6; ++i) {
|
||
if (!std::getline(ss, token, ',')) { ok_hand = false; break; }
|
||
try { hand_vals[i] = std::stoi(token); }
|
||
catch (...) { ok_hand = false; break; }
|
||
}
|
||
if (!ok_hand) {
|
||
std::cerr << "解析第" << idx_line << "行灵巧手角度数据失败\n";
|
||
continue;
|
||
}
|
||
|
||
std::string comment;
|
||
std::getline(ss, comment); // 余下部分视为注释（可能含引号）
|
||
|
||
std::array<double, 6> joints_rad{};
|
||
for (int i = 0; i < 6; ++i) joints_rad[i] = joints_deg[i] * PI / 180.0;
|
||
|
||
std::cout << "复现第" << idx_line << "，注释: " << comment << "\n";
|
||
bool moved = rc.moveJoints(joints_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
if (!moved) {
|
||
std::cerr << "移动到记录点 " << idx_line << " 失败\n";
|
||
std::cout << "是否继续下一个点？(y/n): ";
|
||
std::string yes;
|
||
if (!std::getline(std::cin, yes)) yes = "n";
|
||
if (yes != "y" && yes != "Y") { ifs.close(); return false; }
|
||
} else {
|
||
hand_actions::set_angle_service(hand_vals[0], hand_vals[1], hand_vals[2],
|
||
hand_vals[3], hand_vals[4], hand_vals[5]);
|
||
std::cout << "完成第" << idx_line << "个点\n";
|
||
}
|
||
}
|
||
ifs.close();
|
||
std::cout << "顺序复现结束。\n";
|
||
return true;
|
||
}
|
||
} // namespace
|
||
|
||
void play_rps_game(RobotController &rc)
|
||
{
|
||
bool moved = robot_action::init_place(rc);
|
||
if (!moved) {
|
||
std::cout << "移动到起始姿态失败，无法开始游戏。2 秒后返回手势等待。" << std::endl;
|
||
sleep_ms(2000);
|
||
return;
|
||
}
|
||
//进行重构，这里需要机械臂的动作而不只是灵巧手
|
||
|
||
clear_screen();
|
||
std::cout << "石头剪刀布 - 力觉交互模式" << std::endl;
|
||
std::cout << "食指: 玩家胜利, 无名指: 玩家失败, 中指: 平局, 小指: 退出" << std::endl;
|
||
|
||
int rock[6] = {0,0,0,0,200,700};
|
||
int scissors[6] = {0,0,1000,1000,200,800};
|
||
int paper[6] = {1000,1000,1000,1000,1000,1000};
|
||
|
||
int result = 1;
|
||
std::vector<double> baseline;
|
||
if (!hand_actions::capture_force_baseline(baseline, 12, 50)) {
|
||
std::cout << "无法获取力传感基准，返回菜单。" << std::endl;
|
||
sleep_ms(1200);
|
||
}
|
||
std::cout << "初始基准值:";
|
||
for (std::size_t i = 0; i < baseline.size(); ++i) {
|
||
std::cout << " f" << i << "=" << baseline[i];
|
||
}
|
||
std::cout << std::endl;
|
||
|
||
const double trigger_threshold = 40.0;
|
||
const int pinky_index = 0;
|
||
const int ring_index = 1;
|
||
const int middle_index = 2;
|
||
const int index_index = 3;
|
||
|
||
int total_rounds = 0;
|
||
int player_wins = 0;
|
||
int ties = 0;
|
||
|
||
auto print_score = [&]() {
|
||
int player_losses = total_rounds - player_wins - ties;
|
||
std::cout << "总局数: " << total_rounds
|
||
<< " | 玩家胜: " << player_wins
|
||
<< " | 平局: " << ties
|
||
<< " | 玩家负: " << player_losses << std::endl;
|
||
};
|
||
|
||
bool exit_requested = false;
|
||
|
||
int ready_gesture[6] = {1000,1000,1000,1000,800,0};
|
||
|
||
rc.setJointSpeedDeg(240.0);
|
||
rc.setJointAccDeg(400.0); //加速移动
|
||
|
||
while (true) {
|
||
robot_action::init_place(rc); //恢复初始位置
|
||
hand_actions::set_angle_service(ready_gesture[0],ready_gesture[1],ready_gesture[2],ready_gesture[3],ready_gesture[4],ready_gesture[5]);
|
||
if (total_rounds!=0) {
|
||
std::cout << "请通过手指反馈上一轮结果（食指胜、无名指负、中指平、小指退出）。" << std::endl;
|
||
|
||
/*sleep_ms(1000);
|
||
if (!hand_actions::capture_force_baseline(baseline, 8, 40)) {
|
||
std::cout << "基准刷新失败，沿用上一轮数据。" << std::endl;
|
||
}*/
|
||
|
||
sleep_ms(1000);
|
||
hand_actions::capture_force_baseline(baseline, 10, 30);
|
||
|
||
bool decided = false;
|
||
while (!decided) {
|
||
|
||
auto forces_now = hand_actions::get_force_service();
|
||
|
||
double delta_pinky = hand_actions::force_delta_abs(forces_now, baseline, pinky_index);
|
||
double delta_ring = hand_actions::force_delta_abs(forces_now, baseline, ring_index);
|
||
double delta_middle = hand_actions::force_delta_abs(forces_now, baseline, middle_index);
|
||
double delta_index = hand_actions::force_delta_abs(forces_now, baseline, index_index);
|
||
|
||
if (delta_pinky >= trigger_threshold) {
|
||
std::cout << "检测到退出手势，准备结束游戏。" << std::endl;
|
||
exit_requested = true;
|
||
decided = true;
|
||
} else if (delta_index >= trigger_threshold) {
|
||
++player_wins;
|
||
std::cout << "记录上一轮：玩家胜利。" << std::endl;
|
||
print_score();
|
||
decided = true;
|
||
} else if (delta_ring >= trigger_threshold) {
|
||
std::cout << "记录上一轮：玩家落败。" << std::endl;
|
||
print_score();
|
||
decided = true;
|
||
} else if (delta_middle >= trigger_threshold) {
|
||
++ties;
|
||
std::cout << "本轮平局。" << std::endl;
|
||
print_score();
|
||
decided = true;
|
||
} else {
|
||
sleep_ms(80);
|
||
}
|
||
}
|
||
|
||
if (exit_requested) {
|
||
break;
|
||
}
|
||
}
|
||
|
||
std::cout << "倒计时开始 (回合" << (total_rounds + 1) << ")" << std::endl;
|
||
|
||
hand_actions::action_full_fist();
|
||
// 先将机械臂伸到出拳姿态
|
||
std::array<double,6> extend_deg = {-120.0, -38.0, -0.5, 28, -57.0, 56.3};
|
||
auto extend_rad = degrees_to_radians(extend_deg);
|
||
rc.moveJoints(extend_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
|
||
// 在出拳前，通过第4号关节（下标 4）上下摇摆两次，模拟人晃动手腕
|
||
const int wrist_index = 4;
|
||
double base_wrist = extend_rad[wrist_index];
|
||
double shake_amp = 10.0 * PI / 180.0; // 摆动幅度 10 度
|
||
for (int cycle = 0; cycle < 2; ++cycle) {
|
||
std::array<double,6> target_up = extend_rad;
|
||
target_up[wrist_index] = base_wrist + shake_amp;
|
||
rc.moveJoints(target_up, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
sleep_ms(300);
|
||
|
||
std::array<double,6> target_down = extend_rad;
|
||
target_down[wrist_index] = base_wrist - shake_amp;
|
||
rc.moveJoints(target_down, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
sleep_ms(300);
|
||
}
|
||
|
||
// 摆动结束后回到基准伸出姿态再出拳
|
||
rc.moveJoints(extend_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
|
||
auto seed = static_cast<unsigned>(std::chrono::steady_clock::now().time_since_epoch().count());
|
||
std::srand(seed);
|
||
int choice = std::rand() % 3;
|
||
if (choice == 0) {
|
||
hand_actions::set_angle_service(scissors[0],scissors[1],scissors[2],scissors[3],scissors[4],scissors[5]);
|
||
std::cout << "机器人出：剪刀" << std::endl;
|
||
} else if (choice == 1) {
|
||
hand_actions::set_angle_service(rock[0],rock[1],rock[2],rock[3],rock[4],rock[5]);
|
||
std::cout << "机器人出：石头" << std::endl;
|
||
} else {
|
||
hand_actions::set_angle_service(paper[0],paper[1],paper[2],paper[3],paper[4],paper[5]);
|
||
std::cout << "机器人出：布" << std::endl;
|
||
}
|
||
sleep_ms(3000);
|
||
total_rounds++;
|
||
hand_actions::action_full_extend();
|
||
|
||
}
|
||
|
||
total_rounds--;
|
||
std::cout << "游戏结束。" << std::endl;
|
||
std::cout << "总局数: " << total_rounds
|
||
<< " | 玩家胜: " << player_wins
|
||
<< " | 平局: " << ties
|
||
<< " | 玩家负: " << (total_rounds - player_wins - ties) << std::endl;
|
||
if (exit_requested && total_rounds > 0) {
|
||
int player_losses = total_rounds - player_wins - ties;
|
||
if (player_wins > player_losses)
|
||
result = 1; // player won
|
||
else if (player_wins < player_losses)
|
||
result = -1; // player lost
|
||
else
|
||
result = 0; // tie
|
||
}
|
||
|
||
|
||
if(result==1){ //胜利点赞
|
||
std::array<double,6> preset_deg = {0,0,0,0,90,-90};
|
||
auto preset_rad = degrees_to_radians(preset_deg);
|
||
rc.moveJoints(preset_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
hand_actions::set_angle_service(0,0,0,0,1000,1000);
|
||
}
|
||
else if(result==0){
|
||
std::array<double,6> preset_deg = {0,0,0,0,0,-90};
|
||
auto preset_rad = degrees_to_radians(preset_deg);
|
||
rc.moveJoints(preset_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
hand_actions::set_angle_service(0,0,0,0,1000,1000);
|
||
}
|
||
else if(result==-1){
|
||
std::array<double,6> preset_deg = {0,0,0,0,-90,-90};
|
||
auto preset_rad = degrees_to_radians(preset_deg);
|
||
rc.moveJoints(preset_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
hand_actions::set_angle_service(0,0,0,0,1000,1000);
|
||
}
|
||
sleep_ms(2000);
|
||
const config::AppConfig &cfg = config::get_default_app_config();
|
||
rc.setJointAccDeg(cfg.robot.joint_acc_deg_s2); //恢复默认加速度
|
||
rc.setJointSpeedDeg(cfg.robot.joint_speed_deg_s); //恢复默认速度
|
||
}
|
||
|
||
|
||
|
||
void run_teach_mode(RobotController &rc)
|
||
{
|
||
// 进入示教模式时，检查记录文件是否非空，决定本次会话是清空后记录还是直接追加
|
||
{
|
||
const std::string path = get_actions_record_path();
|
||
std::ifstream ifs(path.c_str(), std::ios::binary);
|
||
if (ifs) {
|
||
ifs.seekg(0, std::ios::end);
|
||
std::streampos sz = ifs.tellg();
|
||
ifs.close();
|
||
if (sz > 0) {
|
||
while (true) {
|
||
std::cout << "检测到联合示教记录文件非空: " << path << "\n";
|
||
std::cout << "请选择 (c: 清空后重新记录, a: 在末尾追加, 回车默认 a): ";
|
||
std::string choice;
|
||
if (!std::getline(std::cin, choice)) choice.clear();
|
||
|
||
if (choice.empty() || choice == "a" || choice == "A") {
|
||
std::cout << "已选择在原有记录后追加新点。\n";
|
||
break;
|
||
} else if (choice == "c" || choice == "C") {
|
||
if (clear_coordinated_file()) {
|
||
std::cout << "记录文件已清空，本次会话将从头开始记录。\n";
|
||
} else {
|
||
std::cout << "清空记录文件失败，本次会话将继续在原文件后追加。\n";
|
||
}
|
||
break;
|
||
} else {
|
||
std::cout << "输入无效，请重新输入。\n";
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
sleep_ms(1000);
|
||
|
||
while (true) {
|
||
std::cout << "示教模式(机械臂 + 灵巧手):\n";
|
||
std::cout << " l : 显示当前状态 (机械臂 + 灵巧手)\n";
|
||
std::cout << " rj : 机械臂关节相对移动(逐关节输入步进, 回车保持原值)\n";
|
||
std::cout << " dj : 机械臂关节绝对移动(逐关节输入目标角度, 回车保持原值)\n";
|
||
std::cout << " rc : 机械臂笛卡尔相对移动(逐轴输入步进, 回车保持原值)\n";
|
||
std::cout << " dc : 机械臂笛卡尔绝对移动(逐轴输入目标值, 回车保持原值)\n";
|
||
std::cout << " ha : 灵巧手角度移动(逐关节输入角度, 回车保持原值)\n";
|
||
std::cout << " rec : 记录当前机械臂 + 灵巧手为一个示教点\n";
|
||
std::cout << " del : 删除联合记录文件\n";
|
||
std::cout << " replay : 顺序复现联合记录(机械臂+灵巧手)\n";
|
||
std::cout << " q : 返回上级菜单\n";
|
||
std::cout << "请输入命令: ";
|
||
|
||
std::string mode;
|
||
if (!std::getline(std::cin, mode)) break;
|
||
if (mode.empty()) continue;
|
||
if (mode == "q") break;
|
||
clear_screen();
|
||
|
||
if (mode == "rec") {
|
||
std::cout << "请输入当前示教点注释（可空）：";
|
||
std::string comment;
|
||
std::getline(std::cin, comment);
|
||
if (record_coordinated_pose(rc, comment)) {
|
||
std::cout << "已记录当前机械臂 + 灵巧手状态到: "
|
||
<< get_actions_record_path() << "\n";
|
||
} else {
|
||
std::cerr << "记录失败\n";
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (mode == "ha") {
|
||
auto hand_angles = hand_actions::get_angle_act_service();
|
||
int cur[6] = {1000,1000,1000,1000,1000,1000};
|
||
if (hand_angles.size() == 6) {
|
||
for (int i = 0; i < 6; ++i) cur[i] = static_cast<int>(hand_angles[i]);
|
||
}
|
||
|
||
int new_vals[6];
|
||
std::cout << "灵巧手关节角度设置(0~1000)，每行一个关节，直接回车保持原值。\n";
|
||
for (int i = 0; i < 6; ++i) {
|
||
std::cout << "关节 " << i << " 当前角度=" << cur[i] << "，请输入新角度: ";
|
||
std::string line;
|
||
if (!std::getline(std::cin, line)) { new_vals[i] = cur[i]; continue; }
|
||
if (line.empty()) {
|
||
new_vals[i] = cur[i];
|
||
continue;
|
||
}
|
||
try {
|
||
int v = std::stoi(line);
|
||
if (v < 0 || v > 1000) {
|
||
std::cout << "输入超出范围，保持原值 " << cur[i] << "\n";
|
||
new_vals[i] = cur[i];
|
||
} else {
|
||
new_vals[i] = v;
|
||
}
|
||
} catch (...) {
|
||
std::cout << "输入无效，保持原值 " << cur[i] << "\n";
|
||
new_vals[i] = cur[i];
|
||
}
|
||
}
|
||
|
||
hand_actions::set_angle_service(new_vals[0], new_vals[1], new_vals[2],
|
||
new_vals[3], new_vals[4], new_vals[5]);
|
||
std::cout << "已下发角度命令: (" << new_vals[0] << "," << new_vals[1] << "," << new_vals[2]
|
||
<< "," << new_vals[3] << "," << new_vals[4] << "," << new_vals[5] << ")" << std::endl;
|
||
sleep_ms(500);
|
||
continue;
|
||
}
|
||
|
||
if (mode == "l") {
|
||
std::array<double, 6> cur_deg;
|
||
if (rc.getJointPositionsDeg(cur_deg)) {
|
||
std::cout << "当前关节位置: ";
|
||
for (int i = 0; i < 6; ++i) {
|
||
double v = cur_deg[i];
|
||
if (std::fabs(v) < 1e-6) v = 0.0; // 消除极小浮点误差
|
||
std::cout << v << (i == 5 ? "\n" : ", ");
|
||
}
|
||
} else {
|
||
std::cout << "获取关节位姿失败\n";
|
||
}
|
||
|
||
// 追加灵巧手当前角度（通过 TIO 信号量读取）
|
||
auto hand_angles = hand_actions::get_angle_act_service();
|
||
if (!hand_angles.empty()) {
|
||
std::cout << "灵巧手当前角度: ";
|
||
for (std::size_t i = 0; i < hand_angles.size(); ++i) {
|
||
std::cout << hand_angles[i] << (i + 1 == hand_angles.size() ? "\n" : ", ");
|
||
}
|
||
}
|
||
|
||
CartesianPose cp;
|
||
if (rc.getTcpPose(cp)) {
|
||
std::cout << "TCP(mm): x=" << cp.tran.x << " y=" << cp.tran.y << " z=" << cp.tran.z;
|
||
std::cout << " RPY(deg): rx=" << (cp.rpy.rx * 180.0 / PI)
|
||
<< " ry=" << (cp.rpy.ry * 180.0 / PI)
|
||
<< " rz=" << (cp.rpy.rz * 180.0 / PI) << "\n";
|
||
} else {
|
||
std::cout << "获取 TCP 位姿失败\n";
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (mode == "del") {
|
||
if (delete_coordinated_file()) {
|
||
std::cout << "已删除记录文件: " << get_actions_record_path() << "\n";
|
||
} else {
|
||
std::cout << "记录文件不存在或删除失败: " << get_actions_record_path() << "\n";
|
||
}
|
||
continue;
|
||
}
|
||
|
||
if (mode == "replay") {
|
||
std::string cwd = get_cwd();
|
||
std::cout << "当前工作路径为: " << cwd << "\n";
|
||
std::cout << "请输入记录文件相对路径(回车使用默认: "
|
||
<< get_actions_record_path() << "): ";
|
||
std::string input;
|
||
if (!std::getline(std::cin, input)) break;
|
||
std::string path;
|
||
if (input.empty()) {
|
||
path = get_actions_record_path();
|
||
} else {
|
||
// 将用户输入视为相对于当前工作目录的相对路径
|
||
path = cwd + std::string("/") + input;
|
||
}
|
||
replay_coordinated_file(rc, path);
|
||
continue;
|
||
}
|
||
|
||
if (mode == "rj" || mode == "dj") {
|
||
bool relative = (mode == "rj");
|
||
std::array<double, 6> cur_deg;
|
||
if (!rc.getJointPositionsDeg(cur_deg)) { std::cerr << "获取当前关节位姿失败\n"; continue; }
|
||
|
||
std::array<double, 6> target_deg = cur_deg;
|
||
std::cout << (relative
|
||
? "机械臂关节相对移动：逐关节输入步进(单位度)，直接回车表示保持不变。\n"
|
||
: "机械臂关节绝对移动：逐关节输入目标角度(单位度)，直接回车表示保持不变。\n");
|
||
for (int i = 0; i < 6; ++i) {
|
||
std::cout << "关节 " << i << " 当前角度=" << cur_deg[i]
|
||
<< (relative ? "，请输入步进: " : "，请输入目标角度: ");
|
||
std::string line;
|
||
if (!std::getline(std::cin, line)) break;
|
||
if (line.empty()) continue; // 保持原值
|
||
try {
|
||
double v = std::stod(line);
|
||
if (relative) target_deg[i] = cur_deg[i] + v;
|
||
else target_deg[i] = v;
|
||
} catch (...) {
|
||
std::cout << "输入无效，保持原值 " << cur_deg[i] << "\n";
|
||
}
|
||
}
|
||
|
||
std::array<double, 6> target_rad{};
|
||
for (int i = 0; i < 6; ++i) target_rad[i] = target_deg[i] * PI / 180.0;
|
||
|
||
bool ok = rc.moveJoints(target_rad, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad());
|
||
std::cout << "移动返回: " << (ok ? "成功" : "失败") << std::endl;
|
||
continue;
|
||
}
|
||
|
||
if (mode == "rc" || mode == "dc") {
|
||
bool relative = (mode == "rc");
|
||
|
||
CartesianPose cp;
|
||
if (!rc.getTcpPose(cp)) { std::cerr << "获取当前 TCP 位姿失败\n"; continue; }
|
||
|
||
std::array<double, 6> cur_vals = {
|
||
cp.tran.x,
|
||
cp.tran.y,
|
||
cp.tran.z,
|
||
cp.rpy.rx * 180.0 / PI,
|
||
cp.rpy.ry * 180.0 / PI,
|
||
cp.rpy.rz * 180.0 / PI
|
||
};
|
||
|
||
std::array<double, 6> target_vals = cur_vals;
|
||
std::cout << (relative
|
||
? "笛卡尔相对移动：逐轴输入步进 (x/y/z 单位 mm, rx/ry/rz 单位 deg)，直接回车表示保持不变。\n"
|
||
: "笛卡尔绝对移动：逐轴输入目标值 (x/y/z 单位 mm, rx/ry/rz 单位 deg)，直接回车表示保持不变。\n");
|
||
const char *axis_names[6] = {"x", "y", "z", "rx", "ry", "rz"};
|
||
for (int i = 0; i < 6; ++i) {
|
||
std::cout << axis_names[i] << " 当前=" << cur_vals[i]
|
||
<< (relative ? ", 输入步进: " : ", 输入目标: ");
|
||
std::string line;
|
||
if (!std::getline(std::cin, line)) break;
|
||
if (line.empty()) continue;
|
||
try {
|
||
double v = std::stod(line);
|
||
if (relative) target_vals[i] = cur_vals[i] + v;
|
||
else target_vals[i] = v;
|
||
} catch (...) {
|
||
std::cout << "输入无效，保持原值 " << cur_vals[i] << "\n";
|
||
}
|
||
}
|
||
|
||
std::array<double, 6> offset = {0, 0, 0, 0, 0, 0};
|
||
// 统一用 "目标 - 当前" 计算偏移，rc 使用 INCR, dc 使用 ABS
|
||
for (int i = 0; i < 6; ++i) {
|
||
offset[i] = target_vals[i] - cur_vals[i];
|
||
}
|
||
|
||
bool ok = rc.moveCartesianOffset(offset, relative ? INCR : ABS,
|
||
rc.getLineSpeed(), rc.getLineAcc());
|
||
std::cout << "移动返回: " << (ok ? "成功" : "失败") << std::endl;
|
||
continue;
|
||
}
|
||
|
||
std::cout << "无效命令\n";
|
||
}
|
||
}
|
||
|
||
bool perform_grasp_task(RobotController &rc)
|
||
{
|
||
clear_screen();
|
||
std::cout<<"开始执行抓握"<<std::endl;
|
||
const config::AppConfig &cfg = config::get_default_app_config();
|
||
const auto &grasp_cfg = cfg.grasp;
|
||
|
||
double descend = grasp_cfg.grasp_descend_mm;
|
||
if (descend <= 1.0) {
|
||
descend = 30.0;
|
||
}
|
||
|
||
//加速
|
||
rc.setJointSpeedDeg(grasp_cfg.grasp_joint_speed_deg);
|
||
rc.setJointAccDeg(grasp_cfg.grasp_joint_acc_deg);
|
||
const double line_speed = rc.getLineSpeed();
|
||
const double line_acc = rc.getLineAcc();
|
||
|
||
std::cout<<"A点位置:";
|
||
for (const auto &v : grasp_cfg.grasp_a.joint_deg) {
|
||
std::cout<<v<<", ";
|
||
}
|
||
std::cout<<std::endl;
|
||
std::cout<<"B点位置:";
|
||
for (const auto &v : grasp_cfg.grasp_b.joint_deg) {
|
||
std::cout<<v<<", ";
|
||
}
|
||
std::cout<<std::endl;
|
||
std::cout<<"抓取下降距离:"<<descend<<" mm"<<std::endl;
|
||
|
||
|
||
// A/B 点采用多策略运动：关节 -> 直线 -> 圆弧，避免奇异点导致直接失败
|
||
if (!robot_action::move_to_joint_with_strategies(rc, grasp_cfg.grasp_a.joint_deg)) {
|
||
std::cerr << "移动到抓取 A 点失败，终止抓取任务。" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
if (!rc.moveZRelative(-descend, line_speed, line_acc)) {
|
||
std::cerr << "下降失败，终止抓取任务。" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
// hand_actions::action_grasp_test();
|
||
// sleep_ms(3000);
|
||
|
||
bool grasp_ok = hand_actions::action_quick_grasp_once();
|
||
sleep_ms(500);
|
||
|
||
if (!grasp_ok) {
|
||
std::cerr << "警告：未检测到明显抓取力，可能没有抓住物体，但继续执行搬运流程。" << std::endl;
|
||
}
|
||
|
||
|
||
rc.moveZRelative(descend, line_speed, line_acc);
|
||
if (!robot_action::move_to_joint_with_strategies(rc, grasp_cfg.grasp_b.joint_deg)) {
|
||
std::cerr << "移动到抓取 B 点失败，终止抓取任务。" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
if (!rc.moveZRelative(-descend, line_speed, line_acc)) {
|
||
std::cerr << "放置下降失败，终止抓取任务。" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
hand_actions::action_full_extend();
|
||
sleep_ms(2000);
|
||
rc.moveZRelative(descend, line_speed, line_acc);
|
||
sleep_ms(3000);
|
||
|
||
//速度改回去
|
||
rc.setJointSpeedDeg(cfg.robot.joint_speed_deg_s);
|
||
rc.setJointAccDeg(cfg.robot.joint_acc_deg_s2);
|
||
return true;
|
||
}
|
||
|
||
void run_interaction_loop(RobotController &rc, robot_action::RobotParams ¶ms)
|
||
{
|
||
std::cout << "开始预热" << std::endl;
|
||
std::array<double, 6> current_rad{};
|
||
if (rc.getJointPositions(current_rad)) {
|
||
auto warmup_target = current_rad;
|
||
warmup_target[4] += 10.0 * PI / 180.0;
|
||
|
||
auto attempt_move = [&](const std::array<double, 6> &target) -> bool {
|
||
if (rc.moveJoints(target, ABS, true, rc.getJointSpeedRad(), rc.getJointAccRad())) {
|
||
return true;
|
||
}
|
||
errno_t err = rc.getLastErrorCode();
|
||
if (err == -12) {
|
||
std::cout << "检测到错误 -12，忽略并继续初始化。" << std::endl;
|
||
return true;
|
||
}
|
||
std::cerr << "预热关节移动失败，错误码: " << err << std::endl;
|
||
return false;
|
||
};
|
||
|
||
if (!attempt_move(warmup_target))
|
||
return;
|
||
|
||
if (!attempt_move(current_rad))
|
||
return;
|
||
|
||
} else {
|
||
std::cerr << "无法获取当前关节位姿，跳过关节预热动作。" << std::endl;
|
||
}
|
||
|
||
auto print_menu = []() {
|
||
std::cout << "手势控制提示:\n";
|
||
std::cout << " - 无名指 触发挥手+握手演示\n";
|
||
std::cout << " - 中指 触发石头剪刀布\n";
|
||
std::cout << " - 食指 触发抓取演示\n";
|
||
std::cout << " - 大拇指旋转 触发进入控制台菜单 (1:重设速度, 2:示教模式)\n";
|
||
std::cout << " - 小拇指 退出程序\n";
|
||
};
|
||
|
||
bool instructions_shown = false;
|
||
|
||
coordinated_actions::set_force_baseline_ready();
|
||
|
||
while (true) {
|
||
robot_action::init_place(rc);
|
||
hand_actions::action_full_extend();
|
||
sleep_ms(2000);
|
||
|
||
const std::string selection = force_trigger_selection();
|
||
if (selection.empty()) {
|
||
if (!instructions_shown) {
|
||
clear_screen();
|
||
print_menu();
|
||
instructions_shown = true;
|
||
}
|
||
sleep_ms(100);
|
||
continue;
|
||
}
|
||
|
||
instructions_shown = false;
|
||
|
||
if (selection == "exit") {
|
||
clear_screen();
|
||
std::cout << "检测到退出手势，正在结束程序...\n";
|
||
sleep_ms(1200);
|
||
break;
|
||
}
|
||
|
||
if (selection == "wave") {
|
||
wave_and_handshake(rc);
|
||
clear_screen();
|
||
print_menu();
|
||
coordinated_actions::set_force_baseline_ready();
|
||
continue;
|
||
}
|
||
if (selection == "rps") {
|
||
play_rps_game(rc);
|
||
clear_screen();
|
||
print_menu();
|
||
coordinated_actions::set_force_baseline_ready();
|
||
continue;
|
||
}
|
||
if (selection == "console") {
|
||
if (!run_console_menu(rc, params)) {
|
||
break;
|
||
}
|
||
clear_screen();
|
||
print_menu();
|
||
coordinated_actions::set_force_baseline_ready();
|
||
continue;
|
||
}
|
||
if (selection == "grasp") {
|
||
perform_grasp_task(rc);
|
||
sleep_ms(1000);
|
||
clear_screen();
|
||
print_menu();
|
||
coordinated_actions::set_force_baseline_ready();
|
||
continue;
|
||
}
|
||
}
|
||
}
|
||
|
||
} // namespace coordinated_actions
|