devbox
Gausst松鼠会
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

【CoppeliaSim仿真】用UR5进行识别、抓取和码垛_coppeliasim edu中实现分拣立方体与码垛

作者：Gausst松鼠会 | 2024-02-16 21:16:29

赞

踩

coppeliasim edu中实现分拣立方体与码垛

简介

1

​ 场景中有一条传送带，传送带上有多个传感器，传送带前方分别是两个UR5和一个同样带有传感器的中转台，再往前则是一个用来展示码垛的平板。开始传送带产生并运送物块至它的前端，此时每个物块的朝向是随机的，然后两个UR5负责分工将物块（在中转台交接后）整齐摆放至展示平板上，不仅如此，摆放的物块之间还需要进行齿合，摆成多层的样式（如3×3×2层）。

​ 仿真演示：

1
1

传送带随机生成物块

对传送带脚本优化后，物块的生成、传送带的启停都十分完好

传送带bug解决

传送带脚本：非线程脚本，跟随仿真频率同步刷新。每次刷新读取前端速度传感器，若未检测到物体，则按心跳生成物块；若检测到物体，则心跳暂停，并等待至该物块消失。
（重要： lua里面的高级存储结构的赋值是浅复制，修改一个新复制的列表，老的列表仍然会被修改。）

function sysCall_init()
    tick=0 -- 物块生成心跳
    pathHandle=sim.getObjectHandle("ConveyorBeltPath")
    forwarder=sim.getObjectHandle('ConveyorBelt_forwarder')
    sensor=sim.getObjectHandle('ConveyorBelt_sensor')
    sim.setPathTargetNominalVelocity(pathHandle,0) -- for backward compatibility
    handle=sim.getObjectHandle("Shape")
    block_handle={handle}
end

function sysCall_actuation() 
    beltVelocity=sim.getScriptSimulationParameter(sim.handle_self,"conveyorBeltVelocity")
    local grippingSignal = sim.getIntegerSignal('beltRing')
    if (sim.readProximitySensor(sensor)>0) then -- when detected cube, emit working signal
        sim.setIntegerSignal('beltRing', 1) 
    end
    if grippingSignal ~= nil then -- pending for compeleting work -- 检测到物块，便停止传送带，而且只有在外部清除'beltRing'之后传送带才会继续
        beltVelocity=0
    end
    
    -- 传送带移动模块
    local dt=sim.getSimulationTimeStep()
    local pos=sim.getPathPosition(pathHandle)
    pos=pos+beltVelocity*dt
    sim.setPathPosition(pathHandle,pos) -- update the path's intrinsic position
    
    relativeLinearVelocity={beltVelocity,0,0}
    -- Reset the dynamic rectangle from the simulation (it will be removed and added again)
    sim.resetDynamicObject(forwarder)
    -- Compute the absolute velocity vector:
    m=sim.getObjectMatrix(forwarder,-1)
    m[4]=0 -- Make sure the translation component is discarded
    m[8]=0 -- Make sure the translation component is discarded
    m[12]=0 -- Make sure the translation component is discarded
    absoluteLinearVelocity=sim.multiplyVector(m,relativeLinearVelocity)
    -- Now set the initial velocity of the dynamic rectangle:
    sim.setObjectFloatParameter(forwarder,sim.shapefloatparam_init_velocity_x,absoluteLinearVelocity[1])
    sim.setObjectFloatParameter(forwarder,sim.shapefloatparam_init_velocity_y,absoluteLinearVelocity[2])
    sim.setObjectFloatParameter(forwarder,sim.shapefloatparam_init_velocity_z,absoluteLinearVelocity[3])

    if (beltVelocity>0) then
        tick=tick+1
        -- 生成物块
        if tick == 40 then	
            copy=sim.copyPasteObjects(block_handle,2) -- 复制初始物块
            local matrix=sim.getObjectMatrix(sim.getObjectHandle('beltStart'),-1) --起始点位姿
            local temp=math.random(6) -- 获取随机姿态，下面是对位姿矩阵的修改
            if temp==1 then
                matrix[1]=90
            elseif temp==2 then
                matrix[2]=90
                matrix[3]=90
            elseif temp==3 then
                matrix[2]=90
                matrix[3]=-90
            elseif temp==4 then
                matrix[1]=90
                matrix[3]=90
            elseif temp==5 then  
                matrix[1]=90
                matrix[3]=-90
            elseif temp==6 then
            end
            sim.setObjectMatrix(copy[1],-1,matrix) -- 将复制的物块投放到起始点处
            tick = 0
        end
    end
