通过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场景文件上传分享。。。