393 lines
14 KiB
C++
393 lines
14 KiB
C++
#include "robot_controller.h"
|
||
#include <iostream>
|
||
#include <array>
|
||
#include <fstream>
|
||
#include <ctime>
|
||
#include <cstdio>
|
||
#include <sstream>
|
||
#include <cmath>
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846
|
||
#endif
|
||
#include <thread>
|
||
#include <chrono>
|
||
#include "error_codes.h"
|
||
#include "tools/tools.h"
|
||
#if defined(_WIN32)
|
||
#include <direct.h>
|
||
#else
|
||
#include <unistd.h>
|
||
#include <limits.h>
|
||
#endif
|
||
#if !defined(_WIN32)
|
||
#include <sys/stat.h>
|
||
#include <sys/types.h>
|
||
#endif
|
||
|
||
namespace {
|
||
// 将极小的浮点误差归零，避免打印出 -4.33e-19 这类值
|
||
inline double clamp_near_zero(double v, double eps = 1e-8)
|
||
{
|
||
return (std::fabs(v) < eps) ? 0.0 : v;
|
||
}
|
||
} // namespace
|
||
|
||
RobotController::RobotController() {
|
||
connected_ = false;
|
||
}
|
||
|
||
RobotController::~RobotController() {
|
||
if (connected_) {
|
||
demo_.login_out();
|
||
connected_ = false;
|
||
}
|
||
}
|
||
|
||
bool RobotController::connect(const std::string &addr) {
|
||
errno_t ret = demo_.login_in(addr.c_str());
|
||
if (ret == 0) {
|
||
connected_ = true;
|
||
return true;
|
||
}
|
||
std::cerr << "connect failed, err=" << ret << std::endl;
|
||
return false;
|
||
}
|
||
|
||
// ----------------- 速度/加速度 getter/setter 实现 -----------------
|
||
void RobotController::setJointSpeedDeg(double v) { joint_speed_deg_s_ = v; }
|
||
double RobotController::getJointSpeedDeg() const { return joint_speed_deg_s_; }
|
||
double RobotController::getJointSpeedRad() const { return joint_speed_deg_s_ * M_PI / 180.0; }
|
||
|
||
void RobotController::setJointAccDeg(double a) { joint_acc_deg_s2_ = a; }
|
||
double RobotController::getJointAccDeg() const { return joint_acc_deg_s2_; }
|
||
double RobotController::getJointAccRad() const { return joint_acc_deg_s2_ * M_PI / 180.0; }
|
||
|
||
void RobotController::setLineSpeed(double v_mm_s) { line_speed_mm_s_ = v_mm_s; }
|
||
double RobotController::getLineSpeed() const { return line_speed_mm_s_; }
|
||
|
||
void RobotController::setLineAcc(double a_mm_s2) { line_acc_mm_s2_ = a_mm_s2; }
|
||
double RobotController::getLineAcc() const { return line_acc_mm_s2_; }
|
||
|
||
void RobotController::setPoseSpeedDeg(double v_deg_s) { pose_speed_deg_s_ = v_deg_s; }
|
||
double RobotController::getPoseSpeedDeg() const { return pose_speed_deg_s_; }
|
||
double RobotController::getPoseSpeedRad() const { return pose_speed_deg_s_ * M_PI / 180.0; }
|
||
|
||
void RobotController::setPoseAccDeg(double a_deg_s2) { pose_acc_deg_s2_ = a_deg_s2; }
|
||
double RobotController::getPoseAccDeg() const { return pose_acc_deg_s2_; }
|
||
double RobotController::getPoseAccRad() const { return pose_acc_deg_s2_ * M_PI / 180.0; }
|
||
|
||
void RobotController::disconnect() {
|
||
if (connected_) {
|
||
demo_.login_out();
|
||
connected_ = false;
|
||
}
|
||
}
|
||
|
||
bool RobotController::powerOn() {
|
||
errno_t ret = demo_.power_on();
|
||
if (ret != 0) std::cerr << "power_on failed: " << ret << std::endl;
|
||
return ret == 0;
|
||
}
|
||
|
||
bool RobotController::powerOff() {
|
||
errno_t ret = demo_.power_off();
|
||
if (ret != 0) std::cerr << "power_off failed: " << ret << std::endl;
|
||
return ret == 0;
|
||
}
|
||
|
||
bool RobotController::enableRobot() {
|
||
errno_t ret = demo_.enable_robot();
|
||
if (ret != 0) std::cerr << "enable_robot failed: " << ret << std::endl;
|
||
return ret == 0;
|
||
}
|
||
|
||
bool RobotController::disableRobot() {
|
||
errno_t ret = demo_.disable_robot();
|
||
if (ret != 0) std::cerr << "disable_robot failed: " << ret << std::endl;
|
||
return ret == 0;
|
||
}
|
||
|
||
bool RobotController::getTcpPose(CartesianPose &pose) {
|
||
errno_t ret = demo_.get_tcp_position(&pose);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "get_tcp_position failed: " << ret << " -> " << describeError(ret) << std::endl;
|
||
return false;
|
||
}
|
||
// 对返回的姿态做一次浮点误差归零（尤其是接近 0 的角度/位置）
|
||
pose.tran.x = clamp_near_zero(pose.tran.x);
|
||
pose.tran.y = clamp_near_zero(pose.tran.y);
|
||
pose.tran.z = clamp_near_zero(pose.tran.z);
|
||
pose.rpy.rx = clamp_near_zero(pose.rpy.rx);
|
||
pose.rpy.ry = clamp_near_zero(pose.rpy.ry);
|
||
pose.rpy.rz = clamp_near_zero(pose.rpy.rz);
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::getJointPositions(std::array<double,6> &out_joints_rad) {
|
||
JointValue jv;
|
||
errno_t ret = demo_.get_joint_position(&jv);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "get_joint_position failed: " << ret << " -> " << describeError(ret) << std::endl;
|
||
return false;
|
||
}
|
||
for (int i = 0; i < 6; ++i) {
|
||
out_joints_rad[i] = clamp_near_zero(jv.jVal[i]);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::getJointPositionsDeg(std::array<double,6> &out_joints_deg) {
|
||
std::array<double,6> rads;
|
||
if (!getJointPositions(rads)) return false;
|
||
for (int i = 0; i < 6; ++i) {
|
||
double deg = rads[i] * 180.0 / M_PI;
|
||
out_joints_deg[i] = clamp_near_zero(deg);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::forwardKinematics(const std::array<double,6> &joints_rad, CartesianPose &out_pose) {
|
||
JointValue jv;
|
||
for (int i = 0; i < 6; ++i) {
|
||
jv.jVal[i] = joints_rad[i];
|
||
}
|
||
errno_t ret = demo_.kine_forward(&jv, &out_pose);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "kine_forward failed: " << ret << " -> " << describeError(ret) << std::endl;
|
||
return false;
|
||
}
|
||
// 对计算结果做一次浮点误差归零
|
||
out_pose.tran.x = clamp_near_zero(out_pose.tran.x);
|
||
out_pose.tran.y = clamp_near_zero(out_pose.tran.y);
|
||
out_pose.tran.z = clamp_near_zero(out_pose.tran.z);
|
||
out_pose.rpy.rx = clamp_near_zero(out_pose.rpy.rx);
|
||
out_pose.rpy.ry = clamp_near_zero(out_pose.rpy.ry);
|
||
out_pose.rpy.rz = clamp_near_zero(out_pose.rpy.rz);
|
||
last_errno_ = ERR_SUCC;
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::moveCartesianOffset(const std::array<double,6> &offset, MoveMode mode, double lin_speed, double lin_acc) {
|
||
// offset: dx,dy,dz (mm), drx,dry,drz (deg)
|
||
CartesianPose pose;
|
||
if (!getTcpPose(pose)) return false;
|
||
pose.tran.x += offset[0];
|
||
pose.tran.y += offset[1];
|
||
pose.tran.z += offset[2];
|
||
// orientation offsets: input degrees -> convert to radians
|
||
pose.rpy.rx += offset[3] * M_PI / 180.0;
|
||
pose.rpy.ry += offset[4] * M_PI / 180.0;
|
||
pose.rpy.rz += offset[5] * M_PI / 180.0;
|
||
|
||
double use_lin_speed = lin_speed > 0.0 ? lin_speed : line_speed_mm_s_;
|
||
double use_lin_acc = lin_acc > 0.0 ? lin_acc : line_acc_mm_s2_;
|
||
double ori_vel = getPoseSpeedRad();
|
||
double ori_acc = getPoseAccRad();
|
||
|
||
errno_t ret = demo_.linear_move(&pose, mode, TRUE, use_lin_speed, use_lin_acc, 1, NULL, ori_vel, ori_acc);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "moveCartesianOffset linear_move failed: " << ret << " -> " << describeError(ret) << std::endl;
|
||
// 不自动重试，按用户要求只报告错误信息
|
||
return false;
|
||
}
|
||
last_errno_ = ERR_SUCC;
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::moveLinearToPose(const CartesianPose &pose,
|
||
MoveMode mode,
|
||
bool is_block,
|
||
double speed,
|
||
double accel,
|
||
double tol) {
|
||
CartesianPose target = pose;
|
||
double use_speed = speed > 0.0 ? speed : line_speed_mm_s_;
|
||
double use_acc = accel > 0.0 ? accel : line_acc_mm_s2_;
|
||
double ori_vel = getPoseSpeedRad();
|
||
double ori_acc = getPoseAccRad();
|
||
|
||
errno_t ret = demo_.linear_move(&target,
|
||
mode,
|
||
is_block ? TRUE : FALSE,
|
||
use_speed,
|
||
use_acc,
|
||
tol,
|
||
nullptr,
|
||
ori_vel,
|
||
ori_acc);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "moveLinearToPose linear_move failed: " << ret
|
||
<< " -> " << describeError(ret) << std::endl;
|
||
return false;
|
||
}
|
||
last_errno_ = ERR_SUCC;
|
||
return true;
|
||
}
|
||
|
||
|
||
bool RobotController::moveZRelative(double delta_z, double velocity, double accel) {
|
||
CartesianPose pose;
|
||
if (!getTcpPose(pose)) return false;
|
||
// 提示：传入 delta_z 单位为 mm，正值为向上
|
||
pose.tran.z += delta_z;
|
||
errno_t ret = demo_.linear_move(&pose, ABS, true, velocity, accel, 1, NULL);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "linear_move failed: " << ret << " -> " << describeError(ret) << std::endl;
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::moveCircular(const CartesianPose &mid_pos,
|
||
const CartesianPose &end_pos,
|
||
MoveMode mode,
|
||
bool is_block,
|
||
double speed,
|
||
double accel,
|
||
double tol,
|
||
int circle_cnt,
|
||
int circle_mode) {
|
||
CartesianPose mid = mid_pos;
|
||
CartesianPose end = end_pos;
|
||
double use_speed = speed > 0.0 ? speed : line_speed_mm_s_;
|
||
double use_acc = accel > 0.0 ? accel : line_acc_mm_s2_;
|
||
|
||
errno_t ret = demo_.circular_move(&end,
|
||
&mid,
|
||
mode,
|
||
is_block ? TRUE : FALSE,
|
||
use_speed,
|
||
use_acc,
|
||
tol,
|
||
nullptr,
|
||
circle_cnt,
|
||
circle_mode);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "moveCircular circular_move failed: " << ret
|
||
<< " -> " << describeError(ret) << std::endl;
|
||
return false;
|
||
}
|
||
last_errno_ = ERR_SUCC;
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::moveJoints(const std::array<double,6> &joints, MoveMode mode, bool is_block, double speed, double acc, double tol) {
|
||
JointValue jv;
|
||
for (int i = 0; i < 6; ++i) jv.jVal[i] = joints[i];
|
||
errno_t ret = demo_.joint_move(&jv, mode, is_block ? TRUE : FALSE, speed, acc, tol, nullptr);
|
||
if (ret != 0) {
|
||
last_errno_ = ret;
|
||
std::cerr << "joint_move failed: " << ret << " -> " << describeError(ret) << std::endl;
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void RobotController::motionAbort() {
|
||
demo_.motion_abort();
|
||
}
|
||
|
||
std::string RobotController::getSdkVersion() {
|
||
char ver[128] = {0};
|
||
demo_.get_sdk_version(ver);
|
||
return std::string(ver);
|
||
}
|
||
|
||
|
||
bool RobotController::recordCurrentPose(const std::string &comment, const std::string &fname) {
|
||
std::string out_fname = fname;
|
||
if (out_fname.empty()) {
|
||
out_fname = tools::get_cwd() + std::string("/../actions/recorded_points.csv");
|
||
}
|
||
// 确保目录存在（../actions）
|
||
std::string dir = out_fname;
|
||
auto pos = dir.find_last_of('/');
|
||
if (pos == std::string::npos) pos = dir.find_last_of('\\');
|
||
if (pos != std::string::npos) dir = dir.substr(0, pos);
|
||
if (!dir.empty()) {
|
||
#if defined(_WIN32)
|
||
_mkdir(dir.c_str());
|
||
#else
|
||
mkdir(dir.c_str(), 0755);
|
||
#endif
|
||
}
|
||
std::array<double,6> cur_deg;
|
||
if (!getJointPositionsDeg(cur_deg)) {
|
||
last_errno_ = -1;
|
||
std::cerr << "recordCurrentPose: 获取关节位姿失败\n";
|
||
return false;
|
||
}
|
||
std::ofstream ofs(out_fname.c_str(), std::ios::app);
|
||
if (!ofs) {
|
||
std::cerr << "recordCurrentPose: 无法打开文件: " << out_fname << std::endl;
|
||
return false;
|
||
}
|
||
auto t = std::time(nullptr);
|
||
ofs << t;
|
||
for (int i=0;i<6;++i) ofs << "," << cur_deg[i];
|
||
ofs << "," << '"' << comment << '"' << std::endl;
|
||
ofs.close();
|
||
return true;
|
||
}
|
||
|
||
bool RobotController::deleteRecordedFile(const std::string &fname) {
|
||
std::string out_fname = fname;
|
||
if (out_fname.empty()) {
|
||
out_fname = tools::get_cwd() + std::string("/../actions/recorded_points.csv");
|
||
}
|
||
if (std::remove(out_fname.c_str()) == 0) return true;
|
||
return false;
|
||
}
|
||
|
||
bool RobotController::replayRecordedPoints(const std::string &fname) {
|
||
std::string in_fname = fname;
|
||
if (in_fname.empty()) in_fname = tools::get_cwd() + std::string("/../actions/recorded_points.csv");
|
||
std::ifstream ifs(in_fname.c_str());
|
||
if (!ifs) {
|
||
std::cerr << "无法打开记录文件: " << in_fname << 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, ',')) { std::cerr << "解析第"<<idx_line<<"行失败\n"; continue; }
|
||
std::array<double,6> joints_deg = {0,0,0,0,0,0};
|
||
bool ok_parse = true;
|
||
for (int i=0;i<6;++i) {
|
||
if (!std::getline(ss, token, ',')) { ok_parse = false; break; }
|
||
try { joints_deg[i] = std::stod(token); } catch(...) { ok_parse = false; break; }
|
||
}
|
||
if (!ok_parse) { std::cerr << "解析第"<<idx_line<<"行关节数据失败\n"; continue; }
|
||
std::string comment;
|
||
if (std::getline(ss, comment)) {
|
||
if (!comment.empty() && comment.front() == '"') comment.erase(0,1);
|
||
if (!comment.empty() && comment.back() == '"') comment.pop_back();
|
||
} else comment = "";
|
||
std::array<double,6> joints_rad;
|
||
for (int i=0;i<6;++i) joints_rad[i] = joints_deg[i] * M_PI / 180.0;
|
||
std::cout << "复现第"<<idx_line<<"，注释: " << comment << "\n";
|
||
bool moved = moveJoints(joints_rad, ABS, true, getJointSpeedRad(), getJointAccRad());
|
||
if (!moved) {
|
||
std::cerr << "移动到记录点 "<<idx_line<<" 失败，错误码="<< getLastErrorCode() <<"\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 {
|
||
std::cout << "完成第"<<idx_line<<"个点\n";
|
||
}
|
||
}
|
||
ifs.close();
|
||
std::cout << "顺序复现结束。\n";
|
||
return true;
|
||
}
|