end 

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68

UR5逆解

这里的逆解是用IKgroup实现的，是一个软件内置的实时计算功能，它能自动计算一组dummy之间的逆解并且实现自动跟随。

ikgroup设置方法：

ikgroup1
ikgroup2
  • 在场景中添加两个dummy：1. 在你的UR5根节点下添加一个dummy（如UR5L_ikTarget），这个节点是一个领航节点，其作用是控制你的手抓到达的位姿的；2. 在RG2的节点下（UR5_link7也行）添加一个dummy（如UR5L_ikTip），这个节点与你的手抓绑定在一起，请务必将其位置调到手抓的两个指头夹紧处，因为移动它就是移动你的手抓；3. 双击dummy图标（如UR5L_ikTip）打开属性编辑界面，点击Linked dummy选择另外一个建好的dummy（如UR5L_ikTarget），这一选项实现两个dummy的关联；再点击Link type选择IK, tip-target即逆解模式，这时两个dummy便成功连接起来，且会有一条红线连接着双方。
ikgroup3
  • 选择侧边栏的第三个按钮进入Calculation Modules properties计算模块，点击Kinematics->Add new IK group添加一个逆解组并命名（如UR5L_ik）。这样，你就建立了一个逆解组，但此时组中还没有计算的目标，需要接下来的设置；
  • 设置逆解组的属性：设置计算模式Calc. method为DLS；设置最大迭代次数Max. iterations为200，这个是越大得到的跟踪效果越好
ikgroup4
  • 点击Edit IK elements进入；然后在下拉页中选择绑定在你的手抓上的那个dummy（UR5L_ikTip），注意一定是绑定在手抓上的，另外一个是领航点，是随意摆放的；然后点Add new IK element with tip添加；接下来进行它的属性设置，选择计算基Base为UR5根节点（如UR5L），然后下面的Target会以粗体的形式自动给出；最后将X、Y、Z、Alpha-beta、Gamma全部打上勾就完成了

至此，该UR5（及其手抓RG2）就已经具备了逆解结算、轨迹规划的能力了，它的位姿将由你的UR5L_ikTarget给定，使用时在代码中获取导航点句柄，再移动目标点就能移动机械臂和手抓了

--示例代码：获取导航点句柄，然后直接用函数移动到目标位置
ikTarget=sim.getObjectHandle('UR5L_ikTarget')
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 gripPos = sim.getObjectPosition(ikTarget,-1) --这是导航点的位置，放在工作空间中的一个点处
local gripQuat = sim.getObjectQuaternion(ikTarget,-1) --导航点姿态
sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,gripPos,gripQuat,nil)

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

RG2抓取物块

要实现手抓对物块的抓取，有两条路可以实现：这里采用的第二种实现

  1. 选择按照真实夹取的方式取实现夹取，去计算力、摩擦、速度等，这样十分的麻烦且不稳定；
  2. 将手抓中的物块以父子节点绑定的方式，来实现手抓抓取物块；

这里是官方给的实现思路，大体上也就是上面所说的两种方法。

-- 1. You try to grasp it in a realistic way. This is quite delicate and sometimes requires
--    to carefully adjust several parameters (e.g. motor forces/torques/velocities, friction
--    coefficients, object masses and inertias)
--
-- 2. You fake the grasping by attaching the object to the gripper via a connector. This is
--    much easier and offers very stable results.

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

这里给出绑定抓取的实现：

当控制信号RG2_open在外部被设置成0时，手抓夹紧并抓住其中物块；当为1时手抓松开，释放物块；当为2，手抓在释放物块的同时，将物块设置成not dynamic使其完全静止。

成功抓取

值得注意的是，手抓的脚本是一个非线程脚本。它的函数入口是sysCall_actuation，也就意味着仿真的每一帧都会去刷新这个脚本；换个角度解释，也就是手抓每关闭一帧/传感器每读取一次数据，这个脚本都要刷新一遍。

function sysCall_init()
    motorHandle=sim.getObjectHandle('RG2_openCloseJoint')
    motorVelocity=0.05 -- m/s
    motorForce=10 -- N
    tick=0
    lock = true
