446 lines
13 KiB
C++
446 lines
13 KiB
C++
/**
|
||
* @file FairinoRobotWrapper.cpp
|
||
* @brief Fairino机器人SDK二次封装实现
|
||
*/
|
||
|
||
#include "FairinoRobotWrapper.h"
|
||
#include <cstring>
|
||
|
||
FairinoRobotWrapper::FairinoRobotWrapper()
|
||
: robot_(std::make_unique<FRRobot>())
|
||
, connected_(false)
|
||
, last_error_(ERR_SUCCESS)
|
||
{
|
||
}
|
||
|
||
FairinoRobotWrapper::~FairinoRobotWrapper()
|
||
{
|
||
if (connected_) {
|
||
Disconnect();
|
||
}
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::Connect(const std::string& ip)
|
||
{
|
||
if (connected_) {
|
||
return ERR_SUCCESS;
|
||
}
|
||
|
||
last_error_ = robot_->RPC(ip.c_str());
|
||
if (last_error_ == ERR_SUCCESS) {
|
||
connected_ = true;
|
||
}
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::Disconnect()
|
||
{
|
||
if (!connected_) {
|
||
return ERR_SUCCESS;
|
||
}
|
||
|
||
last_error_ = robot_->CloseRPC();
|
||
connected_ = false;
|
||
return last_error_;
|
||
}
|
||
|
||
bool FairinoRobotWrapper::IsConnected() const
|
||
{
|
||
return connected_;
|
||
}
|
||
|
||
// ==================== 运动控制接口 ====================
|
||
|
||
errno_t FairinoRobotWrapper::MoveL(double x, double y, double z,
|
||
double rx, double ry, double rz,
|
||
float vel, float acc, float blendR,
|
||
int tool, int user)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
// 构造目标笛卡尔位姿
|
||
DescPose desc_pos(x, y, z, rx, ry, rz);
|
||
|
||
// 扩展轴位置（不使用）
|
||
ExaxisPos epos;
|
||
memset(&epos, 0, sizeof(epos));
|
||
|
||
// 偏移量（不偏移）
|
||
DescPose offset_pos;
|
||
|
||
// 使用不需要关节位置输入的MoveL重载版本
|
||
last_error_ = robot_->MoveL(&desc_pos, tool, user, vel, acc, 100.0f, blendR, 0, &epos, 0, 0, &offset_pos, -1, 0, 0, 10);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::MoveJByPose(double x, double y, double z,
|
||
double rx, double ry, double rz,
|
||
float vel, float acc, float blendT,
|
||
int tool, int user)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
// 首先使用逆运动学计算关节位置
|
||
DescPose desc_pos(x, y, z, rx, ry, rz);
|
||
JointPos joint_pos;
|
||
|
||
last_error_ = robot_->GetInverseKin(0, &desc_pos, -1, &joint_pos);
|
||
if (last_error_ != ERR_SUCCESS) {
|
||
return last_error_;
|
||
}
|
||
|
||
// 扩展轴位置（不使用）
|
||
ExaxisPos epos;
|
||
memset(&epos, 0, sizeof(epos));
|
||
|
||
// 偏移量（不偏移）
|
||
DescPose offset_pos;
|
||
|
||
// 使用MoveJ进行关节空间运动
|
||
last_error_ = robot_->MoveJ(&joint_pos, &desc_pos, tool, user, vel, acc, 100.0f, &epos, blendT, 0, &offset_pos);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::MoveJByJoint(double j1, double j2, double j3,
|
||
double j4, double j5, double j6,
|
||
float vel, float acc, float blendT,
|
||
int tool, int user)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
// 构造目标关节位置
|
||
JointPos joint_pos(j1, j2, j3, j4, j5, j6);
|
||
|
||
// 扩展轴位置（不使用）
|
||
ExaxisPos epos;
|
||
memset(&epos, 0, sizeof(epos));
|
||
|
||
// 偏移量（不偏移）
|
||
DescPose offset_pos;
|
||
|
||
// 使用不需要笛卡尔位置输入的MoveJ重载版本
|
||
last_error_ = robot_->MoveJ(&joint_pos, tool, user, vel, acc, 100.0f, &epos, blendT, 0, &offset_pos);
|
||
return last_error_;
|
||
}
|
||
|
||
// ==================== 辅助接口 ====================
|
||
|
||
errno_t FairinoRobotWrapper::StopMotion()
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->StopMotion();
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::PauseMotion()
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->PauseMotion();
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::ResumeMotion()
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->ResumeMotion();
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::RobotEnable(uint8_t state)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->RobotEnable(state);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::ResetAllError()
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->ResetAllError();
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::GetCurrentJointPos(double& j1, double& j2, double& j3,
|
||
double& j4, double& j5, double& j6,
|
||
uint8_t blocking)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
JointPos joint_pos;
|
||
last_error_ = robot_->GetActualJointPosDegree(blocking, &joint_pos);
|
||
if (last_error_ == ERR_SUCCESS) {
|
||
j1 = joint_pos.jPos[0];
|
||
j2 = joint_pos.jPos[1];
|
||
j3 = joint_pos.jPos[2];
|
||
j4 = joint_pos.jPos[3];
|
||
j5 = joint_pos.jPos[4];
|
||
j6 = joint_pos.jPos[5];
|
||
}
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::GetCurrentTCPPose(double& x, double& y, double& z,
|
||
double& rx, double& ry, double& rz,
|
||
uint8_t blocking)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
DescPose desc_pos;
|
||
last_error_ = robot_->GetActualTCPPose(blocking, &desc_pos);
|
||
if (last_error_ == ERR_SUCCESS) {
|
||
x = desc_pos.tran.x;
|
||
y = desc_pos.tran.y;
|
||
z = desc_pos.tran.z;
|
||
rx = desc_pos.rpy.rx;
|
||
ry = desc_pos.rpy.ry;
|
||
rz = desc_pos.rpy.rz;
|
||
}
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::GetMotionDone(uint8_t& state)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->GetRobotMotionDone(&state);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::GetErrorCode(int& maincode, int& subcode)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->GetRobotErrorCode(&maincode, &subcode);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::GetRobotState(ROBOT_STATE_PKG& state)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->GetRobotRealTimeState(&state);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::SetToolCoord(int id, double x, double y, double z,
|
||
double rx, double ry, double rz)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
DescPose coord(x, y, z, rx, ry, rz);
|
||
last_error_ = robot_->SetToolCoord(id, &coord, 0, 0, 0, 0);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::SetWObjCoord(int id, double x, double y, double z,
|
||
double rx, double ry, double rz, int refFrame)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
DescPose coord(x, y, z, rx, ry, rz);
|
||
last_error_ = robot_->SetWObjCoord(id, &coord, refFrame);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::SetSpeed(int vel)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->SetSpeed(vel);
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::Wait(int ms)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
last_error_ = robot_->WaitMs(ms);
|
||
return last_error_;
|
||
}
|
||
|
||
std::string FairinoRobotWrapper::GetSDKVersion()
|
||
{
|
||
char version[64] = {0};
|
||
if (robot_) {
|
||
robot_->GetSDKVersion(version);
|
||
}
|
||
return std::string(version);
|
||
}
|
||
|
||
std::string FairinoRobotWrapper::GetControllerIP()
|
||
{
|
||
if (!connected_) {
|
||
return "";
|
||
}
|
||
|
||
char ip[64] = {0};
|
||
robot_->GetControllerIP(ip);
|
||
return std::string(ip);
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::ForwardKinematics(double j1, double j2, double j3,
|
||
double j4, double j5, double j6,
|
||
double& x, double& y, double& z,
|
||
double& rx, double& ry, double& rz)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
JointPos joint_pos(j1, j2, j3, j4, j5, j6);
|
||
DescPose desc_pos;
|
||
|
||
last_error_ = robot_->GetForwardKin(&joint_pos, &desc_pos);
|
||
if (last_error_ == ERR_SUCCESS) {
|
||
x = desc_pos.tran.x;
|
||
y = desc_pos.tran.y;
|
||
z = desc_pos.tran.z;
|
||
rx = desc_pos.rpy.rx;
|
||
ry = desc_pos.rpy.ry;
|
||
rz = desc_pos.rpy.rz;
|
||
}
|
||
return last_error_;
|
||
}
|
||
|
||
errno_t FairinoRobotWrapper::InverseKinematics(double x, double y, double z,
|
||
double rx, double ry, double rz,
|
||
double& j1, double& j2, double& j3,
|
||
double& j4, double& j5, double& j6,
|
||
int config)
|
||
{
|
||
if (!connected_) {
|
||
last_error_ = ERR_SOCKET_COM_FAILED;
|
||
return last_error_;
|
||
}
|
||
|
||
DescPose desc_pos(x, y, z, rx, ry, rz);
|
||
JointPos joint_pos;
|
||
|
||
last_error_ = robot_->GetInverseKin(0, &desc_pos, config, &joint_pos);
|
||
if (last_error_ == ERR_SUCCESS) {
|
||
j1 = joint_pos.jPos[0];
|
||
j2 = joint_pos.jPos[1];
|
||
j3 = joint_pos.jPos[2];
|
||
j4 = joint_pos.jPos[3];
|
||
j5 = joint_pos.jPos[4];
|
||
j6 = joint_pos.jPos[5];
|
||
}
|
||
return last_error_;
|
||
}
|
||
|
||
std::string FairinoRobotWrapper::GetLastErrorString() const
|
||
{
|
||
return ErrorToString(last_error_);
|
||
}
|
||
|
||
std::string FairinoRobotWrapper::ErrorToString(errno_t error) const
|
||
{
|
||
switch (error) {
|
||
case ERR_SUCCESS:
|
||
return "成功";
|
||
case ERR_OTHER:
|
||
return "其他错误";
|
||
case ERR_SOCKET_COM_FAILED:
|
||
return "网络通讯异常";
|
||
case ERR_XMLRPC_COM_FAILED:
|
||
return "XMLRPC通讯失败，请检查网络连接以及服务器IP地址";
|
||
case ERR_XMLRPC_CMD_FAILED:
|
||
return "XMLRPC接口执行失败";
|
||
case ERR_LUA_FILE_NOT_FOUND:
|
||
return "LUA文件不存在";
|
||
case ERR_SAVE_FILE_PATH_NOT_FOUND:
|
||
return "保存文件路径不存在";
|
||
case ERR_UPLOAD_FILE_NOT_FOUND:
|
||
return "上传文件不存在";
|
||
case ERR_DOWN_LOAD_FILE_FAILED:
|
||
return "文件下载失败";
|
||
case ERR_DOWN_LOAD_FILE_CHECK_FAILED:
|
||
return "文件下载校验失败";
|
||
case ERR_DOWN_LOAD_FILE_WRITE_FAILED:
|
||
return "下载文件写入失败";
|
||
case ERR_FILE_NAME:
|
||
return "文件名称异常";
|
||
case ERR_UPLOAD_FILE_ERROR:
|
||
return "上传文件异常";
|
||
case ERR_FILE_TOO_LARGE:
|
||
return "文件大小超限";
|
||
case ERR_FILE_OPEN_FAILED:
|
||
return "文件打开失败";
|
||
case ERR_SOCKET_SEND_FAILED:
|
||
return "Socket发送失败";
|
||
case ERR_SOCKET_RECV_FAILED:
|
||
return "Socket接收失败";
|
||
case ERR_PARAM_NUM:
|
||
return "参数个数不一致";
|
||
case ERR_PARAM_VALUE:
|
||
return "参数值不在合理范围";
|
||
case ERR_TPD_FILE_OPEN_FAILED:
|
||
return "TPD文件打开失败";
|
||
case ERR_EXECUTION_FAILED:
|
||
return "指令执行失败";
|
||
case ERR_PROGRAM_IS_RUNNING:
|
||
return "程序正在运行";
|
||
case ERR_COMPUTE_FAILED:
|
||
return "计算失败";
|
||
case ERR_INVERSE_KINEMATICS_COMPUTE_FAILED:
|
||
return "逆运动学计算失败";
|
||
case ERR_SERVOJ_JOINT_OVERRUN:
|
||
return "关节值超限";
|
||
case ERR_NON_RESSETTABLE_FAULT:
|
||
return "不可复位故障，请断电重启控制箱";
|
||
case ERR_STRANGE_POSE:
|
||
return "奇异位姿";
|
||
case ERR_TARGET_POSE_CANNOT_REACHED:
|
||
return "目标位姿无法到达";
|
||
default:
|
||
return "未知错误码: " + std::to_string(error);
|
||
}
|
||
}
|