机器人正运动学为通过机器人各关节角度计算出末端的笛卡尔空间值(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]..'[°]')