文章目录

解析

• 官方给出了 motionPlanningDemo1.ttt 这个代码范例，其中有在计算不同关节角下，机械臂末端的函数 getConfigConfigDistance用到了一个常量 metric，有几个关节，metric中就有几个元素。
• motionPlanningAndGraspingDemo.ttt 中的 Jaco 和 Mico 机械臂与motionPlanningDemo1.ttt 中的 KUKA LBR4 的 metric并不相同，在官方论坛只是说这是一个权重，毕竟每个关节对的D-H参数不同，它们对于机械臂末端的位置影响也不同，在计算空间中两个末端之间的距离时使用的是欧式距离+一个0～1之间的权重。
• 官方论坛上有说metric直接设置成全 1，我目前也是选择全部设置为 1。
• 我目前的想法是可以通过用搜索算法去计算不同关节角下的末端距离与软件中直接通过xyz坐标计算得到的距离进行损失的比较，最终得到合适的权重，当然该方法的实现我还并没有去实践。

代码实现

forbidThreadSwitches=function(forbid)
    if forbid then
        forbidLevel=forbidLevel+1
        if forbidLevel==1 then
            sim.setThreadAutomaticSwitch(false)
        end
    else
        forbidLevel=forbidLevel-1
        if forbidLevel==0 then
            sim.setThreadAutomaticSwitch(true)
        end
    end
end

getConfig=function()
    -- Returns the current robot configuration
    local config={}
    for i=1,#jh,1 do
        config[i]=sim.getJointPosition(jh[i])
    end
    return config
end

setConfig=function(config)
    -- Applies the specified configuration to the robot
    if config then
        for i=1,#jh,1 do
            sim.setJointPosition(jh[i],config[i])
        end
    end
end

getConfigConfigDistance=function(config1,config2)
    -- Returns the distance (in configuration space) between two configurations
    local d=0
    for i=1,#jh,1 do
        local dx=(config1[i]-config2[i])*metric[i]
        d=d+dx*dx
    end
    return math.sqrt(d)
end

function sysCall_threadmain()
    -- Initialization phase:
    jh={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jh[i]=sim.getObjectHandle('UR3_joint'..i)
    end
    
    metric={1,1,1,1,1,1}
    forbidLevel=0
    
    
    config1=getConfig()
    print('config1: ',config1)
    forbidThreadSwitches(true)
    config2={-70.1*math.pi/180,18.85*math.pi/180,93.18*math.pi/180,68.02*math.pi/180,109.9*math.pi/180,90*math.pi/180}
    setConfig(config2)
    print('config2',config2)
    print('New current config: ',getConfig())
    forbidThreadSwitches(false)
    print('Distance between config1 and config2: ',getConfigConfigDistance(config1,config2))
end

终端输出

"config1: ",{0, 0, 0, 0, 0, 0}
"config2",{-1.223475805648, 0.32899456400093, 1.6262977970083, 1.1871729572065, 1.9181168479418, 1.5707963267949}
"New current config: ",{-1.2234758138657, 0.32899457216263, 1.6262978315353, 1.1871730089188, 1.9181168079376, 1.5707963705063}
"Distance between config1 and config2: ",3.4359755481748