EC机器人正运动学计算


机器人正运动学为通过机器人各关节角度计算出末端的笛卡尔空间值(xyz abc)





EC机器人采用DH表示方法,对应平移和旋转顺序如下:

Elibot using DH modeling, d→θ →a→α (TzRzTxRx),即先绕固连坐标系的Z平移,再绕Z旋转,再绕新的X平移,再绕新的X旋转。

即上图机器人的各个固连坐标系,参照 (TzRzTxRx)方式获得。

各EC机器人理论DH参数如下。实际EC机器人会进行基于Leica激光跟踪仪的标定,得到实际机器人的DH参数。该参数可以从示教器或者SDK获得。


使用以下代码,即可以从机器人当前角度计算出当前法兰盘末端在Base下的位姿。




author:chenliao@elibot.cn
-- 20220722

function NumToStr(data, num)
local s = "%." .. num .. "f"
return string.format(s, data)
end

function WaitUntil(var, value)
--local vtmp =
while get_global_variable(var) ~= value do
sleep(0.1)
end
end

-- 字符串分割
function string.split(str,delimiter)
if str == nil or str == '' or delimiter == nil then
return nil
end
local result = {}
for match in (str..delimiter):gmatch("(.-)"..delimiter) do
table.insert(result,match)
end
return result
end

function DH2Pose(d, theta, a, alpha)
local pose = {0, 0, 0, 0, 0, 0}
pose[3] = d
pose[6] = theta

pose = pose_mul(pose, {a, 0, 0, 0, 0, 0})
pose = pose_mul(pose, {0, 0, 0, alpha, 0, 0})
return pose
end

function FwdKinCal(c_angle, DH)
--Elibot using DH modeling, d→θ →a→α (TzRzTxRx)
-- d1 = link parameter d1
-- a2 = link parameter d3
-- a3 = link parameter d5
-- d4 = link parameter d2 - link parameter d4 + link parameter d6
-- d5 = link parameter d7
-- d6 = link parameter d8
local d = {0, 0, 0, 0, 0, 0}
local a = {0, 0, 0, 0, 0, 0}
local theta = {0, 0, 0, 0, 0, 0}
local alpha = {-math.pi / 2, 0, 0, -math.pi / 2, -math.pi / 2, 0}

d[1] = DH[1]
a[2] = DH[3]
a[3] = DH[5]
d[4] = DH[2] - DH[4] + DH[6]
d[5] = DH[7]
d[6] = DH[8]

local pose = {0, 0, 0, 0, 0, 0}
for i = 1, 6, 1 do
pose = pose_mul(pose, DH2Pose(d[i], theta[i] + math.rad(c_angle[i]), a[i], alpha[i]))
end
return pose
end

function DealJson(recv_str)
local Data = string.split(recv_str,',')
local Data1 = string.split(Data[2],'"')
return tonumber(Data1[4])
end

function GetRobotDHPar()
local ret
local recv
local send_str = ''
local DHPar = {}
ret=connect_tcp_server(IPAdd ,8055)
--获取机器人连杆参数,连杆参数含义见下文解释
for i=1,8,1 do
send_str = '{\"jsonrpc\": \"2.0\", \"method\": \"getDH\", \"params\": {\"index\": '..(i-1)..'}, \"id\": '..i..'}\n'
ret=client_send_data(IPAdd,send_str,0,8055)
ret ,recv=client_recv_data (IPAdd ,3,0,8055) while ret<0 do ret=connect_tcp_server(IPAdd ,8055) ret=client_send_data(IPAdd,send_str,0,8055) ret ,recv=client_recv_data (IPAdd ,3,0,8055) end
-- elite_print(ret,recv)
DHPar[i] = DealJson(recv)
end
return DHPar
end

IPAdd ='192.168.1.200'
-- Modify robot IP

DH = {139.897, 119.493, 270.993, 114.007, 256.585, 97.993, 97.974, 89.004}
-- current robot link parameter
--Elibot using DH modeling, d→θ →a→α (TzRzTxRx)
-- d1 = link parameter d1
-- a2 = link parameter d3
-- a3 = link parameter d5
-- d4 = link parameter d2 - link parameter d4 + link parameter d6
-- d5 = link parameter d7
-- d6 = link parameter d8

sleep(0.3)
local c_angle = get_robot_joint()
DH = GetRobotDHPar()

local c_pose = FwdKinCal(c_angle, DH)
local out ={}
for i=1,3,1 do
out[i] = NumToStr(c_pose[i],3)
end
for i=4,6,1 do
out[i] = math.deg(c_pose[i])
out[i] = NumToStr(out[i],3)
end

elite_print('X:'..out[1]..', Y:'..out[2]..', Z:'..out[3]..'[mm]')
elite_print('Rx:'..out[4]..' ,Ry:'..out[5]..', Rz:'..out[6]..'[°]')

点击显示全文
赞同1
发表评论
分享

手机扫码分享
0
488
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
举报
请选择举报理由
举报
举报说明