KUKA RSI接口开发

硬件

  • KUKA KR 16 R1610机器人
  • KUKA KR_C4控制柜
  • Windows电脑
  • 网线

注意!扭控制柜开关的时候要果断,否则可能启动瞬间电流过大导致跳闸

机器人端程序

使用了ROS的RSI程序,链接如下

https://github.com/ros-industrial/kuka_experimental/tree/melodic-devel/kuka_rsi_hw_interface/krl/KR_C4

控制器网络配置

KUKA机器人的示教器是基于Windows的,因此需要确保示教器上的Windows系统和我们的Windows电脑运行在同一个局域网中

示教器是中文界面,需要自己翻译一下

  1. 在示教器上以“专家”或“管理员”登录
  2. 进到网络设置界面: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勾选
  3. 最小化SmartHMI(示教器程序):Start-up > Service > Minimize HMI
  4. 在示教器的Windows界面启动RSI-Network:开始菜单 > 所用程序 > RSI-Network
  5. 确认Network-Kuka User Interface界面上有之前设置的IP地址
  6. 添加新IP地址(如192.168.1.20,网段要跟前面不一样)用于RSI通讯:
    • 点击RSI Ethernet下面的New,并点击Edit
    • 输入用于RSI通讯的IP地址,并点击OK
    • 关闭RSI-Network,并最大化SmartHMI
  7. 冷机重启控制器:Shutdown > Check Force cold start and Reload files > Reboot control PC
  8. 重启后,最小化SmartHMI:Start-up > Service > Minimize HMI
  9. 打开命令行并ping你的Windows电脑:开始菜单 > 所用程序 > cmd.exe,并输入”ping 192.168.250.xxx”,能正常ping说明网络已设置好
  10. 在你的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接口运行中)

莽夫操作:打到自动挡可以减少“恰到好处地按使能键,按绿色开始键”的次数

1人评论了“KUKA RSI接口开发”

  1. Pingback: 机器人位姿反馈控制开发日志 - Fivyex's Blog

发表评论

您的电子邮箱地址不会被公开。

此站点使用Akismet来减少垃圾评论。了解我们如何处理您的评论数据