硬件
- KUKA KR 16 R1610机器人
- KUKA KR_C4控制柜
- Windows电脑
- 网线
注意!扭控制柜开关的时候要果断,否则可能启动瞬间电流过大导致跳闸
机器人端程序
使用了ROS的RSI程序,链接如下
控制器网络配置
KUKA机器人的示教器是基于Windows的,因此需要确保示教器上的Windows系统和我们的Windows电脑运行在同一个局域网中
示教器是中文界面,需要自己翻译一下
- 在示教器上以“专家”或“管理员”登录
- 进到网络设置界面:Start-up > Network configuration > Activate advanced configuration,并进行下列设置
- IP:192.168.250.20
- Subnet mask:255.255.255.0
- Default gateway:192.168.250.20
- Windows interface checkbox勾选
- 最小化SmartHMI(示教器程序):Start-up > Service > Minimize HMI
- 在示教器的Windows界面启动RSI-Network:开始菜单 > 所用程序 > RSI-Network
- 确认Network-Kuka User Interface界面上有之前设置的IP地址
- 添加新IP地址(如192.168.1.20,网段要跟前面不一样)用于RSI通讯:
- 点击RSI Ethernet下面的New,并点击Edit
- 输入用于RSI通讯的IP地址,并点击OK
- 关闭RSI-Network,并最大化SmartHMI
- 冷机重启控制器:Shutdown > Check Force cold start and Reload files > Reboot control PC
- 重启后,最小化SmartHMI:Start-up > Service > Minimize HMI
- 打开命令行并ping你的Windows电脑:开始菜单 > 所用程序 > cmd.exe,并输入”ping 192.168.250.xxx”,能正常ping说明网络已设置好
- 在你的Windows电脑上新加一个IP地址(用于RSI通讯,192.168.1.xxx):右键开始 > 网络连接 > 更改适配器选项 > 右键正确的网卡 > 属性 > 双击Internet Protocal Version 4 (TCP/IPv4) > 使用下面的IP地址 > 高级 > 添加
配置文件
ros_rsi_ethernet.xml
- IP_NUMBER:电脑用于RSI通讯的IP地址,如192.168.1.xxx
- PORT:默认49152即可,这项需要与后面的电脑端程序对应
ros_rsi.rsi.xml
- AXISCORR:关节角绝对位置限位,相对于初始关节角(按需要)
- AXISCORRMON:关节角最大修正量(一般设得较大)
实际运行过程中,两项中的任意一项超了之后,RSI接口就会中断
- ETHERNET:默认为100
机器人端以250Hz的频率发送机器人状态,每条状态会带一个IPOC标签,每个周期加4。而电脑端发送控制指令的时候,会把上一个收到的IPOC发回去。如果机器人收到的IPOC跟目前的IPOC不同(电脑不能在4ms内处理完成),机器人的Timeout变量就会加1,当这个变量达到ETHERNET的值时,RSI接口就断了
理论上这个Timeout变量应该会在每次收到指令之后归0,但是没有实际测试过
测试过之后发现不会归0,只要总数达到100就会断开
ros_rsi.src
初始关节角可以在这个文件设置
默认为:[0, -90, 90, 0, 90, 0]
其他文件
不用管,后面直接拷进去示教器就行
放置文件
- 把这些文件都塞到U盘里
- 把U盘插到示教器或者控制柜上
- 以“专家”或者“管理员”登录
- 把ros_rsi.src放到KRC:\R1\Program
- 把剩下的文件放到C:\KRC\ROBOTER\Config\User\Common\SensorInterface
电脑端程序
本质上是udp服务端+读写xml
- void parse_rsi_state_xml(char* buffer, double* output_joint_angle, char* output_ipoc):输入buffer,输出关节角、IPOC
- void create_rsi_command_xml(double* input_joint_corr, char* input_ipoc, char* buffer) :输入关节角、IPOC,输出buffer
- int main():建立udp服务端,接收和发送buffer,并调用前两个函数
接收到的xml格式
- Rob
- RIst:当前位姿
- RSol:初始位姿
- AIPos:当前角度
- ASPos:初始角度
- Delay
- IPOC
<Rob Type="KUKA"><RIst X="815.0005" Y="-0.0003" Z="1296.9999" A="0.0000" B="-90.0002" C="0.0000"/><RSol X="815.0000" Y="0.0000" Z="1297.0000" A="0.0000" B="-90.0000" C="0.0000"/><AIPos A1="0.0000" A2="-90.0000" A3="90.0000" A4="0.0001" A5="89.9998" A6="0.0000"/><ASPos A1="0.0000" A2="-90.0000" A3="90.0000" A4="0.0000" A5="90.0000" A6="0.0000"/><Delay D="100"/><IPOC>2220485</IPOC></Rob>
发送的xml格式
- Sen
- AK:修正角度(实际角度=初始角度+修正角度)
- IPOC
<Sen Type="ImFree"><AK A0="0.000000" A1="0.000000" A2="0.000000" A3="0.000000" A4="0.000000" A5="0.000000"/><IPOC>2220485</IPOC>
</Sen>
测试代码
读写xml的库:RAPIDXML
udp通讯:参考链接
#include <iostream>
#include <WinSock2.h>
#pragma comment ( lib, "WS2_32.lib" )
#include <rapidxml_ext.h>
constexpr auto STR_LENGTH = 1024;
constexpr auto PORT = 49152;
void parse_rsi_state_xml(char* buffer, double* output_joint_angle, char* output_ipoc)
{
rapidxml::xml_document<>* doc = new rapidxml::xml_document<>();
doc->parse<0>(buffer);
rapidxml::xml_node<>* node_aipos = doc->first_node("Rob")->first_node("AIPos");
int index = 0;
for (rapidxml::xml_attribute<>* attr = node_aipos->first_attribute(); attr; attr = attr->next_attribute())
{
output_joint_angle[index] = strtod(attr->value(), NULL);
std::cout << output_joint_angle[index] << "\t";
index++;
}
rapidxml::xml_node<>* node_ipoc = doc->first_node("Rob")->first_node("IPOC");
strcpy_s(output_ipoc, STR_LENGTH, node_ipoc->value());
std::cout << output_ipoc << std::endl;
delete doc;
}
void create_rsi_command_xml(double* input_joint_corr, char* input_ipoc, char* buffer)
{
rapidxml::xml_document<>* doc = new rapidxml::xml_document<>();
rapidxml::xml_attribute<>* attr = new rapidxml::xml_attribute<>();
char str_name[STR_LENGTH] = { 0 };
char str_value[STR_LENGTH] = { 0 };
rapidxml::xml_node<>* node_sen = doc->allocate_node(rapidxml::node_element, "Sen");
doc->append_node(node_sen);
attr = doc->allocate_attribute("Type", "ImFree");
node_sen->append_attribute(attr);
rapidxml::xml_node<>* node_ak = doc->allocate_node(rapidxml::node_element, "AK");
for (int i = 0; i < 6; i++)
{
sprintf_s(str_name, "A%d", i+1);
sprintf_s(str_value, "%f", input_joint_corr[i]);
attr = doc->allocate_attribute(doc->allocate_string(str_name), doc->allocate_string(str_value));
node_ak->append_attribute(attr);
}
node_sen->append_node(node_ak);
rapidxml::xml_node<>* node_ipoc = doc->allocate_node(rapidxml::node_element, "IPOC", input_ipoc);
node_sen->append_node(node_ipoc);
char* end = rapidxml::print(buffer, *doc, 0);
*end = 0;
}
int main()
{
WSAData my_wsadata;
SOCKET my_server_socket;
SOCKADDR_IN my_server_addr;
SOCKADDR_IN my_client_addr;
memset(&my_client_addr, 0, sizeof(my_client_addr));
int my_client_addr_length = sizeof(my_client_addr);
char buffer[STR_LENGTH] = { 0 };
int buffer_length = STR_LENGTH;
int ret = -1;
double input_joint_corr[6] = { 0 };
double output_joint_angle[6] = { 0 };
char input_ipoc[STR_LENGTH] = { 0 };
char output_ipoc[STR_LENGTH] = { 0 };
int count = 0;
ret = WSAStartup(MAKEWORD(2, 2), &my_wsadata);
if (0 != ret)
{
std::cout << "WSAStartup error with error num: " << WSAGetLastError();
return 0;
}
my_server_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (SOCKET_ERROR == my_server_socket)
{
std::cout << "Create Socket Error!" << std::endl;
goto exit;
}
my_server_addr.sin_family = AF_INET;
my_server_addr.sin_port = htons(PORT);
my_server_addr.sin_addr.S_un.S_addr = htonl(INADDR_ANY);
ret = bind(my_server_socket, (SOCKADDR*)&my_server_addr, sizeof(my_server_addr));
if (0 != ret)
{
std::cout << "bind error with error num: " << WSAGetLastError();
goto exit;
}
while (true)
{
memset(buffer, 0, sizeof(buffer));
ret = recvfrom(my_server_socket, buffer, buffer_length, 0, (SOCKADDR*)&my_client_addr, &my_client_addr_length);
if (ret > 0)
{
parse_rsi_state_xml(buffer, output_joint_angle, output_ipoc);
}
strcpy_s(input_ipoc, output_ipoc);
if (count % 2000 < 1000)
{
input_joint_corr[5] += .05;
}
else
{
input_joint_corr[5] -= .05;
}
memset(buffer, 0, sizeof(buffer));
create_rsi_command_xml(input_joint_corr, input_ipoc, buffer);
ret = sendto(my_server_socket, buffer, sizeof(buffer), 0, (SOCKADDR*)&my_client_addr, sizeof(my_client_addr));
if (ret == 0)
{
std::cout << "failed to send" << std::endl;
}
count++;
}
exit:
closesocket(my_server_socket);
return 0;
}
一些细节
rapidxml相关c6262警告
如测试代码,创建doc时要用指针来创建
rapidxml函数为未定义标识符
将
#include "rapidxml.hpp"
#include "rapidxml_print.hpp"
换成
#include "rapidxml_ext.h"
新建一个rapidxml_ext.h,内容是
#ifndef RAPIDXML_EXT_H_
#define RAPIDXML_EXT_H_
#include "rapidxml.hpp"
/* Adding declarations to make it compatible with gcc 4.7 and greater */
namespace rapidxml {
namespace internal {
template <class OutIt, class Ch>
inline OutIt print_children(OutIt out, const xml_node<Ch>* node, int flags, int indent);
template <class OutIt, class Ch>
inline OutIt print_attributes(OutIt out, const xml_node<Ch>* node, int flags);
template <class OutIt, class Ch>
inline OutIt print_data_node(OutIt out, const xml_node<Ch>* node, int flags, int indent);
template <class OutIt, class Ch>
inline OutIt print_cdata_node(OutIt out, const xml_node<Ch>* node, int flags, int indent);
template <class OutIt, class Ch>
inline OutIt print_element_node(OutIt out, const xml_node<Ch>* node, int flags, int indent);
template <class OutIt, class Ch>
inline OutIt print_declaration_node(OutIt out, const xml_node<Ch>* node, int flags, int indent);
template <class OutIt, class Ch>
inline OutIt print_comment_node(OutIt out, const xml_node<Ch>* node, int flags, int indent);
template <class OutIt, class Ch>
inline OutIt print_doctype_node(OutIt out, const xml_node<Ch>* node, int flags, int indent);
template <class OutIt, class Ch>
inline OutIt print_pi_node(OutIt out, const xml_node<Ch>* node, int flags, int indent);
}
}
#include "rapidxml_print.hpp"
#endif /* RAPIDXML_EXT_H_ */
rapidxml使用字符串赋值
doc->allocate_string(str)
strcpy_s给char*赋值不同于给char数组赋值
给char*赋值还需要指定字符串长度,给char数组赋值则不需要
机器人报警(速度或力矩超限)
二选一
- 在示教器上调高手动挡限速(比较好使)
- 调低程序倍率
经常性timeout
更改rsi运行模式:更改ros_rsi.src文档中的ret = RSI_ON(#ABSOLUTE),改为ret = RSI_ON(#ABSOLUTE, #IPO),这样RSI将会运行在IPO模式(12ms)下而不是默认的IPO Fast模式(4ms)。这两种模式在关节控制上没有区别,但是在位姿控制有区别,详见官方文档
增大timeout值:初步尝试了一下这个方法,但是好像并不能改善连接的稳定性,只是会在连接断开后使RSI继续运行一段时间
提高程序的处理优先级:线程优先级,进程优先级,貌似有一定作用,但不明显
启用编译器优化:把程序的配置属性改成Release,这样默认会开启优化,注意包含路径、库路径等需要重新设置
!!!问题解决!!!原因出在使用了网口转USB口的接头上,这个接头速率不太行,把网线直接接在电脑上就好了,跑4ms的模式也很稳定
the braking ramp of the robot has been violated & Maximum axis-specific velocity in T1 mode exceeded
问题出在程序上,由于RSI的控制频率很高,程序必须确保输出的连续性,否则机器人就会出现急停急动,就会报警并停止
一个解决方法是在输出端加上一个均值滤波器,来确保输出的变化幅度较小,但是在恒速度输入的情况下会引入一定的静差
另一个解决方法是限制控制器的输出。对于位置PID,限制输出的变化;对于速度PID,限制输出的大小
关节速度限制
测试运行
- 把电脑端程序先跑起来
- 把示教器扭到T1档(手动慢挡)
- 打开目录KRC:\R1\Program,选择程序ros_rsi.src
- 恰到好处地按使能键,按绿色开始键,机器人会跑到初始位姿(到之后会弹出Programmed path reached (BCO),确认即可)
- 再次恰到好处地按使能键,按绿色开始键(此时会弹出Attention – Sensor correction goes active,确认即可)
- 再次恰到好处地按使能键,按绿色开始键(RSI接口运行中)
莽夫操作:打到自动挡可以减少“恰到好处地按使能键,按绿色开始键”的次数
Pingback: 机器人位姿反馈控制开发日志 - Fivyex's Blog