V-rep機器人仿真軟件使用的學習筆記-續

前言

這篇博客主要是記錄自己學習和使用機器人仿真軟件V-rep過程中的一些知識點和踩的坑。是之前這篇V-rep機器人仿真軟件使用的學習筆記的續集，新增2020年使用V-rep的筆記，因爲是做項目過程遇到問題解決問題的記錄，內容依舊比較零散且沒有展開詳細講，更基礎的V-rep知識請參看之前的博客V-rep機器人仿真軟件使用的學習筆記。下面進入正文。

正文

1.child script中thread script中如果在while循環中添加sim.switchThread()，則該thread script執行的時間間隔和仿真間隔保持一致（如果仿真間隔爲real-time間隔，則與real-time間隔保持一致），否則該script將以儘可能快的速度進行執行。參考：https://www.cnblogs.com/21207-iHome/p/6767839.html

2.關節電機在力/力矩模式下且電機和控制循環均使能時的三種控制方式：

自定義控制方式：使用Joint callback functions關節回調函數，在回調函數中實現關節電機的閉環控制
位置控制（PID閉環），此時力矩是恆定的
彈簧阻尼模式：通過力/扭矩調製，該關節將起到彈簧-阻尼系統的作用，此時速度是恆定的

3.關節空間的軌跡規劃和位置控制方法

使用RML庫實現軌跡規劃和控制：輸入多個目標關節位置，速度參數等，進行運動，阻塞方式執行。RML官方網站：http://www.reflexxes.ws/products/overview-and-download.html。
需要自己實現位置閉環，利用sim.setJointTargetVelocity作爲速度內環，實現位置速度雙環控制，並對多個關節進行控制實現機械臂位置控制，此時關節電機需要電機使能，但控制迴路不使能。
利用關節電機的位置控制方式，利用sim.setJointTargetPosition輸入關節目標位置，V-rep已實現PID位置閉環控制，然後對多個關節進行控制實現機械臂位置控制

4.V-rep中讀寫文件至本地的方法，參考：http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=571&p=7467&hilit=file+write+in+a+file#p7467, 在腳本中使用Lua的文件讀寫命令實現，寫文件默認路徑是V-rep可執行文件所在的路徑。

寫文件

name = "toto.txt" -- 此時文件位於V-rep可執行文件所在目錄
-- name = "C:/Sam/A-level/Simulation_Environment/4CONTROL_ALGORITHM/toto.txt"  --此時文件位於指定目錄
file = io.open(name, "w")
file:write("Hello World")
file:close()

讀文件

name = "toto.txt"
file = io.open(name, "r")
print(file:read())
file:close()

注意：在windows系統下文件路徑分隔符需要使用左斜槓"/",如果用右斜槓會報錯"Lua runtime error: [string “CHILD SCRIPT Cuboid”]:28: attempt to index global ‘file’ (a nil value)".

5.單個關節實現位置閉環，內環爲速度環的一種實現（增量式PID）

function pid_init(current_p)
    pid = {}
    pid["target_position"] = 0  -- target postion
    pid["actual_position"] = current_p --actual position
    pid["error"] = 0 --??  
    pid["error_next"] = 0 --previous err 
    pid["error_last"] = 0 --previous previous err
    pid["kp"] = 3.5   --kp ki kd parematers   note: rad: kp=0.1    reg: pid = 3.5 0.3 0.3
    pid["ki"] = 0.3
    pid["kd"] = 0.3
    factor = 180.0/3.14159
    increment_position = 0 --incremnet
end

function pid_realise(target_p, current_p)  --implement pid, rarget is deg, current is rad
    pid["target_position"] = target_p  --set target position 
    pid["actual_position"] = current_p  -- current position
    pid["error"] = pid["target_position"] - pid["actual_position"]
    
     
    increment_position =increment_position + pid["kp"]*(pid["error"]-pid["error_next"])+pid["ki"]*pid["error"]+pid["kd"]*(pid["error"]-2*pid["error_next"]+pid["error_last"])  --??????  
    
    
    pid["error_last"] = pid["error_next"]  --next loop
    pid["error_next"] = pid["error"] 

    if (increment_position > 175)   --limited the increment
    then
        increment_position = 175
    end
    if (increment_position < -175)
    then
        increment_position = -175
    end

    print(increment_position, pid["error"], pid["target_position"], pid["actual_position"])
    return increment_position
end
 


