通過D-H參數快速導入機械臂模型至V-rep中

模型導入相關博客：將機器人3維模型導入至V-rep仿真軟件的方法小結

通過D-H參數導入機械臂模型至V-rep中

前言

V-rep仿真軟件主要支持從URDF文件導入機械臂模型，以及輸入D-H參數生成關節，然後手動搭建機械臂模型。本博客主要介紹如何通過機械臂D-H參數快速在V-rep仿真軟件中自動生成機械臂的三維模型。

正文

原理介紹

D-H參數表徵機械臂的機械結構，同時決定了機械臂的運動學模型。通過D-H參數快速在V-rep仿真軟件中自動生成機械臂的三維模型的思路如下：
（1） 利用連桿變換的通式，通過D-H參數可以首先定位各個關節的位姿

（2） 調用V-rep的sim.createJoint()接口函數生成關節
（3） 根據（1）計算出的位姿，調用V-rep的sim.setObjectMatrix()函數將關節移動至計算出的位姿
（4） 然後爲每個關節生成外形包絡圖，這裏使用圓柱體作爲每個關節的外形包絡圖；
（5）最後根據兩個關節的位置座標計算出兩個關節之間的連桿的幾何參數，根據幾何參數生成長方體作爲連桿的外形包絡圖，最終實現由D-H參數導入機器人模型

程序實現

將上述介紹的實現過程在V-rep的non-thread child script中實現，給出實現的函數如下：

-- dh_table:theta_i,d_i,a_i-1, alpha_i-1
function creatRobotFromDH(dh_table, axis_nums)
    if #dh_table/4 ~= axis_nums then
        print("wrong d-h table data")
        return -1
    end
    print(dh_table)
    
    -- creat a cuboid as base_link
    local x, y, z, m = 0.5, 0.5, 0.08, 10
    local o_handle = -1
    o_handle = remote_creatCuboid(x, y, z, m)
    sim.setObjectPosition(o_handle, -1, {0, 0, z/2})  -- move base_link to the ground
    sim.setObjectName(o_handle, 'base')

    -- creat joint and link by D-H table
    local i = 1
    local parent_handle = o_handle  --parement object handle
    local TransMat = {1,0,0,0, 0,1,0,0, 0,0,1,0}  -- base transform matrix


    -- firstly: creat joints and set joints to the right pose according to D-H parameters
    for i=1, axis_nums,1 do
        print(1)
        -- 1.compute the transform matrix of joint i coordinate 
        local joint_i_TransMat = sim.multiplyMatrices(TransMat, buildTransformMatrixFromDH(dh_table[1+4*(i-1)],dh_table[2+4*(i-1)],dh_table[3+4*(i-1)],dh_table[4+4*(i-1)]))
 

        -- 2. creat joint i and set it's name
        local joint_handle = sim.createJoint(sim.joint_revolute_subtype,sim.jointmode_ik,-1)
        sim.setObjectName(joint_handle, 'joint'..i)

        -- 3. set joint pose with transform matrix
        if i == 1 then
            sim.setObjectMatrix(joint_handle, o_handle,  sim.getObjectMatrix(o_handle, -1))
        else
            sim.setObjectMatrix(joint_handle, o_handle, joint_i_TransMat)
        end

        -- 4. set parent of joint
        sim.setObjectParent(joint_handle, parent_handle, true)

        -- 5. finish joint set and update some data
        TransMat = joint_i_TransMat
        parent_handle = joint_handle
        
    end
    
    -- secondly: build a joint mesh 
    local x,y,z,mass = 0.1,0.1,0.2,1   -- set cylinder radius and height
    for i=1, axis_nums,1 do
        local j_h = sim.getObjectHandle('joint'..i)
        local handle = sim.createPureShape(2, 26, {x,y,z}, mass)
        sim.setObjectName(handle, 'joint_mesh'..i)
        sim.setObjectMatrix(handle, -1, sim.getObjectMatrix(j_h, -1)) -- set joint mesh same pose as joint
        sim.setObjectParent(handle, j_h, true)
    end
   
   
    -- thirdly: build a link for every joint and set link to the right pose according to joint pose
    local  x,y,z,mass = 0.2,0.2,0.2,1
    for i=2, axis_nums,1 do
        print(1)
        local j2_h = sim.getObjectHandle('joint'..i)   -- i joint
        local j1_h = sim.getObjectHandle('joint'..(i-1))  -- i-1 joint
       
        local xyz2 = sim.getObjectPosition(j2_h, -1)
        local xyz1 = sim.getObjectPosition(j1_h, -1)

        local height = math.max(math.abs(xyz2[1]-xyz1[1]), math.abs(xyz2[2]-xyz1[2]), math.abs(xyz2[3]-xyz1[3]))
        if math.abs(xyz2[1]-xyz1[1]) >= height then    -- along x axis
            local handle = sim.createPureShape(0, 26, {height,y,z}, mass)
            sim.setObjectName(handle, 'robot_link'..(i-1))
            
            -- set link position  relative to joint1
            local xyz = xyz1
            xyz[1] = xyz[1] + (xyz2[1]-xyz1[1])/2
  
            sim.setObjectPosition(handle, -1, xyz)
            sim.setObjectParent(handle, j1_h, true)
        elseif  math.abs(xyz2[2]-xyz1[2]) >= height then   -- along y axis
            local handle = sim.createPureShape(0, 26, {x,height,z}, mass)
            sim.setObjectName(handle, 'robot_link'..(i-1))

            -- set link position  relative to joint1
            local xyz = xyz1
            xyz[2] = xyz[2] + (xyz2[2]-xyz1[2])/2
  
            sim.setObjectPosition(handle, -1, xyz)
            sim.setObjectParent(handle, j1_h, true)
        else 
            local handle = sim.createPureShape(0, 26, {x,y,height}, mass)
            sim.setObjectName(handle, 'robot_link'..(i-1))
            
            -- set link position  relative to joint1
            local xyz = xyz1
            xyz[3] = xyz[3] + (xyz2[3]-xyz1[3])/2
            
            sim.setObjectPosition(handle, -1, xyz)
            sim.setObjectParent(handle, j1_h, true)
        end

    end

