• [連載 0]Vrep入門介紹
• [連載 1]Vrep小車建恼占ぃ——前進(jìn)和轉(zhuǎn)向
• [連載 2]Vrep小車建目袅——內(nèi)嵌腳本
• [連載 3]Vrep小車建模——matlab控制
• [連載 4]Vrep導(dǎo)入三維模型——PUMA560機(jī)械臂
• [連載 5]Vrep--Matlab Robitic Toolbox--PUMA560機(jī)械臂控制
• [番外 1]Vrep小車機(jī)械臂抓取
• [Lib庫(kù) 1]CoppeliaSim差分避障小車的Simulink實(shí)現(xiàn)(B站視頻講解)
• [Lib庫(kù) 2]CoppeliaSim機(jī)械臂抓取的Simulink實(shí)現(xiàn)(B站視頻講解)
• 知乎專欄：Vrep機(jī)器人動(dòng)力學(xué)建模仿真

前言

機(jī)械臂抓取和擺放物體是常見的應(yīng)用場(chǎng)景，在上一節(jié)機(jī)械臂的案例中（B站視頻）我們實(shí)現(xiàn)了逆運(yùn)動(dòng)學(xué)控制之后，便可進(jìn)行一些任務(wù)功能的實(shí)現(xiàn)痢甘。在這個(gè)過程中，我們?cè)囋囋赟imulink控制CoppeliaSim茉贡，跳過建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型塞栅，我們可以做到什么地步。錄制B站視頻的時(shí)候一行一行敲代碼真是非常耗費(fèi)時(shí)間的腔丧，大家也很難從中間找到重點(diǎn)放椰，而且也不方便后期查詢和檢索作烟，還是通過文本來輔助，視頻讓大家了解主要功能砾医，文字了解一些細(xì)節(jié)拿撩，然后代碼和模型就留給大家自己去啃了……（不過放心，有不懂的可以聯(lián)系我藻烤，淘寶或微信都可以）绷雏。
所有CoppeliaSim的Simulink的模型都可以在購(gòu)買CoppeliaLib庫(kù)后免費(fèi)獲得（購(gòu)買鏈接），最新版本請(qǐng)聯(lián)系淘寶客服怖亭。

整體效果

整個(gè)執(zhí)行過程可以看下面這張圖：

機(jī)械臂抓取和擺放的過程

CoppeliaSim模型

首先我們用一張圖描述一下整個(gè)模型：

Dummy點(diǎn)的使用

我們使用了幾個(gè)dummy來進(jìn)行笛卡爾空間的輔助定位，控制機(jī)械臂的時(shí)候主要參考這幾個(gè)中間輔助點(diǎn)就可以了

• relaseDummy是用來標(biāo)記物體的釋放點(diǎn)坤检，也就是每隔一段時(shí)間兴猩，會(huì)從傳送帶上方釋放一個(gè)物體塊；
• GraspPoint是表示抓取點(diǎn)的位置早歇，也就是說物體在傳送帶上停留的位置倾芝，機(jī)械臂不管物體的姿態(tài)和形狀，都是從這個(gè)點(diǎn)開始抓取箭跳。（這里其實(shí)是有一個(gè)很強(qiáng)的假設(shè)條件的晨另，就是物體肯定處于機(jī)械臂可以抓取的狀態(tài)，所以物體中途如果遇到擾動(dòng)的話谱姓，抓取可能會(huì)失斀枘颉）；
• RelasePoint是表示釋放點(diǎn)的基準(zhǔn)位置屉来，后面機(jī)械臂的真實(shí)釋放點(diǎn)是通過基準(zhǔn)位置加偏移位置共同確定的路翻。

IKGroupToSimulink

這個(gè)模塊在B站的視頻 機(jī)械臂控制中已經(jīng)介紹過了，這里再贅述一下茄靠。
首先要設(shè)置CoppeliaSim的IK計(jì)算模塊茂契，設(shè)置過程如下圖：

然后依次設(shè)置UR5的每個(gè)關(guān)節(jié)的，注意關(guān)節(jié)要設(shè)置為Torque/force Mode慨绳，不要設(shè)置為Inverse kinematics mode掉冶，因?yàn)槲覀冞@合理要通過Simulink來控制機(jī)械臂的關(guān)節(jié)位置。

• 第一個(gè)為IK group的名稱，1號(hào)箭頭所指的地方
• 第二個(gè)為關(guān)節(jié)數(shù)量获询，UR5位6關(guān)節(jié)機(jī)械臂涨岁，因此設(shè)置為6
• 第三個(gè)為關(guān)節(jié)的名稱拐袜，UR5的命名格式為UR5_joint+編號(hào)的格式，因此第三部分為前面去掉編號(hào)的字符串梢薪。
  

最后Simulink中模塊的配置如下蹬铺，含義和剛才的類似。

