12：遨博机器人SDK二次开发-海口c网

一、流程步骤

1.获取当前点关节坐标

2.走当前点关节坐标

1.获取目标点x,y,z（位置坐标，以m为单位，需要*1000变成mm）和四元素（位姿坐标）

2.四元素→欧拉角（弧度制）

3.欧拉角（弧度制）→（角度制）

4.得到x,y,z+欧拉角坐标（法兰盘坐标）

5.法兰盘坐标→工具末端坐标

5.保存该点

换一个位置

1.读取该点：工具末端坐标→法兰盘坐标

2.法兰盘坐标：x,y,z + 欧拉角（角度制→弧度制）→目标点四元素

3.获取当前点坐标 + 法兰盘转化后的 x,y,z + 目标点四元素

4.逆解得到目标点的关节坐标，然后走点。

二、程序

.pro

INCLUDEPATH += $$PWD/include/HalconX20
INCLUDEPATH += $$PWD/include/HalconX20/halconcppLIBS += -L$$PWD/lib/HalconX20 -lhalcon
LIBS += -L$$PWD/lib/HalconX20 -lhalconcppINCLUDEPATH += $$PWD/include/AuboX64
LIBS += -L$$PWD/lib/AuboX64 -llibserviceinterface

.h文件

#ifndef MAINWINDOW_H
#define MAINWINDOW_H#include <QMainWindow>
#include "rsdef.h"
#include "HalconCpp.h"using namespace  HalconCpp;namespace Ui {
class MainWindow;
}class MainWindow : public QMainWindow
{Q_OBJECTpublic:explicit MainWindow(QWidget *parent = 0);~MainWindow();//机械臂控制上下文句柄RSHD g_rshd = -1;bool example_login(RSHD &rshd, const char * addr, int port);  //登录int GetCurrentWayPoint();                                     //获取坐标bool example_moveJ(RSHD rshd);                                //走关节角void GetPoseFromWayPoint(wayPoint_S wayPoint,  HTuple  &Point1);  //路点 转  halcon posevoid WriteHalconPose(HTuple Point1);                              //保存halcon posevoid ChangeFlangerToToolPose(HTuple FlangerCenterPoint,HTuple &ToolInBasePose); //法兰盘转工具//读取 坐标 并且走点void ReadLocalToolPose(HTuple &Point13);    //读取  工具坐标void ChangeToolPoseToFlangerPose(HTuple ToolInBasePose,HTuple &FlangerCenterPoseX);//工具转法兰盘void GetOrientationPoseFromHalconPose(HTuple Point13,Pos &pos,wayPoint_S &wayPoint2);//换算4元素与pose，从halconPosevoid RunInverseKinPoint(Pos pos,wayPoint_S wayPoint2);  //逆解与走点bool result = false;
private slots:void on_pB_ConnectRobtics_clicked();void on_pB_GetCurrentWayPoint_clicked();void on_pB_GetJointPos_clicked();void on_pB_MoveGJJ_clicked();void on_pB_MoveFlangerCenterPose_clicked();void on_RobSpeed_valueChanged(double arg1);private:Ui::MainWindow *ui;
};#endif // MAINWINDOW_H

.cpp头文件

#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "qdebug.h"
#include "qfile.h"
#define ROBOT_ADDR "192.168.2.60"
#define ROBOT_PORT 8899
#define M_PI 3.14159265358979323846
MainWindow::MainWindow(QWidget *parent) :QMainWindow(parent),ui(new Ui::MainWindow)
{ui->setupUi(this);
}MainWindow::~MainWindow()
{delete ui;
}

1.连接机器人