function sysCall_threadmain()
    -- Put some initialization code here
    position = sim.getJointPosition(18)   -- 18 is the handle of the controled motor
    pid_init(position)

    -- Put your main loop here, e.g.:
    --
    while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
    --     local p=sim.getObjectPosition(objHandle,-1)
    --     p[1]=p[1]+0.001
    --     sim.setObjectPosition(objHandle,-1,p)
         --time_step = sim.getSimulationTimeStep()
         time_step = sim.getSimulationTime()
         position = sim.getJointPosition(18)
         increment = pid_realise(90, position*factor)

         --print('Test')
         print(time_step, increment)--position*factor)
         sim.setJointTargetVelocity(18, increment/factor)
         --name = "C:/Sam/A-level/Simulation_Environment/4CONTROL_ALGORITHM/toto.txt"
         --file = io.open(name, "a")
         --file:write("Hello World \n")
         --file:write(increment)
         --file:write("\n")
         --file:close()
         sim.switchThread() -- resume in next simulation step
    end
end

5.機械臂逆解的兩種方法
V-rep支持僞逆法和數值法兩種機械臂逆解方法，通常僞逆法速度快，但是遇到奇異問題可能解不存在，數值法速度慢但是基本能保證解的存在。。可以同時設置這兩種解法，V-rep會默認先使用僞逆法，當僞逆法無解時，會再調用數值法求解，對於DLS逆解方法需要設置Edit conditional parameters，在Perform if…條件欄中進行配置：如果undamped方法失敗，則執行damped方法。可以參考官方robots–>non-mobile–>7 Dof manipulator.ttm場景模型中的設置方法以及博客：https://www.cnblogs.com/21207-iHome/p/7420733.html。

6.機械臂逆運動學模式的使用
在逆運動學模式下，拖動target，關節將自動求逆解並轉動至對應位置。這種模式的設置參考官方robots–>non-mobile–>7 Dof manipulator.ttm場景模型中的設置方法，主要有以下幾個要點：

機械臂關節處於Inverse kinematics mode，如果機械臂均設置爲動力學使能，則Hybrid operation也要使能
必須設置兩組逆運動學解算對，一組使用Pseudo inverse（僞逆解）方法，一組使用DLS（數值解）方法

7.URDF文件不支持對並行結構的機器人結構（包含閉式運動鏈結構）進行描述，https://answers.ros.org/question/9050/using-ros-for-a-delta-robot/?answer=13094?answer=13094#post-id-13094

8.V-rep中將模型保存至本地
選中模型的根節點，然後在菜單欄中選擇File --> Save model as… 將其保存。
注意： 如果Save model as...選項爲灰色，說明所選中的模型沒有被設置爲根節點，這時可以通過設置模型屬性Scene Object Properties-->common-->Object is model base即可，還在在Object is model base中Edit model properties編輯模型的相關屬性和說明。

9.外部控制器時間步設置與同步的問題
外部力矩控制：https://blog.csdn.net/huangdianye/article/details/80628932

10.讓轉動關節成爲固定關節（不發生轉動）的設置方法
可以設置關節的最小位置和轉動範圍均爲0，則關節相當於不發生轉動。

11.逆運動學解算模塊的細節知識點
位於官方文檔的Calculation modules–>Inverse kinematics–>Basics on IK groups and IK elements位置。

連桿：如果關節不處於IK模式，則被認爲是連桿
存在多個IK group時，如果各個group的連桿運動之間存在干涉，則先求解對其他group有影響的group。
如果IK chains包含共享關節的結構，則將對應的tip-target包含至一個IK group中：

12.並聯機構的正逆運動學

13.雙臂緊約束協作得到路徑的幾種嘗試

兩個機械臂都工作在IK mode，首先將兩臂的吸盤吸至板材，然後同時選中兩臂的IK_Target進行移動和旋轉，由於兩臂工作在IK mode，兩臂將跟隨IK_Target進行運動，這時就可以得到兩臂的協作路徑，這種方法相當於是人工示教得到雙臂協作路徑，兩臂同時進行路徑規劃。
將兩臂分爲主從臂，主臂正常進行路徑規劃和運動，從臂的IK_Target設置在主臂上，這樣當主臂運動時，從臂就可以跟隨主臂運動。參考官方場景robotCollaboration.ttt的實現方法，利用主從思想和協作約束進行協作路徑規劃。
如圖所示將一個機械臂的IK_Target附着在另一個機械臂末端（主臂）上，且與所附着機械臂（主臂）的IK_Target保持協作約束關係，此時主臂運動時，從臂將跟隨運動。
將兩臂和板材看成閉式運動鏈，構成配置空間，然後進行運動規劃。這種應該是最合理的方法，但是實現難度較大。

