最近在使用c++对UR3和UR5进行控制,发现网上内容不太好找,并且有的编程的一些点没有点清楚,现在记录在这里,方便后人使用.
copyright (c) 2020 余晨 in HITSZ, 协议CPLv.2 转载请标明原地址.
机械臂TCP/ip通信
-
通信概述
机械臂的网口通信有几个端口可以使用,我只讲解30003端口,我用的UR3和UR5是e系列.关于机械臂tcp/ip通信的参考在这里,下图就来自这个网页里,并且提醒:这个参考网页最下方有一个xlsx格式的表格附件,里面详细描述了UR机械臂传输数据的详细格式,很多人没留意到这个附件,为了方便使用,这里也把这个附件链接放上来.
首先看图1,可以看到30003通信端口频率为500HZ,并且可以使用URScript,所以速度较快,控制较全面,我们就是用这个方法.但是现在官方推荐使用RTDE(real time data exchange),是封装好的API,你们可以用这个看看文档,以及引导教程.
图二就是机械臂30003端口通信数据包详细细节,这个图就来自参考网页的附件.可以看到数据包的第一个数据是一个int,描述了整个数据包的长度.这里提醒:不同机械臂的数据包长度可能是不一样的,要通过读取这个第一个int来判断真实的长度.并且由于是通过网络进行传输,所以遵循网络标准,即:网络字节顺序都是大端字节顺序(big-endian),如果你使用的是Intel等little-endian的芯片,那么在读取到数据包之后要进行一个直接顺序转换.同时从机械臂读到的数据的默认格式是图2所示,但是你可以在机械臂示教器更改数据传输,如果你更改了,那么可能就不是这个默认格式了!
-
通信编程要点
在说完基础信息后,这里将说明一些编程实现要注意的地方.
-
基础通信
可以使用QtcpSocket(如果你使用Qt)或者任意的网络通信模块都可以,将目标地址和端口设置成你机械臂示教器里设置的ip地址,端口用30003.如果想要看具体收数据实现可看这篇文章,以及他的代码实现.这里也给出我的数据接受代码参考.
//这个一个大小端转换函数,大小端转换的基本单位是bytes,当一个64bit数据要做大小端转换时只需要将这8个bytes镜像对调
#define ntohll(x) ( ((uint64_t)x & 0xff00000000000000LL)>>56 | \
((uint64_t)x & 0x00ff000000000000LL)>>40 | \
((uint64_t)x & 0x0000ff0000000000LL)>>24 | \
((uint64_t)x & 0x000000ff00000000LL)>>8 | \
((uint64_t)x & 0x00000000ff000000LL)<<8 | \
((uint64_t)x & 0x0000000000ff0000LL)<<24 | \
((uint64_t)x & 0x000000000000ff00LL)<<40 | \
((uint64_t)x & 0x00000000000000ffLL)<<56 )
/**
* @brief 从机械臂读取数据,每一次机械臂有新的数据过来,即QTcpSocket::readyRead状态的槽函数就是这个.
* @author 余晨
* @date 20200921
*/
void robotCtl::readData() {
QByteArray byteArray;//临时变量,用来存储每次从机械臂读进来的数据
byteArray.resize(this->tcpSocket->bytesAvailable());//先设置每次读取的大小为可接收缓存的大小
int byteRead = this->tcpSocket->read(byteArray.data(), byteArray.size());//把缓存区所有数据读取回来,可以用readAll替代,但是现在用的函数可以判断读取结果
if (byteRead == -1) {//读取错误
qDebug() << "read data from robot error!";
return;
} else if (byteRead == 0) {//没读到东西
qDebug() << "read 0 bytes data from robot";
return;
} else if (byteRead % this->URtypes == 0) {//读取回来的数据长度等于对应机械臂每次发送的数据长度,即读取正确;UR3和UR5的数据包长度不一样,这么作比较方便.这里URtypes就是对应机械臂传输的数据包长度!
int offset = 252;//数据包的252个位后才是目标数据
double buff;
for (int i = 0; i < 6; i++, offset += 8) {//读取6个关节角,是弧度
memcpy(&buff, byteArray.data() + offset, 8);//将读目标数据内存拷贝到buff中,拷贝8个字节,是因为机械臂每个数据都是一个double为8个字节
buff = this->swapDoubleEndian(&buff);//把目标数据进行字节顺序转换
this->robotData.joint_position[i] = buff;
}
for (int i = 0; i < 6; i++, offset += 8) {//读取6个关节速度
memcpy(&buff, byteArray.data() + offset, 8);
buff = this->swapDoubleEndian(&buff);
this->robotData.joint_velocity[i] = buff;
}
offset += 96;//有12个double数据不想要,因此跳过, 12 * 8 = 96
for (int i = 0; i < 6; i++, offset += 8) {//读取末端位置,x,y,z,Rx,Ry,Rz
memcpy(&buff, byteArray.data() + offset, 8);
buff = this->swapDoubleEndian(&buff);
this->robotData.coordinate[i] = buff;
}
for (int i = 0; i < 6; i++, offset += 8) {//读取末端速度,x,y,z,Rx,Ry,Rz
memcpy(&buff, byteArray.data() + offset, 8);
buff = this->swapDoubleEndian(&buff);
this->robotData.coordinate_speed[i] = buff;
}
//所有想要的数据读完了就显示在GUI界面
emit this->dispRobotDataSignal(this->robotData);
}
}
/**
* @brief 将bigEnd改成littleEnd,因为网络字节顺序是大端模式,而intel芯片存储格式是小端模式,因此需要做一个转换
* @author 余晨
* @date 20200921
*/
double robotCtl::swapDoubleEndian(double *val) {
uint64_t llVal = ntohll(*((uint64_t*)val));//这个一个宏函数,在此文件头部,这里转换成指针原因见下两行
return *((double*)&llVal);//uint64是无符号数,和double储存格式不一样,不能进行强制转换直接返回double类型,会错误; \
所以这里将uint64的地址取出来,再把这个uint64类型的地址指针转换成double类型指针,再返回double指针的内容,这种基础数据类型要谨慎隐式转换
}
-
64位数据字节顺序转换
由于我们要读取的数据是角度等,在机械臂发送的数据包里是一个double,长度是8 bytes,这里提供三个思路给你们参考:
- 可以得到目标数据包的地址指针,然后强制转换成char*指针(因为char长度就是1byte),然后将这个数据的8位数据进行镜像对换.最后再转换回64位数据应该就可以了(这个方法我没测试过)
//假设data就是我们要的double类型数据
char * ptr = (char*) &data;
for (int i = 0; i < 4; i++) {
swap(ptr + i, ptr + 7 - i);//交换两个地址的数据
}
使用位操作进行转换,可以参考这篇文章
这里定义的ntohll()函数输入是long long int.这里要注意一点,在double类型转换成uint64_t(或 long long int)时不能使用强制类型转换,因为此时字节顺序都不对了,强制转换操作得到的结果是错误的,所以要用上面一个方法类似的操作,先得到数据地址,然后改变指针类型,再取改地址指向的数据来进行格式转换.而不能直接把double传参进去进行隐式强制转换使用linux的be64toh()函数(定义在endian.h),32位的int字节转换可以使用此文件定义的be32toh()或者in.h文件定义的ntohl().当然这里要传参之前也要记得类型转换不能隐式转换.
-
UR script (此部分网上很少有教程!)
UR scrip编程语言下载网页
在这里选择你的机械臂型号,下载机械臂使用的函数参考.按照如图3选择就可以下载e系列的编程语言参考了!
这里只给大家提出最重要的几个点:
- 每一个命令结束后必须使用\n换行符,否则该指令不执行!
//sendCommand是我自己定义的发送数据指令,每句结束后必须带有\n转义符
sendCommand("powerdown()\n");
- 如果要使用一段指令,那么必须使用def f(): end来定义一个函数,否则机械臂将执行你写的每一句指令!正确做法如下,以下代码段实现了判断输入的位姿,如果可以到达就运动到该位姿,否则弹出一个窗口在示教器并停止运行:
//.arg()是QString的格式化输入,用于填充字符串内的%0-%6数据.\t是制表符,个人强迫缩进习惯,
QString command = tr("def f():\n\t" \
"solution = is_within_safety_limits(p[%1,%2,%3,%4,%5,%6])\n\t" \
"if (solution == True):\n\t\t" \
"\tmovel(p[%1,%2,%3,%4,%5,%6])\n\t" \
"else:\n\t\t" \
"\tpopup(\"can't reach pose!\", title=\"Popup#1\",blocking=True)\n\t" \
"end\n" \
"end\n"
).arg(ui->lineEdit_setPoseX->text()).arg(ui->lineEdit_setPoseY->text()).arg(ui->lineEdit_setPoseZ->text()) \
.arg(ui->lineEdit_setPoseRX->text()).arg(ui->lineEdit_setPoseRY->text()).arg(ui->lineEdit_setPoseRZ->text());
emit this->sendCommand(command.toLatin1());
- 发送的一段指令,机械臂在执行完后就会退出指令执行,所以如果要实现freedrive,自由驱动,这样的功能,那么你得将进程卡住,不然使用了freedrive_mode()一进去就执行完会立马退出freedrive_mode,这样你进入自由驱动模式只有几个毫秒,根本无法实现,你可以查看示教器上的机械臂执行日志来确定问题.经过查找,我发现如下解决方法:
在按钮按下时,进入自由驱动模式,并且锁死进程不退出,然后在按钮松开时发送退出自由驱动模式指令:
/**
* @brief 机械臂进入自由驱动模式
* @author 余晨
* @date 20200928
* @note 执行一个指令的时间非常短,机械臂在执行指令后就会退出指令,所以必须进入一个循环里,保持自由驱动模式.
*/
void gui_robotCtl::on_pushButton_freeDrive_pressed()
{
emit this->sendCommand("def free():\n\t" \
"freedrive_mode()\n" \
"while (True) :\n\t\t" \
"sync()\n\t" \
"end\n"\
"end\n");
}
/**
* @brief 机械臂退出自由驱动模式
* @author 余晨
* @date 20200928
*/
void gui_robotCtl::on_pushButton_freeDrive_released()
{
emit this->sendCommand("end_freedrive_mode()\n");
}