傳送帶代碼

傳送帶代碼主要怎加了讀取測(cè)距傳感器的功能和添加物體的功能秉撇。如果測(cè)距傳感器檢測(cè)到障礙物甜攀，那么傳送帶停止運(yùn)動(dòng)；每隔20秒向傳送帶添加一個(gè)物體琐馆。兩個(gè)功能代碼比較簡(jiǎn)單规阀，大家自己參考。

function sysCall_init()
         
    pathHandle=sim.getObjectHandle("ConveyorBeltPath")
    forwarder=sim.getObjectHandle('ConveyorBelt_forwarder')
    sensor=sim.getObjectHandle('conveyorBelt_sensor')
    sim.setPathTargetNominalVelocity(pathHandle,0) -- for backward compatibility
    
    sampleTime = sim.getSimulationTimeStep() --獲取系統(tǒng)運(yùn)行周期
    
    ChangeCount = 20/sampleTime     --20秒釋放一個(gè)物體瘦麸，獲取執(zhí)行周期的次數(shù)
    count = ChangeCount
    relaseDummy = sim.getObjectHandle('relaseDummy')
    count = 0
    cuboidHandle = sim.getObjectHandle('Cuboid')

end

function sysCall_cleanup() 
 
end 

function sysCall_actuation() 

    count = count + 1
    if count >=ChangeCount then
        count  =0   --重新置零
        local hndShapes_table = sim.copyPasteObjects({cuboidHandle},0)
        hndShape = hndShapes_table[1]
        sim.setObjectPosition(hndShape,relaseDummy,{0,0,0})
        
    end

    beltVelocity=0.08

    if sim.readProximitySensor(sensor)>0 then
        --如果檢測(cè)到物體谁撼，傳送帶停止運(yùn)行
        beltVelocity=0
    end

    local dt=sim.getSimulationTimeStep()
    local p=sim.getPathPosition(pathHandle)
    sim.setPathPosition(pathHandle,p+beltVelocity*dt)
    --這后面的不用看，不用改
    -- Here we "fake" the transportation pads with a single static rectangle that we dynamically reset
    -- at each simulation pass (while not forgetting to set its initial velocity vector) :
    
    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])
end 

Simulink模型

Simulink模型的總體圖如下：

位置的沒有太多需要說明的地方滋饲，這里都是獲取和設(shè)置的絕對(duì)位置厉碟，也就是相對(duì)于世界坐標(biāo)系的位置。

姿態(tài)的獲取和設(shè)定

姿態(tài)的話需要進(jìn)行一下重點(diǎn)說明屠缭。我這里為了案例演示箍鼓，姿態(tài)的獲取采用四元數(shù)，姿態(tài)的設(shè)置采用了歐拉角呵曹，兩種表示各有優(yōu)缺點(diǎn)款咖。

• 四元數(shù)的優(yōu)點(diǎn)就是使用方便，它可以準(zhǔn)確的表示出空間的姿態(tài)逢并，并且也方便姿態(tài)之間的插值之剧，但是缺點(diǎn)是優(yōu)點(diǎn)抽象，很難和實(shí)際的物理量對(duì)應(yīng)起來（這里又要BB兩句了砍聊，很多同學(xué)非要知道它的物理意義才能用背稼，而對(duì)數(shù)學(xué)概念不接受，你就知道它是一個(gè)姿態(tài)的表示玻蝌，能拿來用就可以了蟹肘，說實(shí)話我對(duì)四元數(shù)的物理意義到底是啥也不懂(￣ェ￣;)，你要是非要物理意義就轉(zhuǎn)換成歐拉角俯树，多好理解）
• 歐拉角的有點(diǎn)是非常的形象和直觀帘腹，而且還可以根據(jù)你坐標(biāo)系的建立方式選擇不同的旋轉(zhuǎn)方式，所以就會(huì)出現(xiàn)那么多種歐拉角的格式了许饿；然而缺點(diǎn)就是歐拉角奇異性的問題阳欲，導(dǎo)致姿態(tài)直接插值會(huì)有問題。

因此，我們使用兩種方式球化，獲取姿態(tài)采用四元數(shù)秽晚，方便后面姿態(tài)進(jìn)行插值；設(shè)置姿態(tài)就無(wú)所謂了筒愚，可以選擇四元素或者是歐拉角赴蝇，這里為了演示就選擇歐拉角。兩個(gè)的使用注意事項(xiàng)我在模塊說明中都已經(jīng)寫清楚了巢掺，大家可以看一下句伶。主要區(qū)別就是四元數(shù)的順序和matlab中的不一樣，歐拉角的默認(rèn)旋轉(zhuǎn)順序不一樣陆淀，所以做轉(zhuǎn)換的時(shí)候一定要明確指定歐拉角的旋轉(zhuǎn)順序（不想麻煩的話設(shè)置的時(shí)候也直接用四元數(shù)考余，就不用考慮歐拉角的旋轉(zhuǎn)順序了）。