void MainWindow::on_pB_ConnectRobtics_clicked()
{if(result){return;}example_login(g_rshd, ROBOT_ADDR, ROBOT_PORT);
}bool MainWindow::example_login(RSHD &rshd, const char * addr, int port)
{rshd = RS_FAILED;//初始化接口库if (rs_initialize() == RS_SUCC){//创建上下文if (rs_create_context(&rshd)  == RS_SUCC ){//登陆机械臂服务器if (rs_login(rshd, addr, port) == RS_SUCC){result = true;//登陆成功ui->textBrowser->append(QString::fromLocal8Bit("连接机器人成功"));std::cout<<"login succ"<<std::endl;}else{//登陆失败std::cerr<<"login failed"<<std::endl;ui->textBrowser->append(QString::fromLocal8Bit("连接机器人失败"));}}else{//创建上下文失败std::cerr<<"rs_create_context error"<<std::endl;ui->textBrowser->append(QString::fromLocal8Bit("连接机器人失败"));}}else{//初始化接口库失败std::cerr<<"rs_initialize error"<<std::endl;ui->textBrowser->append(QString::fromLocal8Bit("连接机器人失败"));}return result;
}

2.获取关节角坐标

void MainWindow::on_pB_GetJointPos_clicked()
{wayPoint_S wayPoint;  //AOBO  自身定位 数据类型if (RS_SUCC==rs_get_current_waypoint(g_rshd, &wayPoint)){//保存 poseHTuple  Point1;Point1.Clear();Point1[0] = wayPoint.jointpos[0];Point1[1] = wayPoint.jointpos[1];Point1[2] = wayPoint.jointpos[2];Point1[3] = wayPoint.jointpos[3];Point1[4] = wayPoint.jointpos[4];Point1[5] = wayPoint.jointpos[5];Point1[6] = 0; //旋转轴 ： --- 机器人  2   ， halcon  0int Num=ui->PointNum->value();QString FileName="./Image/JointPose"+QString::number(Num)+".dat";HTuple  FileName1=HTuple(FileName.toLatin1().data());  //QString类型  转换到HTuple 方法WritePose(Point1, FileName1);ui->textBrowser->append("jointpos[0]:"+QString::number(Point1[0].D()));ui->textBrowser->append(QString::fromLocal8Bit("坐标保存在:")+FileName);}else{ui->textBrowser->append(QString::fromLocal8Bit("坐标获取失败"));}
}

3.走关节角坐标

//走关节角
void MainWindow::on_pB_MoveGJJ_clicked()
{example_moveJ(g_rshd);
}bool MainWindow::example_moveJ(RSHD rshd)
{bool result = false;RobotRecongnitionParam param;rs_get_robot_recognition_param(rshd, 1, &param);//该位置为机械臂的初始位置（提供6个关节角的关节信息（单位：弧度））//加载点HTuple Point13;int Num=ui->PointNum->value();QString FileName="./Image/JointPose"+QString::number(Num)+".dat";HTuple  FileName1=HTuple(FileName.toLatin1().data());  //QString类型  转换到HTuple 方法try{ReadPose(FileName1, &Point13);}catch(...){ui->textBrowser->append(QString::fromLocal8Bit("读取路点成功"));return false;}double initPos[6]={Point13[0].D(),Point13[1].D(),Point13[2].D(),Point13[3].D(),Point13[4].D(),Point13[5].D()};//首先运动到初始位置if (rs_move_joint(rshd, initPos) == RS_SUCC){result = true;std::cout<<"movej succ"<<std::endl;ui->textBrowser->append(QString::fromLocal8Bit("机器人走路点成功"));}else{std::cerr<<"movej failed!"<<std::endl;ui->textBrowser->append(QString::fromLocal8Bit("机器人走路点失败"));}return result;
}

4.获取X,Y,Z和四元素坐标

1.获取xyz和四元素

2.四元素→欧拉角（得到xyz和欧拉角（法兰盘坐标））

3.法兰盘坐标→工具末端坐标

4.保存坐标