end
-- 存放手抓之间的距离，当有物块存在时会依次递减到距离最小以保证加夹紧
attachFlag = 1  -- this falg store the distance between gripper and cube,
                -- it stoped when reach minimum 

function sysCall_actuation()
    tick  = tick+1
    if tick == 5 then
        local v=-motorVelocity
        local data=sim.getIntegerSignal('RG2l_open')
        
        --连接点和传感器句柄
        connector=sim.getObjectHandle('RG2_attachPoint')
        objectSensor=sim.getObjectHandle('RG2_attachProxSensor')

        if data==1 then	-- loose gripper: release the cube and set it child of the world
            v=motorVelocity
            loose(connector, objectSensor)
        elseif data == 2 then -- loose gripper & make cube static
            v=motorVelocity
            loose(connector, objectSensor, true)
        else	-- tighten gripper: make cube child of the gripper
            tighten(connector, objectSensor)
        end
        --手柄动作执行
        sim.setJointForce(motorHandle,motorForce)
        sim.setJointTargetVelocity(motorHandle,v)
        tick = 0
    end
end

function tighten(father, fatherSensor)
    index=0 -- shape序号从0开始
    while true do	--在这一桢中，遍历场景中的所有shape
        shape=sim.getObjects(index,sim.object_shape_type)               
        if (shape==-1) then
            break
        end

        res, dis= sim.checkProximitySensor(fatherSensor,shape)--速度传感器做检测
        if res~=nil and dis~=nil then -- 只有在手抓闭合动作时才有值
            
            _, para1 = sim.getObjectInt32Parameter(shape,sim.shapeintparam_respondable)
            _, para2 = sim.getObjectInt32Parameter(shape,sim.shapeintparam_static)
            if para1 == 1 and para2 == 0 then -- 筛选出cube
                -- step minus attachFlag and make it reach to minimum
                if attachFlag > dis+0.03 then
                    attachFlag = dis
                else
                    attachedShape=shape
                    -- Do the connection:
                    sim.setObjectParent(attachedShape,father,true)--将物块绑定在手抓上
                    lock = false
                    break
                end
            end
        end
        index=index+1
    end
end

function loose(father, fatherSensor, make_static)
    child = sim.getObjectChild(father, 0) -- get children of this father
    if child ~= -1 then
        sim.setObjectParent(child,-1,true) -- 释放物块，将其还原到世界中
        attachFlag = 1  -- recover  the distance ffalg
        if make_static==true then
            -- 将物块设置成not dynamic
            sim.setModelProperty(child, sim.modelproperty_not_dynamic)
        end
    end
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78

视觉传感器初步

在世界中右键->Add->Vision sensor->Orthographic type便可添加一个视觉传感器

简单获取图像并保存至本地

这只是对图像的简单处理，在程序中任何地方都可以调用sim的接口来获取视觉传感器的信息。

  • getVisionSensorCharImage()获取图像传感器的图像信息，（虽然还有别的接口，但是好像就这个好用一点）。接收参数：传感器句柄[、图像起始位置、图像范围、图像模式]；返回：图像流指针、X方向分辨率、Y方向分辨率
  • sim.saveImage()保存图像到缓存或文件。接收参数：图片流指针、XY分辨率列表、图像模式、保存文件路径、图片质量；返回：保存结果
--简单获取图像
topSensor=sim.getObjectHandle('Vsup') -- 传感器句柄
image, resX, resY =sim.getVisionSensorCharImage(topSensor)--读取传感器内容
sim.saveImage(image,{resX, resY}, 0, 'C:\\Users\\Administrator\\Desktop\\demo.png', -1)--保存传感器图片到本地,(resX、resY为256)

--获取部分图像
image, resX, resY =sim.getVisionSensorCharImage(topSensor, 0, 0, 100, 100, 0) --后面5个参数分别是：截取图像的X、Y坐标，截取图像的X、Y长度，图像模式
sim.saveImage(image,{100, 100},0, 'C:\\Users\\Administrator\\Desktop\\demo.png', 0) --注意，由于image是100像素的图片，所以此处第二个参数不能再是{resX,resY}，必须与现在的图像分辨率一样

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
用窗口显示传感器图像
  1. 世界中右键->add->floating view
  2. 显示窗口中右键->view->绑定选中相机即可为传感器建立一个实时的显示界面。注意相机只有在开始仿真时才有图像
