文章目录

实现效果

• 给定目标点，生成机械臂工具坐标中心点到达该目标点时，关节角的配置。
• 因为机械臂在抓取物品时需要先到达“作业临近点”，所以这里设置的目标点在原来的Z轴上偏移一小段距离。
• 可以看到此时生成的关节角的配置只是xyz位置相同，欧拉角表示的姿态并不相同，在“改进”中给出了解决方法。
  

一些设置

导航点设置

• 关于如何添加设置 dummy 的细节建议参考官方手册，这里不再赘述。
  
  
  

添加连接用于碰撞的设置

• 按照序号点击（一个是机械臂，一个是所有的物体除了机械臂）：
  
  

代码实现

getMatrixShiftedAlongZ=function(matrix,localZShift)
    local m={}
    for i=1,12,1 do
        m[i]=matrix[i]
    end
    m[4]=m[4]+m[3]*localZShift
    m[8]=m[8]+m[7]*localZShift
    m[12]=m[12]+m[11]*localZShift
    return m
end

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

findCollisionFreeConfigAndCheckApproach=function(matrix)
    sim.setObjectMatrix(ikTarget,-1,matrix)
    local c=sim.getConfigForTipPose(ikGroup,jh,0.65,10,nil,collisionPairs)
    if c then
        local m=getMatrixShiftedAlongZ(matrix,ikShift)
        local path=generateIkPath(c,m,ikSteps)
        if path==nil then
            c=nil
        end
    end
    return c
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

generateIkPath=function(startConfig,goalPose,steps)
    -- Generates (if possible) a linear, collision free path between a robot config and a target pose
    forbidThreadSwitches(true)
    local currentConfig=getConfig()
    setConfig(startConfig)
    sim.setObjectMatrix(ikTarget,-1,goalPose)
    local c=sim.generateIkPath(ikGroup,jh,steps,collisionPairs)
    setConfig(currentConfig)
    forbidThreadSwitches(false)
    return c
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
    ikGroup=sim.getIkGroupHandle('ik')
    ikTarget=sim.getObjectHandle('target')
    collisionPairs={sim.getCollectionHandle('manipulator'),sim.getCollectionHandle('environment')}
    
    metric={1,1,1,1,1,1}
    forbidLevel=0
    ikShift=0.1
    ikSteps=50
    
    
    local theTarget=sim.getObjectHandle('testTarget')
    local m=sim.getObjectMatrix(theTarget,-1)
    print('m0: ',m)
    m=getMatrixShiftedAlongZ(m,-ikShift)
    print('m1: ',m)
    local c=findCollisionFreeConfigAndCheckApproach(m)
    print('c1: ',c)
    setConfig(c)
    sim.wait(20)
    c=findCollisionFreeConfigAndCheckApproach(m)
    print('c2: ',c)
    setConfig(c)
    sim.wait(20)
    c=findCollisionFreeConfigAndCheckApproach(m)
    print('c3: ',c)
    setConfig(c)
end

终端输出

• 可以看到两次输出的 c1 和 c2 不同，这是因为对于末端到达同一个位姿的逆解不是唯一的。

  "m0: ",{-2.2649765014648e-06, -1.0000025033951, 3.7252902984619e-09, -0.40000009536743, -0.99619722366333, -2.6226043701172e-06, -0.087155133485794, 0.024999998509884, 0.087155133485794, 1.1175870895386e-08, -0.99619972705841, 0.25}
  "m1: ",{-2.2649765014648e-06, -1.0000025033951, 3.7252902984619e-09, -0.40000009573996, -0.99619722366333, -2.6226043701172e-06, -0.087155133485794, 0.033715511858463, 0.087155133485794, 1.1175870895386e-08, -0.99619972705841, 0.34961997270584}
  "c1: ",{-1.5824935436249, -1.4476417303085, 1.1558154821396, 4.5667972564697, 3.6568768024445, -2.2497673034668}
  "c2: ",{-1.1846302747726, -0.17786026000977, -1.2501794099808, 5.2080044746399, -0.75768041610718, -3.0309326648712}
  "c3: ",{-4.8921489715576, -5.6154088973999, -5.9951300621033, 2.0139036178589, -3.8300971984863, -1.1612557172775}
  [sandboxScript:info]   simulation stopping...

改进

• 保证逆解满足欧拉角的设定：
  
• 实现效果：