根据改进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