相机的简单设置

相机分为透视相机和正交相机，二者的区别也只有镜头上的差异，一个的视界为锥形、一个为柱形。

相机设置

这是设置界面，其中：

  1. explicit handling可以理解为自己控制相机脚本的触发。在不勾选的情况下相机脚本的刷新频率为仿真频率，勾选后相机的刷新可以由自己来控制，但是这需要自己手动调用handleVisionSensor句柄。
  2. perspective mode：透视模式/或正交模式
  3. Ignore RGB/depth info：在处理时忽视RGB/深度信息，一般选择性勾选
  4. Packet1 is blank：勾选后使用readVisionSensor等类似接口将得不到处理的数据包。一般不勾选
  5. Packet1 - 15 auxiliary values：也就是read传感器接口得到的数据包，其中包含了灰度、RGB、深度信息。

P a c k e t 1 = { I m i n , R m i n , G m i n , B m i n , D m i n ,   I m a x , R m a x , G m a x , B m a x , D m a x ,   I a v g , R a v g , G a v g , B a v g , D a v g } Packet1 = \{Imin,Rmin,Gmin,Bmin,Dmin,\ Imax,Rmax,Gmax,Bmax,Dmax,\ Iavg,Ravg,Gavg,Bavg,Davg\} Packet1={Imin,Rmin,Gmin,Bmin,Dmin, Imax,Rmax,Gmax,Bmax,Dmax, Iavg,Ravg,Gavg,Bavg,Davg}

  1. Near/far clipping plane：相机的最近/最远拍摄距离，一般包括到你所拍摄的物体就够了
  2. persp. angle / ortho. size：当为透视模式时，这个值是相机的张角（角度为单位）；当为正交模式时，这个值是相机视角的宽度值（m为单位）
  3. Resolution X/Y：相机的分辨率。数值越高运算越慢
添加脚本处理图片：

这才是正确的处理方式，将图片处理大部分交给相机脚本去做：

  1. 添加一个non-thread script：其中init部分仅开始时执行一遍；actuation刷新频率为仿真设置的频率，基本在50ms量级。
function sysCall_init()
    -- do some initialization here
    cam = sim.getObjectAssociatedWithScript(sim.handle_self)	--获取该相机句柄
    view = sim.floatingViewAdd(0.9, 0.9, 0.9, 0.9, 0)	--添加子窗口
    sim.adjustView(view, cam ,64)
    res = sim.getVisionSensorResolution(cam)	--获取相机分辨率
    tick = 0
end

function sysCall_actuation()
    -- put your actuation code here
    tick = tick + 1	--自定义触发频率
    if tick%10 == 0 then
        sim.handleVisionSensor(cam)	--调用处理脚本
        res, t0, t1 = sim.readVisionSensor(cam)
    end
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  1. 编写handleVisionSensor函数所触发的函数，即如下的sysCall_vision函数。其中：
    1. 工作空间（workImg）是相机处理图片的内部空间；
    2. 传感器空间（sensorImg）是传感器输出到外界的图片信息，也就是显示在窗口上、或者在别的脚本处理时所使用的空间；
    3. buffer1指的是一个新建的图层，原处理是在工作空间的图层0上进行的，需要新的图层使用它即可，在最后别忘了将所有图层的信息汇总。
function sysCall_vision(inData)
    local handler = inData.handle
    simVision.sensorImgToWorkImg(handler)	--将传感器数据保存到工作空间来处理
    simVision.selectiveColorOnWorkImg(handler, {1,1,1}, {0.15, 0.15, 0.15}, true,true,false)	-- 由颜色来分割当前图片
    simVision.workImgToSensorImg(handler)	--将工作空间的图像输出到传感器空间去
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  1. 这里用到的simVision是coppeliaSim内置的一个简单处理图像的包，能够完成对图像的滤波、边缘检测、区域分割等功能。内置还有一个openCV的包*simIM**，功能十分强大。*
传感器信号订阅发布：

在完成一个动作时（如速度传感器检测到物体），发送信号到一个主题上，此后在ur5中就可以根据这个信号（或者数据）来做接下来的逻辑。例：

传送带脚本：检测速度传感器的值，当检测到物体时发送和信号beltRing

