EC机器人第三方力控使用
1.简介
在当前开发的力控模式中，机器人末端装有六维力传感器，用于检测外界环境对机器人施加的力度，配合适当的控制策略，可以达到调整机器人期望的运行轨迹和外界接触力的目的。该模式目前主要应用于曲面打磨、轴孔装配等工艺。 使用该模式需要首先完成以下三个步骤：
(a) 传感器的安装与配置
(b) 传感器标定
(c) 负载辨识
在进行上述操作前，请务必按照传感器的要求进行预热，具体步骤如下文所示。
2.力传感器的安装与配置
机器人版本建议3.9.2以上，机械臂末端工具IO版本需要更新至v8.2.727及以上的版本。
末端工具IO版本查看方法：
依次查看0002,0003,0004分别有数字，如0002:8,0003:2,0004:1209则此末端工具 IO版本为 V8.2.1209
2.1安装
对传感器的安装无特定要求（尽量与工具坐标系保持一致），但在传感器安装完成后，用户需要在传感器的 参数设置界面进行一些设置，具体的内容如下所示。
2.2接线
力传感器的线可直接接在末端IO上，下图是末端IO的对应的针脚。力传感器通常需要接线24V,0V,RS485+,RS485-。
2.3参数配置
2.3.1 工艺配置
运行准备-工艺配置-勾选末端IO-设置，重启后生效（注意：不要勾选末端力矩传感器）。
2.3.2 力控模式配置
扩展-力控模式配置-选择LUA数据源-设置数据间隔和容差-点击设置 。
注：用户可根据需求设置“数据间隔”和“容差”，数据间隔表示数据输入周期，单位为 ms，建议将数据间隔与容差的乘积设置为1000，请注意当输入力/力矩数据时，若其间隔时间 超过容差乘以数据间隔时，系统将会报错。
2.3.3 传感器参数设置
扩展-力控模式配置-传感器参数-输入参数后-点击设置
注：1.所选单位必须与传感器的单位保持一致
2. 若有转接板，则此处传感器的厚度包含转接板的厚度，质量也包含转接板的质量
3. 安装方式：传感器坐标系和法兰坐标系只相差一个Z向旋转角度称为正装
4. 查找传感器手册，确定传感器Z轴方向，和X轴Y轴方向，保证传感器方向和法兰坐标系一致
2.3.4 力控参数设置
扩展-力控模式配置-力控参数-输入参数-点击设置。
注：该功能用于调节力跟踪的灵敏度。需要注意的是，力跟踪数值设置的越大，装配的负 载就越大，机器人会出现明显的抖动。
以上步骤完成后，可以点击监视-力控传感器数据，若以上步骤都正确，则传感 器数据在不断变化，可施加不同方向的力，看力的方向是否和法兰坐标系一致， 后续的步骤可参考用户手册_3.10.2力控功能章节进行传感器标定，负载辨识及负载惯量辨识。设置完成后即可正常使用。
3.传感器标定
参考用户手册6.5.9.2传感器标定章节
5.负载辨识
参考用户手册6.5.9.3负载辨识章节
6.负载惯量辨识
参考用户手册6.5.9.4负载惯量辨识章节
7. 编程（通过lua读取传感器数据）
机器人与力传感器之间通过RS485通讯来读取传感器数据，按照指定格式解析，并写入控 制器中。可参考下方脚本，下方脚本是鑫金诚的传感器，如需适配其它品牌传感器，可按照这个脚本编写。
function str2hex(str)
--判断输入类型
if (type(str) ~= "string") then
return nil, "str2hex invalid input type"
end
--滤掉分隔符
str = str:gsub("[%s%p]", ""):upper()
--检查内容是否合法
if (str:find("[^0-9A-Fa-f]") ~= nil) then
return nil, "str2hex invalid input content"
end
--检查字符串长度
if (str:len() % 2 ~= 0) then
return nil, "str2hex invalid input lenth"
end
--拼接字符串
local index = 1
local ret = ""
for index = 1, str:len(), 2 do
ret = ret .. string.char(tonumber(str:sub(index, index + 1), 16))
end
return ret
end
--[[
-- extract_nice_frame: 从raw_data中返回符合要求的数据帧
-- return: 若无有效数据返回nil, 否则返回完整的一帧数据
--]]
function extract_nice_frame(raw_data)
local begin_idx, end_idx, valid_data
local len = string.len(raw_data)
local actual_frame_len = FRAME_LEN * 2 -- FRAME_LEN * 2 代表16进制数据转换成ASCII后的长度
if len < actual_frame_len then -- 先判断字符长度是否满足
--elite_print("recv wrong data len: "..len)
return nil
end
if (FRAME_END) then -- 一帧数据包含固定帧尾的情况
begin_idx, end_idx = string.find(raw_data, FRAME_BEGIN..'.*'..FRAME_END, 1, false) --string.find 会匹配满足要求的最长字串
if (not begin_idx or not end_idx) then
return nil
elseif (end_idx - begin_idx == actual_frame_len - 1) then -- 正好符合帧的格式，这是大多数情况
valid_data = string.sub(recv_buff, begin_idx, begin_idx + actual_frame_len -1)
return valid_data
elseif (end_idx - begin_idx > actual_frame_len - 1) then
local idx1,idx2
idx1 = begin_idx
repeat
-- 比较帧最后的字符是否为帧尾
local sub_star_idx = idx1 + actual_frame_len - string.len(FRAME_END)
local sub_end_idx = sub_star_idx + string.len(FRAME_END) - 1
local tmp_tail = string.sub(raw_data, sub_star_idx, sub_end_idx)
if (tmp_tail == FRAME_END) then
valid_data = string.sub(raw_data, idx1, idx1 + actual_frame_len -1)
return valid_data
end
idx1, idx2 = string.find(raw_data, FRAME_BEGIN, idx1 + string.len(FRAME_BEGIN))
until(not idx1)
return nil
end
else -- 没有固定帧尾的情况
begin_idx, end_idx = string.find(raw_data, FRAME_BEGIN)
if (begin_idx and end_idx and begin_idx <= len - actual_frame_len) then
local idx1,idx2 = string.find(raw_data, FRAME_BEGIN, end_idx + 1)
if (not idx1 or idx1 > len - actual_frame_len) then -- 没固定帧尾时，这两种条件下截取的一定是有效帧
valid_data = string.sub(recv_buff, begin_idx, begin_idx + actual_frame_len -1)
return valid_data
end
else
return nil
end
end
return nil
end
---------------------------End of Internal Function--------------------------
-------------------------- User Config First---------------------------------
-- 脚本运行前请先配置下面的基本参数
FRAME_LEN = 16 -- 传感器发送的一帧数据帧的长度，单位：字节
FRAME_BEGIN = "204E" -- 数据帧的帧头标识，此处以鑫精诚sensor为例，如果是坤为sensor填入"48AA"
FRAME_END = nil -- 数据帧的帧尾标识, 如果没有帧尾请填入nil,此处以鑫精诚sensor为例.如果是坤为sensor填入"0D0A"
FRAME_MAX_RECV_TM = 40 -- 串口每次接收等待的最大时间，unit: ms
SERIAL_speed = 115200 -- 串口波特率
SERIAL_bits = 8 -- 串口传输数据位
SERIAL_event = "N" -- 串口传输停止位
SERIAL_stop = 1 -- 串口传输停止位
TCIorController = 2 -- 1使用末端Tci, 2使用控制柜485接口
-------------------------- End of User Config First -------------------------
-- 用户变量
Control_script = "D0" --控制脚本运行
SCRIPT_MODE = "D1" --将脚本指令写入到传感器1:停止持续传输数据
FX = "D2" --传感器参数
FY = "D3" --传感器参数
FZ = "D4" --传感器参数
MX = "D5" --传感器参数
MY = "D6" --传感器参数
MZ = "D7" --传感器参数
ID = "D8" --传感器地址
---------------------------Script start------------------------------------
frame_idx = 0
torque = {0, 0, 0, 0, 0, 0}
err_tm = 0
stop_push_force()
if (TCIorController == 1) then
tci_close()
end
if (TCIorController == 2) then
rs485_close()
end
sleep(0.5)
if (TCIorController == 1) then
open = tci_open()
end
if (TCIorController == 2) then
open = rs485_open()
end
if (open >= 0) then
if (TCIorController == 1) then
tci_setopt(SERIAL_speed, SERIAL_bits, SERIAL_event, SERIAL_stop)
end
if (TCIorController == 2) then
rs485_setopt(SERIAL_speed, SERIAL_bits, SERIAL_event, SERIAL_stop)
end
else
elite_print("串口打开失败！")
exit() -- 并没有exit函数，只是为了让系统报错，脚本退出执行
end
sensor_init() -- User implement
start_push_force()
last_tm = os.clock()
-------------------------------main loop-------------------------------------------------------
repeat
now_tm = os.clock()
if TCIorController==1 then
ret, recv_buff = tci_recv(FRAME_MAX_RECV_TM, 1, FRAME_LEN * 2) --接收2帧长度的数据，确保有一帧为有效数据
end
if TCIorController ==2 then
ret,recv_buff = rs485_recv(FRAME_MAX_RECV_TM, 1, FRAME_LEN * 2) --接收2帧长度的数据，确保有一帧为有效数据
end
valid_frame = extract_nice_frame(recv_buff)
--elite_print("valid_frame : ",valid_frame)
if (valid_frame) then
torque = convert_str_to_torque_array(valid_frame) -- User implement
last_tm = now_tm
else
torque = {0, 0, 0, 0, 0, 0}
err_tm = now_tm - last_tm -- 计算传感器数据异常的持继时间
set_global_variable("D9", err_tm)
end
push_external_force(frame_idx, torque)
tci_flush()
frame_idx = frame_idx + 1
if frame_idx >= 65535 then
frame_idx = 1
end
sleep(0.001)
until (get_global_variable("D0") == 0)
stop_push_force()
if TCIorController==1 then
tci_close()
end
if TCIorController==2 then
rs485_close()
end
注：力传感器要改为连续发出数据模式。