14.IK Mode和FK Mode機械臂的控制
在IK Mode下從空間位置解算得到關節位置，如果機械臂處於IK Mode，則移動IK_Target時機械臂會自動跟隨，在IK mode下給定關節位置，機械臂是不會運動的。
在FK Mode時給定機械臂關節角，機械臂移動至對應的位置。
在程序中常使用如下代碼實現以上兩種狀態的切換：

setIkMode=function()
    sim.setExplicitHandling(ikGroup,0) -- that enables implicit IK handling
    sim.switchThread()
end

setFkMode=function()
    sim.setExplicitHandling(ikGroup,1) -- that disables implicit IK handling
    sim.switchThread()
    sim.setObjectParent(ikTarget,ikTip,true)
    sim.setObjectPosition(ikTarget,ikTip,{0,0,0})
    sim.setObjectPosition(ikTarget,ikTip,{0,0,0})
end

15.MATLAB Simscape Multibody中導入URDF文件
官方文檔：https://ww2.mathworks.cn/help/physmod/sm/ug/urdf-import.html
URDF中的部分標籤Simscape Multibody支持，有些屬性標籤Simscape Multibody並不支持（材料屬性，碰撞屬性，關節限位屬性等），對於不支持的屬性標籤，導入時會自動忽略。模型的mesh只支持stl格式，其他格式雖然無法導入，但只會導致機械臂外形無法顯示出來，不影響機械臂模型的動力學仿真。

導入abb-4600機械臂模型的過程記錄（官方文檔）：
切換至URDF工作路徑，然後使用smimport命令進行導入：
smimport('./abb_irb4600_support/urdf/test_abb_4600.urdf')
導入後在simulink界面使用simulation->update diagram更新後可以在Simscape explorer裏看到機械臂模型，如果機械臂顯示不正常，可能是mesh導入不正常，手動修改simulink中機械臂每個link子模塊的visual的stl路徑之後再次simulation->update diagram就可以正常顯示，或者通過編程自動設置stl文件路徑也可以。
ps: 此urdf文件中沒有包含inertial屬性，默認全爲零，所以動力學模型無法正常使用，下面嘗試使用meshlab軟件結合此機械臂官網datasheet中整機質量參數，來計算inertial參數。

16.機械臂的慣性參數獲取問題
① 從Solidworks導出成URDF文件時，可以導出inertial屬性，包含慣性參數
② 使用V-rep軟件獲取慣性參數
③ 可以使用軟件Meshlab進行計算，參考ROS官網文檔的說明：http://wiki.ros.org/abb_irb1200_support 。大概思路是：官方的機械臂datasheet只提供了整個機械臂的重量，假設機械臂密度均勻恆定，則每個連桿的體積決定其重量，在meshlab中可以得到機械臂各個連桿的體積，進一步得到慣性參數。
PS： 可以去機械臂的官網下載機械臂的CAD文件，比如：https://new.abb.com/products/robotics/zh/industrial-robots/irb-4600/irb-4600-cad

慣性參數獲取問題，參見另一篇博客機械臂慣性參數的獲取方法。

MATLAB聯合V-rep進行機械臂力矩控制仿真
切記： V-rep力矩控制時必須將仿真時間步設置爲5ms與物理引擎同步，默認是50ms，參考官方文檔說明。
[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-vzDsSbTP-1592535376092)(./pics_vrep/pics1_32.png)]
參考實現：

V-rep關節類型與控制方式和代碼實現：coppeliarobotics.com/helpFiles/en/jointDescription.htm#controller
V-rep力矩控制實現：https://forum.coppeliarobotics.com/viewtopic.php?t=25
https://blog.csdn.net/huangdianye/article/details/80628932
共享內存插件：https://github.com/santdiego/SASTools
V-rep共享內存的使用：https://forum.coppeliarobotics.com/viewtopic.php?t=4157
需要將仿真週期設置爲5ms，與物理引擎的時間步相同。
MATLAB SIMULINK實時仿真插件：https://www.mathworks.com/matlabcentral/fileexchange/29107-real-time-pacer-for-simulink
V-rep同步模式介紹：https://www.coppeliarobotics.com/helpFiles/en/remoteApiModusOperandi.htm#synchronous