if (sim.readProximitySensor(sensor)>0) then
    sim.setIntegerSignal('beltRing', 1) -- when belt stopped, emit signal
    beltVelocity=0
else
    sim.clearIntegerSignal('beltRing')-- when belt started, clear signal
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

ur5l脚本：执行ur5l循环，在每个循环中等待beltRing

while true do
    backToWait(waitPos, waitQuat)
    sim.waitForSignal('beltRing')   -- block for beltRing, ifso grip cube 
    								-- and clear signal for next waiting
    gripCube(gripPos)
    backToWait(waitPos, waitQuat)
    deliverCube(exchangePos, exchangeQuat)
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
数据块存储：

一般的，在视觉传感器的处理脚本和ur5的运行脚本往往独立，而ur5在需要得到处理的数据时就会用到数据块存储技术。数据块存储包括：一般数据存储和持续数据存储。

  1. 一般数据存储为sim.writeCustomDataBlock()，它将数据保存在它的脚本对应的存储区中，在此次仿真运行期间有效，外界需要使用时凭句柄号就可访问；
  2. 持续数据存储为sim.persistentDataWrite()，它将数据保存在ttt文件中，不论打开或关闭项目都不会失效，不同的，它是凭键名访问。

视觉传感器脚本：将direction数据保存在该脚本的数据存储区中

sim.writeCustomDataBlock(handler, 'data', sim.packTable(direction))

  • 1

ur5l脚本：获取视觉传感器的数据存储区中data数据，并解码

local gripQuat = sim.unpackTable(sim.readCustomDataBlock(topSensor, 'data'))

  • 1

值得注意的是：数据块中的数据是以字符串编码的形式保存的，在存/取时都要进行编码解码操作。

视觉传感器

传送带视觉传感器：

当传送带运送物块至前端时，此处的视觉传感器Vsup便能拍摄到物块的信息。然后通过物块的朝向控制ur5l的抓取姿态。

工件2

获取每一帧的图像，对图像做光斑检测，分割出物块；通过检测物块的像素和深度信息从而得到物块的朝向，最后保存至脚本的数据存储区中由外界（ur5l）调用。如下：

function sysCall_init()
    -- do some initialization here
    cam = sim.getObjectAssociatedWithScript(sim.handle_self) -- 获取传感器句柄
    view = sim.floatingViewAdd(0.9, 0.9, 0.9, 0.9, 0)	-- 建立一个悬浮窗用以显示图像
    sim.adjustView(view, cam ,64)
    
    resolution = sim.getVisionSensorResolution(cam)
    tick = 0
end

function sysCall_actuation()
    -- put your actuation code here
    tick = tick + 1
    if tick==10 then
        sim.handleVisionSensor(cam) -- 执行图像处理
        tick = 0
    end
end

