RML 抓取-放置 运动应用

• 抓取阶段：（工业一般如此）

• 放置阶段：

-- This is a threaded script!

activateSuctionPad=function(active)
    if (active) then
        simSetScriptSimulationParameter(suctionPadScript,‘active‘,‘true‘)        --设置子脚本相关参数
    else
        simSetScriptSimulationParameter(suctionPadScript,‘active‘,‘false‘)
    end
end

getTargetPosVectorFromObjectPose=function(objectHandle)
    local p=simGetObjectPosition(objectHandle,targetBase)
    local o=simGetObjectQuaternion(objectHandle,targetBase)
    return p,o
end

getNextContainerIndex=function(index)
    index=index+1
    if index>3 then
        index=1
    end
    return index
end

getNextBoxIndex=function(index)
    index=index+1
    if index>2 then
        index=1
    end
    return index
end

threadFunction=function()
    while simGetSimulationState()~=sim_simulation_advancing_abouttostop do
        -- 1. Pick-up a box:
        -- Go to approach pose near container X:
        targetP,targetO=getTargetPosVectorFromObjectPose(approaches[containerIndex])
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        simWait(1)
        -- Go to grasp pose on box A:
        targetP,targetO=getTargetPosVectorFromObjectPose(boxes[boxIndex])
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        -- Activate suction pad:
        activateSuctionPad(true)
        -- Go to approach pose near container X:
        targetP,targetO=getTargetPosVectorFromObjectPose(approaches[containerIndex])
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        -- Go to initial pose:
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,initPos,initOr,nil)


        -- 2. Drop a box:
        -- Get the next container:
        containerIndex=getNextContainerIndex(containerIndex)
        -- Go to approach pose near container X+1:
        targetP,targetO=getTargetPosVectorFromObjectPose(approaches[containerIndex])
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        -- Go to drop pose on container X+1:
        targetP,targetO=getTargetPosVectorFromObjectPose(drops[containerIndex])
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        -- Deactivate suction pad:
        activateSuctionPad(false)
        -- Go to approach pose near container X+1:
        targetP,targetO=getTargetPosVectorFromObjectPose(approaches[containerIndex])
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,targetP,targetO,nil)
        simWait(1)
        -- Go to initial pose:
        simRMLMoveToPosition(target,targetBase,-1,nil,nil,maxVel,maxAccel,maxJerk,initPos,initOr,nil)


        -- 3. Now handle the other box:
        boxIndex=getNextBoxIndex(boxIndex)
        containerIndex=getNextContainerIndex(containerIndex)
    end
end

-- Initialization:
simSetThreadSwitchTiming(2) 

suctionPad=simGetObjectHandle(‘suctionPad‘)
suctionPadScript=simGetScriptAssociatedWithObject(suctionPad)        -- 获取子脚本

target=simGetObjectHandle(‘RobotTarget‘)
targetBase=simGetObjectHandle(‘Robot1‘)

box1=simGetObjectHandle(‘Cuboid1Grasp‘)
box2=simGetObjectHandle(‘Cuboid2Grasp‘)
boxes={box1,box2}

drop1=simGetObjectHandle(‘CuboidDrop1‘)
drop2=simGetObjectHandle(‘CuboidDrop2‘)
drop3=simGetObjectHandle(‘CuboidDrop3‘)
drops={drop1,drop2,drop3}

approach1=simGetObjectHandle(‘CuboidApproach1‘)
approach2=simGetObjectHandle(‘CuboidApproach2‘)
approach3=simGetObjectHandle(‘CuboidApproach3‘)
approaches={approach1,approach2,approach3}

-- targetSphere is the object that the robot‘s tooltip will try to follow via IK, which means that
-- if you change the position/orientation of targetSphere, then the robot‘s tooltip will try to follow
-- targetSphereBase is used so that all position and orientation values are always relative to the robot base
-- (e.g. so that if you move the robot to another position, you will not have to rewrite this code!)

-- Get the current position and orientation of the robot‘s tooltip:
initPos=simGetObjectPosition(target,targetBase)
initOr=simGetObjectQuaternion(target,targetBase)

-- Set-up some of the RML vectors:
maxVel={0.4,0.4,0.4,1.8}
maxAccel={0.3,0.3,0.3,0.9}
maxJerk={0.2,0.2,0.2,0.8}

activateSuctionPad(false)
boxIndex=2
containerIndex=2

-- Here we execute the regular thread code:
res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)
if not res then
    simAddStatusbarMessage(‘Lua runtime error: ‘..err)
end

-- Clean-up: