Vrep 仿真机械臂搬砖垒墙

爱牛牛啊

已于 2022-12-13 17:37:12 修改

阅读量353

点赞数

分类专栏：杂文章标签：单元测试

于 2022-11-27 15:06:53 首次发布

本文链接：https://blog.csdn.net/weixin_45302322/article/details/127186406

版权

杂专栏收录该内容

1 篇文章 1 订阅

订阅专栏

function sysCall_init()
    corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
end

showList = function(list)
    for i=1,#list,1 do
        sim.addStatusbarMessage(list[i])
    end
end

showMsg = function(msg)
    sim.addStatusbarMessage(msg)
end

wait = function(ms)
    local s = ms/1000
    sim.wait(s)
end

enableIk=function(enable)
    if enable then
        sim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1))
        for i=1,#jointHandles,1 do
            sim.setJointMode(jointHandles[i],sim.jointmode_ik,1)
        end

        sim.setExplicitHandling(ikGroupHandle,0)
    else
        sim.setExplicitHandling(ikGroupHandle,1)
        for i=1,#jointHandles,1 do
            sim.setJointMode(jointHandles[i],sim.jointmode_force,0)
        end
    end
end

function pendingForGripper(attachHandle, signal)
    if signal == true then -- if true ,wait for gripper finish loosing
        while true do
            child = sim.getObjectChild(attachHandle,0) -- if exist, return child Number, otherwise -1
            if child==-1 then
                --sim.addStatusbarMessage(child)
                return 0
            end
            wait(500)
        end
    else -- tighting
        while true do
            child = sim.getObjectChild(attachHandle,0) -- if exist, return child Number, otherwise -1
            if child~=-1 then
                --sim.addStatusbarMessage(child)
                return 0
            end
            wait(500)
        end
    end
end

function loosegripper(signal)
    if signal==true then
        --showMsg('loose gripper')
        sim.setIntegerSignal('RG2l_open', 1)
    else
        --showMsg('tighten gripper')
        sim.setIntegerSignal('RG2l_open', 0)
        sim.waitForSignal('RG2lRing')
        sim.clearIntegerSignal('RG2lRing')
    end
    pendingForGripper(attachPoint, signal)
end

function backToWait(waitPos, waitQuat)
    --loosegripper(true)
    sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,waitPos,waitQuat,nil)
end

function gripCube(gripPos)
    -- check vision sensor 'Vsup'    
    local gripQuat = sim.unpackTable(sim.readCustomDataBlock(topSensor, 'data'))
    --sim.addStatusbarMessage(string.format('direction: %f, %f, %f, %f',gripQuat[1], gripQuat[2], gripQuat[3], gripQuat[4]))

    -- at last we choose a direction to grip it   
    sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,gripPos,gripQuat,nil)
    wait(500)
    loosegripper(false)
end

function deliverCube(exchangePos, exchangeQuat)
    while true do -- wait for ur5r to finish receiving(clearing the doorRing)
        local s = sim.getIntegerSignal('doorRing')
        if s == nil then
            break
        end
        wait(500)
    end
    sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{exchangePos[1], exchangePos[2], exchangePos[3]+0.03}, exchangeQuat,nil)
    loosegripper(true)
    -------------------------------------------------------------------------
    -- finally, ur5l delievered a cube to the platform and now needs to 
    -- send a signal to notify ur5r to get it
    -------------------------------------------------------------------------
    sim.setIntegerSignal('doorRing', 1)
end

function coroutineMain()
    -- Initialize some values:
    jointHandles={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jointHandles[i]=sim.getObjectHandle('UR5_joint'..i)
    end
    
    ikGroupHandle=sim.getIkGroupHandle('UR5L_ik')
    ikTip=sim.getObjectHandle('UR5L_ikTip')
    attachPoint = sim.getObjectHandle('RG2_attachPoint')
    ikTarget=sim.getObjectHandle('UR5L_ikTarget')
    exchange=sim.getObjectHandle('exchangePos')
    waitHandle = sim.getObjectHandle('waitPosL')
    topSensor=sim.getObjectHandle('Vsup')
    --modelBase=sim.getObjectHandle(sim.handle_self)
    --modelName=sim.getObjectName(modelBase)

    -- Set-up some of the RML vectors:
    vel=180
    accel=40
    jerk=80
    currentVel={0,0,0,0,0,0,0}
    currentAccel={0,0,0,0,0,0,0}
    maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
    maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
    targetVel={0,0,0,0,0,0}

    ikMaxVel={0.4,0.4,0.4,1.8}
    ikMaxAccel={0.8,0.8,0.8,0.9}
    ikMaxJerk={0.6,0.6,0.6,0.8}
    
    local waitPos = sim.getObjectPosition(waitHandle,-1)
    local waitQuat = sim.getObjectQuaternion(waitHandle,-1)
    local gripPos = sim.getObjectPosition(ikTarget,-1)
    local gripQuat = sim.getObjectQuaternion(ikTarget,-1)
    local exchangePos = sim.getObjectPosition(exchange,-1)
    local exchangeQuat = sim.getObjectQuaternion(exchange,-1)

    --[[local s = sim.getObjectQuaternion(sim.getObjectHandle('Dummy'),-1)
    for i=1,#s,1 do
        sim.addStatusbarMessage(s[i])
    end]]
    enableIk(true)
    if true then
        loosegripper(true)
        while true do
            backToWait(waitPos, waitQuat)
            --sim.waitForSignal('gripperReady')
            sim.waitForSignal('beltRing')   -- block for beltRing, ifso grip cube 
                                            -- and clear signal for next waiting
            gripCube(gripPos)
            backToWait(waitPos, waitQuat)
            sim.clearIntegerSignal('beltRing')
            deliverCube(exchangePos, exchangeQuat)
            --sim.addStatusbarMessage(123)
        end
    end
    enableIk(false)
end```

function sysCall_init()
corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()
if coroutine.status(corout)~=‘dead’ then
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
end

showList = function(list)
for i=1,#list,1 do
sim.addStatusbarMessage(list[i])
end
end

showMsg = function(msg)
sim.addStatusbarMessage(msg)
end

wait = function(ms)
local s = ms/1000
sim.wait(s)
end

enableIk=function(enable)
if enable then
sim.setObjectMatrix(ikTarget,-1,sim.getObjectMatrix(ikTip,-1))
for i=1,#jointHandles,1 do
sim.setJointMode(jointHandles[i],sim.jointmode_ik,1)
end

    sim.setExplicitHandling(ikGroupHandle,0)
else
    sim.setExplicitHandling(ikGroupHandle,1)
    for i=1,#jointHandles,1 do
        sim.setJointMode(jointHandles[i],sim.jointmode_force,0)
    end
end

end

function pendingForGripper(attachHandle, signal)
if signal == true then – if true ,wait for gripper finish loosing
while true do
child = sim.getObjectChild(attachHandle,0) – if exist, return child Number, otherwise -1
if child==-1 then
–sim.addStatusbarMessage(child)
return 0
end
wait(500)
end
else – tighting
while true do
child = sim.getObjectChild(attachHandle,0) – if exist, return child Number, otherwise -1
if child~=-1 then
–sim.addStatusbarMessage(child)
return 0
end
wait(500)
end
end
end

function loosegripper(signal, make_static)
if signaltrue then
–showMsg(‘loose gripper’)
if make_statictrue then
sim.setIntegerSignal(‘RG2r_open’, 2) – loose&make cube static
else
sim.setIntegerSignal(‘RG2r_open’, 1) --loose
end
else
–showMsg(‘tighten gripper’)
sim.setIntegerSignal(‘RG2r_open’, 0)
end
pendingForGripper(attchPoint, signal)
end

function backToWait(waitPos, waitQuat)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,waitPos,waitQuat,nil)
–sim.setIntegerSignal(‘gripperReady’, 1) --gripper ready on waiting pos
end

function gripCube(gripPos)
local data = sim.unpackFloatTable(sim.readCustomDataBlock(platformHandle, ‘data’))
–[[sim.addStatusbarMessage(‘—’)
for i=1,3,1 do
sim.addStatusbarMessage(data[i])
end]]
local targetPos = {data[1], data[2], gripPos[3]}
sim.setObjectOrientation(ikTarget,-1, {0,0,data[3]})
local temp = sim.getObjectQuaternion(ikTarget,-1)
local targetQuat = {temp[1], temp[2], temp[3], temp[4]}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{targetPos[1], targetPos[2], targetPos[3]+0.06},targetQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,targetPos,targetQuat,nil)
loosegripper(false)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{targetPos[1], targetPos[2], targetPos[3]+0.03},targetQuat,nil)
end

function makeCubeUpsideDown(workPos)
–rotate rg2 faced left horizon and loose gripper
local temp = {0,-math.sqrt(2)/2,0,math.sqrt(2)/2}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.02},temp,nil)
loosegripper(true)

-- then make it above cube, grip cube and rotate 180
temp = {0,0,0,1}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]},temp,nil)
loosegripper(false)
-- WARNING:In order to avoid the rotation joint of the hand from 
-- exceeding the limit, it must be returned in the original way
temp = {0,0,math.sqrt(2)/2,-math.sqrt(2)/2} -- {0,0,math.sqrt(2)/2,math.sqrt(2)/2}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.02},temp,nil)
temp = {0,0,-1,0}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.02},temp,nil)
loosegripper(true)

-- again rg2 faced left horizon and in this case griper reach to the butt of cube ^^
temp = {0,0,math.sqrt(2)/2,-math.sqrt(2)/2}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.02},temp,nil)
temp = {0,0,0,1}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.02},temp,nil)
temp = {0,-math.sqrt(2)/2,0,math.sqrt(2)/2}--{math.sqrt(2)/2,0,-math.sqrt(2)/2,0}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]},temp,nil)
loosegripper(false)
temp = {0,0,0,1}
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.03},temp,nil)

end

function deliverCube(workPos, workQuat, Floor)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.07},workQuat,nil)
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,workPos,workQuat,nil)

wait(500)
--local child = sim.getObjectChild(attchPoint,0)
loosegripper(true,true)

-- ----------------------------------useless----------------------------------------
-- NOTICE: set cube manually because it has a y-direction error of 0.0004~0.0009
-- BUT this solution is not the best, it is only a temporary way, although it can solve this problem perfectly
--child_pos = sim.getObjectPosition(child,-1)
--sim.setObjectPosition(child,-1, {workPos[1], workPos[2], child_pos[3]})
-- IN THE END, we found the bug of y-offset: it's IK-Tip not being in the middle
-- of gripper that cause the problem
-- ---------------------------------------------------------------------------------



sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.07},workQuat,nil)

end

xy_table = {
{0,0},
{0.032,0.0133}, {0.064,0.0252}, {0.096,0.0357}, {0.128,0.0448}, {0.160,0.0525}, {0.192,0.0588},
{0.224,0.0637}, {0.256,0.0672}, {0.288,0.0693},
{0.320,0.070},
{0.352,0.0693}, {0.384,0.0672}, {0.416,0.0637}, {0.448,0.0588}, {0.480,0.0525}, {0.512,0.0448},
{0.544,0.0357}, {0.576,0.0252}, {0.608,0.0133},

{0,-0.3}, 
{0.032,-0.3}, {0.064,-0.3}, {0.096,-0.3}, {0.128,-0.3}, {0.160,-0.3}, {0.192,-0.3}, 
{0.224,-0.3}, {0.256,-0.3}, {0.288,-0.3}, 
{0.320,-0.3},
{0.352,-0.3}, {0.384,-0.3}, {0.416,-0.3}, {0.448,-0.3}, {0.480,-0.3}, {0.512,-0.3}, 
{0.544,-0.3}, {0.576,-0.3}, {0.608,-0.3},

}
–[[, {0.07,0}, {0.14,0},
{0,0.07}, {0.07,0.07}, {0.14,0.07},
{0,0.14}, {0.07,0.14}, {0.14,0.14}, ]]

function coroutineMain()

jointHandles={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
    jointHandles[i]=sim.getObjectHandle('UR5_joint'..i..'#0')
end

ikGroupHandle=sim.getIkGroupHandle('UR5R_ik')--2030004
ikTip=sim.getObjectHandle('UR5R_ikTip')
ikTarget=sim.getObjectHandle('UR5R_ikTarget')
attchPoint = sim.getObjectHandle('RG2_attachPoint#0')
exchange=sim.getObjectHandle('exchangePos')
platformHandle = sim.getObjectHandle('centerCam')
waitHandle = sim.getObjectHandle('waitPosR')

-- Set-up some of the RML vectors:
vel=180
accel=40
jerk=80
currentVel={0,0,0,0,0,0,0}
currentAccel={0,0,0,0,0,0,0}
maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0}

ikMaxVel={0.4,0.4,0.4,1.8}
ikMaxAccel={0.8,0.8,0.8,1.4}
ikMaxJerk={0.6,0.6,0.6,0.8}

local startPos = sim.getObjectPosition(ikTarget,-1)
local startQuat = sim.getObjectQuaternion(ikTarget,-1)
local waitPos = sim.getObjectPosition(waitHandle,-1)
local waitQuat = sim.getObjectQuaternion(waitHandle,-1)
local exchangePos = sim.getObjectPosition(exchange,-1)
local exchangeQuat = sim.getObjectQuaternion(exchange,-1)
enableIk(true)
if true then
    loosegripper(true)
    local workPos = {}
    local workQuat = startQuat
    local Floor = 0 -- the floor of 3*3 grids, place cubes face up 
                    -- when it's odd, face down when even
    local Floor_height_odd = 0.03
    local Floor_height_even = 0.03
    while true do
        --sim.addStatusbarMessage(math.floor(Floor/2)) -- even num
        --sim.addStatusbarMessage((math.floor(Floor/2) + Floor%2))  -- odd num
        local _waitPos = {waitPos[1], waitPos[2], waitPos[3]+ math.floor(Floor/2)*Floor_height_even + (math.floor(Floor/2) + Floor%2)*Floor_height_odd}
        for i=1,#xy_table,1 do
            backToWait(_waitPos, waitQuat)
            sim.waitForSignal('doorRing')   -- block for doorRing, ifso grip cube 
                                            -- and clear signal for next waiting
            wait(1500)
            -- grip the cube
            gripCube(exchangePos)
            
            -- make cube face down when the floor is even
            --if Floor%2 == 1 then
            --    makeCubeUpsideDown(exchangePos)
                --sim.addStatusbarMessage(123)
            --end
            -- clear the signal emitted by ur5l, now ut5l can deliver cube to platform
            sim.clearIntegerSignal('doorRing')
            
            -- rg2 back to wait pos
            backToWait(_waitPos, waitQuat)
            
            -- rg2 diliver cube and place it well
            workPos = {startPos[1]+xy_table[i][1], startPos[2]+xy_table[i][2], startPos[3] + math.floor(Floor/2)*Floor_height_even + (math.floor(Floor/2) + Floor%2)*Floor_height_odd}
            sim.addStatusbarMessage(string.format('level %d  cube %d:%f, %f, %f', Floor, i, workPos[1], workPos[2], workPos[3]))   -- cube position log info
            deliverCube(workPos, workQuat, Floor)
        end
        Floor = Floor + 1
        if Floor == 4 then
            sim.pauseSimulation()
        end
    end
end
enableIk(false)

end