end

函數傳入的參數是一個機械臂的標準型D-H參數表和機械臂的軸數，給出一個ABB4600機械臂的標準型D-H參數表：

-- stadard d-h table: {theta_i,d_i,a_i-1, alpha_i-1}
-- abb4600:theta_i,d_i,a_i-1, alpha_i-1
pi = math.pi
DH_table = {0, 0.495, 0, 0,
            -pi/2, 0, 0.175, -pi/2,
            0, 0, 0.900, 0,
            0, 0.960, 0.175, -pi/2,
            0, 0, 0, pi/2,
            -pi, 0.135, 0, -pi/2}

將這個ABB4600機械臂D-H參數表自動生成模型後得到的機械臂模型如下圖所示，下方的是自動生成的模型，上方的是V-rep中自帶的ABB4600機械臂模型，因爲D-H參數並沒有包含外形包絡圖方面的信息，僅通過D-H參數生成3D模型只能達到這樣的效果，但是以及足夠用於機械臂的仿真了。

PS
上面實現的通過D-H參數導入模型，所傳入的D-H參數必須是標準型D-H參數，改進型D-H參數無法正常完成導入，標準型和改進型D-H參數的區別和相互轉換可以看參考博客，通常將改進型D-H參數變爲標準型D-H參數只需要將a和alpha值向下移動一行即可。

參考博客

• D-H參數生成機器人三維仿真系統：https://wenku.baidu.com/view/aaff395a52ea551811a68730.html
• 標準型和改進型D-H參數：https://blog.csdn.net/qq_44926743/article/details/105777789
• ABB4600機械臂標準型D-H參數來源：https://www.doc88.com/p-0478665844349.html
• ABB4600機械臂改進型D-H參數來源：https://bulletin.incas.ro/files/apostolescu_savu_ion-guta_ionita__vol_11_iss_4.pdf
• 標準型和改進型D-H參數之間的轉換方法：https://blog.csdn.net/qq_44926743/article/details/105777789

More

之後整理好文件後，將本博客通過D-H參數導入機械臂模型至V-rep的ttt場景文件上傳分享。。。