//获取xyz坐标和四元素
void MainWindow::on_pB_GetCurrentWayPoint_clicked()
{int ret=GetCurrentWayPoint();if(ret==-1){qDebug()<<"read coord faild";}}int MainWindow::GetCurrentWayPoint()
{HTuple FlangerCenterPoint;  //法兰盘坐标HTuple ToolInBasePose;      //工具末端坐标wayPoint_S wayPoint;  //AOBO  自身定位 数据类型//1.获取点--此刻  4元素if (RS_SUCC==rs_get_current_waypoint(g_rshd, &wayPoint)){//为了 后面 图像处理的  计算  ，--xyz  +  欧拉角//2.路点  换算到 pose （xyz  rpy）--法兰盘坐标GetPoseFromWayPoint(wayPoint,FlangerCenterPoint);//3.法兰盘坐标  --换算成为工具坐标  --对照下看看ChangeFlangerToToolPose(FlangerCenterPoint,ToolInBasePose);//4.保存ToolInBasePoseWriteHalconPose(ToolInBasePose);//保存了 工具末端坐标  为了后期 手眼标定的使用return 0;}else{return -1;}}//从路点 换算成 halcon pose 姿态
void  MainWindow::GetPoseFromWayPoint(wayPoint_S wayPoint,  HTuple  &Point1)
{// 1. 得到 xyz 和 四元素，// 2. 四元素 -欧拉角-// 3.-弧度RAD   --角度Deg// 4.--writeposeqDebug()<<"x:"<<wayPoint.cartPos.position.x;  //以米的单位double  x=wayPoint.cartPos.position.x*1000;  //  毫米//转换4元数 到 欧拉角Rpy rpy;HTuple  Rx,Ry,Rz;int ret=rs_quaternion_to_rpy(g_rshd, &wayPoint.orientation , &rpy);//转换下  弧度制 转 角度制TupleDeg(rpy.rx, &Rx);TupleDeg(rpy.ry, &Ry);TupleDeg(rpy.rz, &Rz);//保存 posePoint1.Clear();Point1[0] = wayPoint.cartPos.position.x;Point1[1] = wayPoint.cartPos.position.y;Point1[2] = wayPoint.cartPos.position.z;Point1[3] = Rx;Point1[4] = Ry;Point1[5] = Rz;Point1[6] = 2; //旋转轴 ： --- 机器人  2   ， halcon  0qDebug()<<QString::fromLocal8Bit("欧拉角 -第1个值:")<<Point1[3].D();}void MainWindow::ChangeFlangerToToolPose(HTuple FlangerCenterPoint,HTuple &ToolInBasePose)
{HTuple TcpPose;          //标定的结果 tool center point ，相对于 法兰盘 的工具中心HTuple I;                //数组下标//看一下  法兰盘 和 工具坐标//偏离值TcpPose.Clear();TcpPose[0] = -0.002479;TcpPose[1] = 0.003514;TcpPose[2] = 0.211092;TcpPose[3] = 0;TcpPose[4] = 0;TcpPose[5] = 0;TcpPose[6] = 2;//法兰盘换算到工具末端//相乘PoseCompose(FlangerCenterPoint, TcpPose, &ToolInBasePose);for (I=3; I<=5; I+=1){if (0 != (int(HTuple(ToolInBasePose[I])>180))){ToolInBasePose[I] = HTuple(ToolInBasePose[I])-360;}}
}void  MainWindow::WriteHalconPose(HTuple Point1)
{//./Image  判断QString str=  "./Image";QFile File00(str);if(!File00.exists()){qDebug()<<"file not exist";return ;}int Num=ui->PointNum->value();QString FileName="./Image/Pose"+QString::number(Num)+".dat";HTuple  FileName1=HTuple(FileName.toLatin1().data());  //QString类型  转换到HTuple 方法//乘以tcp标定结果 就得到工具末端坐标WritePose(Point1, FileName1);ui->textBrowser->append(QString::fromLocal8Bit("坐标保存在:")+FileName);ui->textBrowser->append("x:"+QString::number(Point1[0].D()));ui->textBrowser->append("y:"+QString::number(Point1[1].D()));ui->textBrowser->append("z:"+QString::number(Point1[2].D()));ui->textBrowser->append("Rx:"+QString::number(Point1[3].D()));ui->textBrowser->append("Ry:"+QString::number(Point1[4].D()));ui->textBrowser->append("Rz:"+QString::number(Point1[5].D()));}

5.走xyz rx,ry,rz

1.读取坐标

2.工具坐标→法兰盘坐标

3.欧拉角→四元素

4.逆解关节角坐标

//走  基于base坐标系的 法兰盘坐标
void MainWindow::on_pB_MoveFlangerCenterPose_clicked()
{bool result = false;HTuple ToolInBasePose;  //工具末端坐标HTuple FlangerCenterPose;  //法兰盘坐标RobotRecongnitionParam param;rs_get_robot_recognition_param(g_rshd, 1, &param);//该位置为机械臂的初始位置（提供6个关节角的关节信息（单位：弧度））//1.读取坐标ReadLocalToolPose(ToolInBasePose);//2.工具转法兰盘ChangeToolPoseToFlangerPose(ToolInBasePose,FlangerCenterPose);//3.欧拉角换算4元数 与pos//XYZ  RPY  ---欧拉角--Point13    ---->逆解成 关节角aubo_robot_namespace::wayPoint_S wayPoint2;Pos pos;GetOrientationPoseFromHalconPose(FlangerCenterPose,pos,wayPoint2);// 4.逆解与走点RunInverseKinPoint(pos,wayPoint2);}//加载本地坐标
void MainWindow::ReadLocalToolPose(HTuple &Point13)
{//加载点int Num=ui->PointNum->value();QString FileName="./Image/Pose"+QString::number(Num)+".dat";HTuple  FileName1=HTuple(FileName.toLatin1().data());  //QString类型  转换到HTuple 方法try{ReadPose(FileName1, &Point13);//工具转法兰盘}catch(...){ui->textBrowser->append(QString::fromLocal8Bit("读取路点成功"));return ;}
}void  MainWindow::ChangeToolPoseToFlangerPose(HTuple ToolInBasePose,HTuple &FlangerCenterPoseX)
{//工具末端换算法兰盘HTuple  I, TcpPose,PoseInvert1;//偏离值TcpPose.Clear();TcpPose[0] = -0.002479;TcpPose[1] = 0.003514;TcpPose[2] = 0.211092;TcpPose[3] = 0;TcpPose[4] = 0;TcpPose[5] = 0;TcpPose[6] = 2;//逆变换PoseInvert(TcpPose, &PoseInvert1);PoseCompose(ToolInBasePose, PoseInvert1, &FlangerCenterPoseX);for (I=3; I<=5; I+=1){if (0 != (int(HTuple(FlangerCenterPoseX[I])>180))){FlangerCenterPoseX[I] = HTuple(FlangerCenterPoseX[I])-360;}}
}//欧拉角换算成为4元数
void MainWindow::GetOrientationPoseFromHalconPose(HTuple Point13,Pos &pos,wayPoint_S &wayPoint2)
{//目标位置pos = {Point13[0].D(), Point13[1].D(),Point13[2].D()};Rpy rpy;  //已有 欧拉角 ，目标 得到4元数 --wayPoint.orientationHTuple  Rx,Ry,Rz;  //弧度制TupleRad(Point13[3].D(), &Rx); //角度 换算成为 弧度TupleRad(Point13[4].D(), &Ry);TupleRad(Point13[5].D(), &Rz);rpy.rx=Rx[0].D();  //HTUPLE  换算成为 结构体 double类型rpy.ry=Ry[0].D();rpy.rz=Rz[0].D();//欧拉角换算成为4元数int ret=rs_rpy_to_quaternion(g_rshd, &rpy, &wayPoint2.orientation);
}//逆解位置信息
void MainWindow::RunInverseKinPoint(Pos pos,wayPoint_S wayPoint2)
{//获取当前路点信息aubo_robot_namespace::wayPoint_S wayPoint1;//逆解位置信息aubo_robot_namespace::wayPoint_S targetPoint;//目标位置对应的关节角double targetRadian[ARM_DOF] = {0};if (RS_SUCC == rs_get_current_waypoint(g_rshd, &wayPoint1)){//参考当前姿态逆解得到六个关节角if (RS_SUCC == rs_inverse_kin(g_rshd, wayPoint1.jointpos, &pos, &wayPoint2.orientation, &targetPoint)){//将得到目标位置,将6关节角度设置为用户给定的角度（必须在+-175度）targetRadian[0] = targetPoint.jointpos[0];targetRadian[1] = targetPoint.jointpos[1];targetRadian[2] = targetPoint.jointpos[2];targetRadian[3] = targetPoint.jointpos[3];targetRadian[4] = targetPoint.jointpos[4];targetRadian[5] = targetPoint.jointpos[5];HTuple Pose,Deg1;Pose[0]=targetRadian[0];TupleDeg( Pose[0],&Deg1);ui->textBrowser->append(" joint Deg"+QString::number(Deg1[0].D()));for(int i=0; i<6 ;i++){qDebug()<<" i"<<i  <<" targetRadian"<<targetRadian[i];}//轴动到目标位置if (RS_SUCC == rs_move_line(g_rshd, targetRadian)){std::cout<<"at target"<<std::endl;}else{std::cerr<<"move joint error"<<std::endl;}}else{std::cerr<<"ik failed"<<std::endl;}}else{std::cerr<<"get current waypoint error"<<std::endl;}
}

6.直线速度

void MainWindow::on_RobSpeed_valueChanged(double arg1)
{/** 模拟业务 **//** 接口调用: 初始化运动属性 ***/qDebug()<<"arg1:="<<arg1;rs_init_global_move_profile(g_rshd);/** 接口调用: 设置关节型运动的最大加速度 ***/aubo_robot_namespace::JointVelcAccParam jointMaxAcc;jointMaxAcc.jointPara[0] = 50.0/180.0*M_PI;jointMaxAcc.jointPara[1] = 50.0/180.0*M_PI;jointMaxAcc.jointPara[2] = 50.0/180.0*M_PI;jointMaxAcc.jointPara[3] = 50.0/180.0*M_PI;jointMaxAcc.jointPara[4] = 50.0/180.0*M_PI;jointMaxAcc.jointPara[5] = 50.0/180.0*M_PI;   //接口要求单位是弧度rs_set_global_joint_maxacc(g_rshd, &jointMaxAcc);/** 接口调用: 设置关节型运动的最大速度 ***/aubo_robot_namespace::JointVelcAccParam jointMaxVelc;jointMaxVelc.jointPara[0] = 50.0/180.0*M_PI;jointMaxVelc.jointPara[1] = 50.0/180.0*M_PI;jointMaxVelc.jointPara[2] = 50.0/180.0*M_PI;jointMaxVelc.jointPara[3] = 50.0/180.0*M_PI;jointMaxVelc.jointPara[4] = 50.0/180.0*M_PI;jointMaxVelc.jointPara[5] = 50.0/180.0*M_PI;   //接口要求单位是弧度rs_set_global_joint_maxvelc(g_rshd, &jointMaxVelc);/** 接口调用: 初始化运动属性 ***/rs_init_global_move_profile(g_rshd);qDebug()<<"arg2:="<<arg1;/** 接口调用: 设置末端型运动的最大加速度 　　直线运动属于末端型运动***/double endMoveMaxAcc;endMoveMaxAcc = 0.2;   //单位米每秒rs_set_global_end_max_line_acc(g_rshd, endMoveMaxAcc);rs_set_global_end_max_angle_acc(g_rshd, endMoveMaxAcc);/** 接口调用: 设置末端型运动的最大速度 直线运动属于末端型运动***/double endMoveMaxVelc;endMoveMaxVelc =ui->RobSpeed->value();   //单位米每秒qDebug()<<"arg3:="<<arg1;ui->textBrowser->append(QString::fromLocal8Bit("当前速度是")+QString::number(endMoveMaxVelc)+"m/s");rs_set_global_end_max_line_velc(g_rshd, endMoveMaxVelc);rs_set_global_end_max_angle_velc(g_rshd, endMoveMaxVelc);}