【VREP】根据DH参数快速建立机械臂模型

根据改进DH生成简单机械臂模型

通过D-H参数快速导入机械臂模型至V-rep中（参考链接）
【VREP】通过DH参数在CoppeliaSim(VREP)中生成机器人模型
标准和改进DH法区别

补全的代码，non threaded script 整体复制

function sysCall_init()
    -- do some initialization here
    -- 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}
            
creatRobotFromDH(DH_table,6)

end
function buildTransformMatrixFromDH(a,b,c,d)
    --using abcd for theta_i,d_i,a_i-1, alpha_i-1
    matrix = {math.cos(a),-math.sin(a),0,c,
                math.sin(a)*math.cos(d),math.cos(a)*math.cos(d),-math.sin(d),-b*math.sin(d),
                math.sin(a)*math.sin(d),math.cos(a)*math.sin(d),math.cos(d),b*math.cos(d),
                0,0,0,1}
    return matrix

end
-- 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 = sim.createPureShape(2, 26, {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

function sysCall_actuation()
    -- put your actuation code here
end

function sysCall_sensing()
    -- put your sensing code here
end

function sysCall_cleanup()
    -- do some clean-up here
end

-- See the user manual or the available code snippets for additional callback functions and details