function sysCall_vision(inData)
    -- beacuse outside cant receive the data we return through 
    -- interface sim.readVisionSensor, so we keep data handled here
    local flag = false
    local packet = {}
        
    local handler = inData.handle
    simVision.sensorImgToWorkImg(handler)
    simVision.selectiveColorOnWorkImg(handler, {1,1,1}, {0.15, 0.15, 0.15}, true,true,false) -- 分割出图像中白色区域
    
    local _, ans = simVision.blobDetectionOnWorkImg(handler, 0.1, 0, false)
    packet = sim.unpackFloatTable(ans) --should unpack first
    if #packet == packet[2]+2 then    -- the detection is safe
        flag = true
    end
    if flag then
        blobSize = packet[3] -- 光斑面积
        blobOrientation = packet[4] -- 光斑旋转角度
        blobDimension = {packet[7]*resolution[1], packet[8]*resolution[2]}    -- width, height
        blobCenter = {packet[5]*resolution[1], packet[6]*resolution[2]} -- 光斑重心
        blobPosition = {packet[5]*resolution[1]-blobDimension[1]/2, packet[6]*resolution[2]-blobDimension[2]/2}
        --sim.addStatusbarMessage(string.format('size:'..blobSize))
        --sim.addStatusbarMessage(string.format('orientation:'..blobOrientation))
        --sim.addStatusbarMessage(string.format('center:%f, %f',blobCenter[1], blobCenter[2]))
        --sim.addStatusbarMessage(string.format('width&height:%f, %f',blobDimension[1], blobDimension[2]))
    else
        return 0
    end

    simVision.workImgToSensorImg(handler)
    local direction = {}
     -- focus on depth data, the smaller the closer to camera, the bigger the farther
    center = sim.getVisionSensorDepthBuffer(handler,blobCenter[1],blobCenter[2],1,1)	--光斑重心的深度
    if math.abs(blobDimension[1]-blobDimension[2]) < 2 then  -- the cube lay up
        direction = {0, 0, -math.sqrt(2)/2, math.sqrt(2)/2} -- 给出朝上时手抓的四元数
    else -- cube lay right/left/front/behind
        local width = blobDimension[1]
        local height = blobDimension[2]
        if width < height then
            target = sim.getVisionSensorDepthBuffer(handler,blobCenter[1]-blobDimension[1]/4,blobCenter[2]-blobDimension[2]/2.5,1,1)
            if math.abs(center[1] - target[1]) < 0.01 then  --lay behind
                direction = {0, -math.sqrt(2)/2, 0, math.sqrt(2)/2}--{0,0,1,0}
            else    -- lay front
                direction = {-math.sqrt(2)/2, 0, -math.sqrt(2)/2, 0}--{0,0,0,1}             
            end
        else
            target = sim.getVisionSensorDepthBuffer(handler,blobCenter[1]-blobDimension[1]/2.5,blobCenter[2]-blobDimension[2]/4,1,1)
            if math.abs(center[1] - target[1]) < 0.01 then  --lay right
                direction = {0.5,0.5,0.5,-0.5}
            else    -- lay left
                direction = {0.5,-0.5,0.5,0.5}--{0, 0, -math.sqrt(2)/2, math.sqrt(2)/2}             
            end
        end
    end
    -- store to 'data'
    sim.writeCustomDataBlock(handler, 'data', sim.packTable(direction)) -- 保存至脚本的数据块中，在外界可以方便调用
    --return direction 
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
中台视觉传感器：

在ur5l在传送带前端抓取到物块之后，便转而将物块转运到中台并且使之朝上；然后ur5r在通过中台视觉传感器的识别后以准确的姿态抓取物块，进行码垛。在这其中，中台起了转存和识别物体的作用。

中台

获取每一帧图像，通过区域分割、光斑检测获取到物块的二值图像；然后结合*键槽（图中的凸起和凹槽）*的深度信息计算图像的图像矩，从而映射到手抓的朝向中去，进而精确地抓取到物块。

  1. 图像的分割
function sysCall_init()
    -- do some initialization here
    cam = sim.getObjectAssociatedWithScript(sim.handle_self)
    view = sim.floatingViewAdd(0.9, 0.5, 0.3, 0.3, 0)
    sim.adjustView(view, cam ,64)
    resolution = sim.getVisionSensorResolution(cam)
    cam_pos = sim.getObjectPosition(cam,-1) -- 相机的世界坐标
    cam_xy = 0.15 -- view angle width
    tick = 0
end

function sysCall_actuation()
    -- put your actuation code here
    tick = tick + 1
    if tick==5 then
        sim.handleVisionSensor(cam)
        tick = 0
    end
end


