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
注：力傳感器要改為連續發出數據模式。