雙臂機器人抓取仿真
https://www.mathworks.com/help/robotics/examples/model-and-control-a-manipulator-arm-with-simscape.html
MATLAB實時仿真
方法一：使用這個模塊實現與實際時間的同步：https://www.mathworks.com/matlabcentral/fileexchange/29107-real-time-pacer-for-simulink
方法二：使用官方提供的實時模塊Real-time sync，不過在使用這個模塊時需要安裝Real-Time Kernel，然後重啓電腦使Real-Time Kernel生效。
方法二使用官方例程時：open_system(fullfile(matlabroot,'toolbox','sldrt','sldrtexamples','sldrtex_vdp'));，出現如下錯誤：Error reported by S-function 'sldrtsync' in 'sldrtex_vdp/Real-Time Synchronization': Synchronization to real-time kernel has failed.
該問題在網上找到的解決辦法：https://www.mathworks.com/matlabcentral/answers/434279-why-does-simulink-real-time-run-very-slow-in-matlab-r2018b-on-windows-and-a-real-time-synchronizati，但是嘗試之後仍沒有解決，甚至導致電腦藍屏，暫且放棄方法二實現實時仿真。
MATLAB 物理建模與仿真

MATLAB SIMSCAPE物理模型建立與仿真：通用型物理模型建模
MATLAB Simscape Multibody：針對機械臂等機器人進行物理模型建模仿真
Simscape Multibody Link：導入CAD軟件3D模型至MATLAB進行物理建模
MATLAB Simscape Multibody常用模塊作用：
Rigid Transform：座標系之間的位姿變換，同時存在平移和旋轉變換時，應該先平移再旋轉
Brick Solid：剛性立方體模型
Mechanism Configuration block：可以指定整個系統的重力

初級例程：

高級例程：

MATLAB Simulink Data Inspector出現空白，無法顯示圖像的問題
使用Simulink.sdi.clearPreferences解決了上述問題。google查到的解決辦法使用sdi.Repository.clearRepositoryFile沒有任何效果。另附Simulink Data Inspector官方教程。
MATLAB Robotics System Toolbox的使用
常用函數文檔：
- 正運動學：getTransform
- 逆運動學：inverseKinematics，用法可以根據工具箱中源碼文檔確定，不同版本的MATLAB用法不同。
MATLAB Simulink生成C代碼
參考博客：
- 初級教程
- 官方講座1
- 官方講座2：在1小時20分30秒時講解了如何將生成的代碼集成到自己的項目中。
- 官方文檔
6自由度機械臂動力學模型的推導和符號運算：https://github.com/aseleit/robotic_manipulator_dynamics
C聯合V-rep仿真：機械臂力矩控制
1. V-rep端需要使用simRemoteApi.start(19999,1300,false,true)打開遠程api接口，第四項參數很重要，當爲true時才能開啓同步模式。此處的portNumber不使用19997是因爲端口19997是默認端口，工作在continuous remote API server service下，無法更改。
2. 在Qtcreator中創建控制器C++程序，並添加MATLAB生成的控制器C代碼。如圖所示，由於主函數爲CPP文件，所以添加的C函數和C頭文件都要放在extern "C"中，否則就會出現undefine reference to xx的編譯錯誤。
V-rep啓動多個RemoteAPI Server，從而可以在一個Client中使用多個thread連接V-rep的方法
當外部client程序一方面需要對V-rep中的機器人進行控制，另一方面需要定時獲取機器人狀態，這時候就需要多個thread和V-rep進行通訊，就產生了標題的需求。首先官方文檔說V-rep是支持多個外部client同時連接的，關鍵點在於要在v-rep端開啓不同port的server服務：

v-rep端開啓不同port的server服務主要有以下兩種方法：
這裏我使用第一種方法，直接修改V-rep根目錄下的remoteApiConnections.txt文件，修改後的文件如下：
可以看到該文件註釋部分也介紹瞭如何開啓多個port，這裏開啓了19997和19998兩個port，這個文件修改完成後啓動V-rep，將完成對應port的server的開啓，這樣外部client程序就可以同時使用兩個thread與V-rep進行通訊了。
在V-rep命令行中使用simRemoteApi.status()命令可以查看server端口是否開啓成功：
可以看到19997和19998端口都已經成功開啓。
V-rep中通過D-H參數自動生成機器人模型
D-H參考：
- D-H參數生成三維模型： https://wenku.baidu.com/view/aaff395a52ea551811a68730.html
- D-H參數介紹： https://blog.csdn.net/pengjc2001/article/details/70156333
- abb4600標準型D-H參數： https://www.doc88.com/p-0478665844349.html
- abb4600改進型D-H參數： https://bulletin.incas.ro/files/apostolescu_savu_ion-guta_ionita__vol_11_iss_4.pdf