function sysCall_vision(inData)
    -- beacuse outside cant receive the data we return through 
    -- interface sim.readVisionSensor, so we keep data handled here 
    -- and store it in data block
    local flag = false
    local packet = {}
        
    local handler = inData.handle -- 传感器句柄
    simVision.sensorImgToWorkImg(handler)	--	将传感器信息拷贝到工作空间
    simVision.selectiveColorOnWorkImg(handler, {1,1,1}, {0.15, 0.1, 0.15}, true,true,false)	-- 选出rgb为1，1，1的图像，并提供一定的容差

    local _, ans = simVision.blobDetectionOnWorkImg(handler, 0.1, 0, false)	-- 光斑检测
    packet = sim.unpackFloatTable(ans) --should unpack first
    if #packet == packet[2]+2 then    -- the detection is safe
        flag = true
    end
    if flag then
        blobSize = packet[3]
        blobOrientation = packet[4]*180/math.pi --angle
        if blobOrientation > 0 then	-- 传感器计算待正方形的图像矩很不平滑， 后面又专门的操作
            blobOrientation = -90 + blobOrientation
        end
        blobDimension = {packet[7]*resolution[1], packet[8]*resolution[2]}    -- width, height -- 将光斑的宽、高投射到图像空间
        blobCenter = {packet[5]*resolution[1], packet[6]*resolution[2]}	-- 光斑的重心
        blobPosition = {packet[5]*resolution[1]-blobDimension[1]/2, packet[6]*resolution[2]-blobDimension[2]/2}	-- 光斑的起始点
    else
        return 0
    end
    simVision.workImgToSensorImg(handler)

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  1. 图像矩的计算：
	-- color some key point, like center of blob, center of circle
    local image, resx, resy = sim.getVisionSensorCharImage(handler)
    local point = {blobCenter[1], blobCenter[2]}
    ans = colorAPoint(image, point[1], point[2], resx, resy) --给图像中的特殊点上色，调试时用到
    point = {   
        blobCenter[1]-math.sqrt(2)*(blobDimension[1]/4*math.cos((45+blobOrientation)*math.pi/180)),
        blobCenter[2]-math.sqrt(2)*(blobDimension[2]/4*math.sin((45+blobOrientation)*math.pi/180))
    } -- 用来提取深度的特殊点
    ans = colorAPoint(ans, point[1], point[2], resx, resy)
    sim.setVisionSensorCharImage(handler, ans)
    if point[1]<0 or point[2]<0 then
        return 0
    end
    -- use oritation to obtain the center of circle and judge direction with depth info
    local depth_center = sim.getVisionSensorDepthBuffer(handler,blobCenter[1],blobCenter[2],1,1)
    local depth_point = sim.getVisionSensorDepthBuffer(handler, point[1], point[2],1,1)
    local rotate = 0
    if depth_point[1] < depth_center[1] then    --higher
        rotate = blobOrientation+90
    else    --lower
        rotate = blobOrientation
    end

    local data = {cam_pos[1] - (blobCenter[2]/resolution[2]-0.5)*cam_xy, cam_pos[2] + (blobCenter[1]/resolution[1]-0.5)*cam_xy, rotate*math.pi/180} --x, y, oritentation
    -- store to 'data'
    sim.writeCustomDataBlock(handler, 'data', sim.packFloatTable(data)) -- 保存至脚本的数据块中，在外界可以方便调用

function colorAPoint(imageString, x, y, resX, resY)   -- 上色
    local image = sim.unpackUInt8Table(imageString)
    local point = math.floor(y-1)*resY+math.floor(x-1)
    image[point*3+1] = 255
    image[point*3+2] = 0
    image[point*3+3] = 0
    image = sim.packUInt8Table(image)
    return image
    --sim.saveImage(image, {x,y}, 0, 'C:\\Users\\Administrator\\Desktop\\a.png',0)
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37

机械臂

UR5L：

左机械臂是一个ur5机械臂，执行一个子线程脚本。这个脚本功能上完成物块从传送带到中台的转移，将随机朝向的物块按照统一的朝向——朝上放置到中台上，然后通知右机械臂ur5r来接取。

  1. 左机械臂包含三个执行过程：抓取—等待—放置。这三个过程分别对应了三个位置：gripPos、waitPos、exchangePos。
  2. 仿真开始，机械臂运动到等待位置waitPos，等待传送带的到来信号beltRing；
  3. 执行抓取过程，机械臂运动到抓取位置gripPos，并且读取传送带视觉传感器的位姿数据，以恰当的姿势抓取物块；
  4. 回到waitPos，并等待ur5r将完成信号doorRing消除，说明此时右机械臂已经将中台上的物块取走；
  5. 执行放置过程，将物块放置到中台exchangePos，并设置完成信号；
  6. 回到2，结束此次循环；
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	-- loose gripper
        sim.setIntegerSignal('RG2l_open', 1)
    else -- tighten gripper
        sim.setIntegerSignal('RG2l_open', 0)
        sim.waitForSignal('RG2lRing')
        sim.clearIntegerSignal('RG2lRing')
    end
    pendingForGripper(attachPoint, signal)
end

function backToWait(waitPos, waitQuat)
            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 sysCall_threadmain()
    -- 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')

    -- Set-up some of the RML vectors:
    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)

    enableIk(true)
    if sim.getSimulationState()~=sim.simulation_advancing_abouttostop then
        loosegripper(true)
        while true do
            backToWait(waitPos, waitQuat)
            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)
        end
    end
    enableIk(false)
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
UR5R:

右臂同样也是一个ur5机械臂，执行一个子线程脚本。将物块从中台上取出，并且将物块翻转，然后按照特定的位置放到平板上。

  1. 右机械臂同样包含三个位置：exchangePos、waitPos、startPos，交换点是右臂从中台抓取物块的点，等待点做与左臂的交互工作，开始点是展示平板上的开始摆放的位置
  2. 仿真开始，机械臂运动到等待位置waitPos，等待ur5l的完成信号doorRing；
  3. 执行抓取过程，机械臂运动到exchangePos，并且读中台带视觉传感器的位姿数据，以精确的姿势抓取物块；
  4. 判断当前码垛的层数，若该层为偶数层则将物块倒向抓取；
  5. 回到waitPos，并清除信号doorRing；
  6. 将物块放置到中台startPos，并添加合适的偏置；
  7. 回到2，结束此次循环；

核心代码：（代码中的码垛层数与上面的层数不一致）

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 = 0.051
while true do
    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
        workPos = {startPos[1]+xy_table[i][1], startPos[2]+xy_table[i][2], startPos[3] + Floor*Floor_height}
        if Floor%2 == 1 then
            makeCubeUpsideDown(exchangePos)
            workPos = {startPos[1]+xy_table[i][1], startPos[2]+xy_table[i][2]+0.0006, startPos[3] + Floor*Floor_height}
        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
        deliverCube(workPos, workQuat)
    end
    Floor = Floor + 1
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31

偶数层倒向码垛处理办法：

由于是4×9的码垛规格，第一层的物块可以按中台的姿势直接进行摆放，但是第2、4层的物块就要求面朝下摆放。此时的处理便是：在中台将物块由朝上变成躺平，然后再抓取物块的底部，最后便可以按照一样的目标位姿摆放第二层的物块。

（有一个重要的点就是，在手抓旋转的时候一定要按照原路转回去，否则旋转角度一直累加便会超限）

转换代码：

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.03},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)
    pendingForGripper(attchPoint)
    -- 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.03},temp,nil)
    temp = {0,0,-1,0}
    sim.rmlMoveToPosition(ikTarget,-1,-1,nil,nil,ikMaxVel,ikMaxAccel,ikMaxJerk,{workPos[1], workPos[2], workPos[3]+0.03},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.03},temp,nil)
    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)
    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)
    pendingForGripper(attchPoint)
    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)
    wait(500)
end

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32

常见bug解决：

物块无法咬合bug：

期望的是物块倒置的时候两个工件是能够咬合的，但是由于模型（设置）的问题，在Bullet引擎下它们会悬浮在交合面处，即是给上方一个力它也会迅速弹开。目前还没有找到原因

工件咬合bug

有两个解决办法：

  1. 将引擎设置成ODE引擎便能实现咬合（但是即使咬合了仍会在莫名其妙的时候弹开）；
  2. 在右臂释放时将物块设置成not dynamic模式，即此时物块不参与引擎的碰撞检测。这种方法十分稳固。见RG2的抓取
物块莫名其妙的粘在手抓上bug：

手抓在释放物块时偶尔可以看到即使物块已经脱离ur5的绑定（属于世界的子类），但是它仍然“粘”在手抓上。

抓取bug1

引擎问题，不论是使用的绑定为子类的抓取方法还是力控的抓取方法，都有可能出现这个问题。在抓取的时候尽量保证与抓取面平行就好。

有一个解决办法：

  1. （绑定为子类的抓取）将物块设置成子类时，手抓中距离传感器的阈值设置的比物块大即可。换言之，就是在手抓还没有完全接触到物块时就设置成子类就行了。
handleVisionSensor函数无法得到packet2/3等的bug：

写的时候碰到了个很头疼的问题，在调用handleVisionSensor之时只能获取到res和packet1，也就是检测数和默认打包的参数，而自己处理后返回的参数无法得到。使用readVisionSensor也是如此。

相机问题1

其实这完全就不是一个bug，观察能够获取到的packet1就可知，返回的packet是需要解码的，因此，我们在sysCall_vision中返回的packet2/3等就需要先编码后才能return。

在不知道这一点的情况下有一种解决办法：

  1. 使用dataBlock，即数据块存储；

总结

制作时长：5-6D

工程文件也同步分享了出来：https://download.csdn.net/download/uuuuur/12722192

欢迎讨论
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Gausst松鼠会/article/detail/96740
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号