RG2手抓控制

手抓控制需要使用到simulink和CoppeliaSim通信的一個(gè)重要功能轧苫，就是發(fā)送float數(shù)組秃殉，其實(shí)這個(gè)場(chǎng)景非常的常見，如果你內(nèi)嵌腳本比較熟悉的話浸剩，你可以用內(nèi)嵌腳本直接把數(shù)據(jù)打包好，然后發(fā)送給simulink鳄袍，然后將來真是的機(jī)器人就把通信模塊換成數(shù)據(jù)總線绢要，就就可以將控制器應(yīng)用在實(shí)際機(jī)器人上了。
simulink模塊的控制邏輯是輸入0或1表示打開和閉合手爪拗小，然后生成第一個(gè)量重罪，就是目標(biāo)速度，從而操作手爪的打開和閉合哀九。具體還是自己看模塊的解釋和手爪的代碼吧剿配。

Stateflow

Stateflow的流程動(dòng)態(tài)是這樣的

回顧一下整個(gè)執(zhí)行過程

機(jī)械臂抓取和擺放的過程

這里還需要說明的一點(diǎn)就是有的同學(xué)可能會(huì)說“我感覺這個(gè)過程不復(fù)雜啊厨幻，為啥用了這么多狀態(tài)，是不是簡(jiǎn)單問題復(fù)雜化了”捏雌，這樣的問題我最開始學(xué)習(xí)Stateflow的時(shí)候也時(shí)常有性湿，因?yàn)?strong>習(xí)慣了文本編程語(yǔ)言的思維方式，用文本語(yǔ)言我們一直希望通過封裝增加程序可讀性满败，各種使用類或者是把某一塊功能單獨(dú)做成一個(gè)函數(shù)文件等等肤频，所有這些操作都是通過隱藏足夠多的細(xì)節(jié)來提升程序的可讀性，所以到最后的狀態(tài)機(jī)可能就是幾十行代碼算墨，看起來比較簡(jiǎn)潔宵荒，但是這樣的問題是有看到的細(xì)節(jié)不夠多净嘀。使用Stateflow也可以進(jìn)行功能模塊的組裝暑刃，但是我個(gè)人風(fēng)格就是一張藍(lán)圖繪到底膜眠，二話不說需要狀態(tài)就直接堆，縮小了就是整個(gè)狀態(tài)流圖宵膨，放大了就可以看到狀態(tài)的細(xì)節(jié)架谎，多方便；反之對(duì)于文本編程的話鸿脓，這樣寫肯定除了你誰(shuí)都看不懂了蛔溃，這就是Simulink的優(yōu)勢(shì)。而且對(duì)于一個(gè)成熟產(chǎn)品稚虎，狀態(tài)機(jī)一般都至少十幾個(gè)或者幾十個(gè)或者上百個(gè)或者上千個(gè)或者……沒有上限，所以Ｕ衫巍６涤鳌！狀態(tài)不是越少越好赡麦，代碼不是越短越好，不要糾結(jié)1+1=2用matlab計(jì)算是不是有點(diǎn)掉價(jià)帕识，搞工程的泛粹，要有總體概念肮疗，不要老是反省自己我是不是用牛刀殺雞了晶姊，我們干的就是用牛刀殺雞的活！Ｎ被酢们衙！（這段話個(gè)人情緒有點(diǎn)重，但實(shí)在是肺腑之言碱呼，實(shí)際大部分的情況蒙挑，就是你連個(gè)鉛筆刀都沒有，問題怎么都解決不了窗市，哈哈稻据，所以快速用牛刀解決好解決的問題蕾域，然后留下時(shí)間找你的鉛筆刀吧）信粮。

跳過上面的一堆廢話后馋袜，我們?cè)偃タ匆幌逻@個(gè)Stateflow模型男旗，過程劃分的比較明顯，具體過程還是去看B站視頻吧欣鳖，畢竟展示流程還是視屏更加的方便察皇。這里我們以第一個(gè)狀態(tài)講一下。

狀態(tài)1

狀態(tài)2

狀態(tài)3

狀態(tài)4

狀態(tài)5

狀態(tài)6

狀態(tài)7

總結(jié)

好了商佑，我們的這個(gè)demo的講解就到這里了，昨天錄制完B站視頻后厢塘，我自己聽了一下茶没，雖然前期列了底稿，但是還是有很多點(diǎn)沒有表達(dá)出來晚碾，所以增加了這篇文字說明抓半，希望把整個(gè)模型的思路說明白。真誠(chéng)的希望不管是視頻還是教程對(duì)你有所幫助格嘁，謝謝大家的支持笛求，下一期我們?cè)贂?huì)。