KUKA RSI接口开发

一条评论 / KUKA / 作者： / 4月 25, 2022

硬件

  • 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，限制输出的大小

关节速度限制

库卡（KUKA）关节速度限制

测试运行

  • 把电脑端程序先跑起来
  • 把示教器扭到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来减少垃圾评论。了解我们如何处理您的评论数据。