vrep外部控制器力矩控制執行個體——以matlab腳本控制平面兩連杆為例

前言

抱歉，我不知道要怎麼将每個自然段開頭空兩格。大家将就看吧。

V-rep（virtual robot experimentation platform）是一款較為小衆的機器人仿真軟體，其最大的優勢是跨平台優勢明顯，支援多種語言對場景中的對象進行控制，被戲稱為機器人仿真軟體中的“瑞士軍刀”，其EDU版本對學習科研用途免費開放。跟具體的介紹就不繼續說了，本部落格目的在于講述如何對已經建好的被控對象（機械臂） 通過外部控制器對其進行控制，而且專指力矩控制。

想要入門Vrep的同學可以仔細地把vrep 的user manual通讀一遍，并把tutorial都仔細看一遍并動手做幾個實踐，然後遇到不懂的問題在forum裡面搜尋一下，想要獲得更具體的功能，可以翻牆到youtube裡面看一些關于vrep的教程，國内的教程暫時還比較稀缺。建議連接配接有：

[1] http://www.coppeliarobotics.com/helpFiles/ （該連結可以在vrep的菜單欄中的help->Help topics中直接跳轉）

[2] http://www.forum.coppeliarobotics.com/

因為前面幾篇部落格的學習筆記暫時還沒有提到控制器的設計，是以這裡先不讨論比較進階的控制器的設計方法，隻是先做一個鋪墊。先用PD控制器給大家看一下控制效果，了解①利用vrep建立被控對象。②編寫外部控制器實作被控對象的控制。③理清整個架構的搭建。

本教程以下面版本的Vrep進行講解，其他資料的版本比較老，這裡的函數會跟其他教程有些不同。

過程

廢話就不多說了，來點實際的東西，下面開始講要怎麼做

1. 在Vrep内建立一個平面兩連杆模型

• 打開Vrep，ctrl+N打開新的場景 。
• 添加部件。add->primitive shape->cylinder，add->primitive shape->cuboid，add->joint->revolute 注意添加部件的時候，dynamic和respondable shape要check√（因為我們需要用到的是力矩控制，要使用實體引擎）。使用快捷鍵ctrl+E來調節擺放的位置，然後通過ctrl+D選擇view geometry，uncheck keep proportion，修改文本框的參數設定部件的大小和長度，還可以adjust color來調節顔色。最終擺放效果如下：

  

• 配置scene hierarchy中的shapes之間的關系如下圖所示：

  

  左側的hierarchy tree表示了部件之間的連接配接方式，關節将兩個連杆之間的運動互相關聯。按照上圖左側的層次數配置好部件之間的層級關系之後，還需要更改部件之間的dynamic properties。除了要mask部件之間的respondable關系之外，還要注意uncheck掉base的dynamic屬性，使得各個關節就像是有一個平穩的底座，固連在大地上一樣，保證不會因為動力學的原因底座不穩。最後還要在common properties中令兩個關節的visibility勾選到隻剩下第10層，讓場景中看不到關節。

2. 連接配接Vrep中的兩連杆模型和matlab控制腳本

完成第一個步驟之後，我們将獲得一個完整的被控對象，這時候需要将vrep中的兩連杆模型與matlab互相連接配接。

• Vrep場景中添加腳本。在場景中層次數的任意一個object中右鍵，add->associate child script->non-thread. 然後輕按兩下新出現的icon，在sysCall_init()函數中添加以下語句，其他可以不用管，這個場景不需要添加任何代碼了。

repeat until (simRemoteApi.start(,,false,true)~=-)
           

為什麼要這麼添加呢？其實之前看的到現在寫blog隔了這麼久，我也忘了具體的語句要怎麼寫，但是我知道要怎麼查user manual，是以檢視這些資料還是很重要的。打開[1]連結，在導航欄中的Writing code in and around Vrep->V-REP API framework-> Remote API->Enabling the Remote API- server side（注意Vrep是server side而外部控制器則是client side）

打開之後通讀一遍，着重要關注的是simRemoteApi.start這個函數。

number result=simRemoteApi.start(number portNumber,number maxPacketSize=,Boolean debug=false,Boolean preEnableTrigger=false)
           

我們需要把該函數添加到場景中的任意一處的子腳本内，且一個場景隻要有一個腳本有這個東西就行了。通常portNumber會選擇19999，其他沒有特殊要求的話，使用預設值即可。當然我們說的是沒有特殊要求，實際上，在進行力矩控制的時候，力矩控制是precise control 不是loose control是以我們需要vrep與matlab的控制指令同步運作，是以，我們需要選擇使用EnableTrigger功能。

為什麼需要打開這個功能呢？再次查詢user manual，剛剛的Remote API下面有一個叫做“Remote API modus operandi”即一些控制模式的慣用技巧。在進行同步模式的時候，必須要注意提前使能這種控制模式。

synchronous mode同步模式是什麼将會在第3點中給大家解釋。

• 上面是Vrep部分的配置。下面我們再來講一下matlab部分要怎麼配置。matlab部分要聲明連接配接的IP位址和端口号。打開matlab,建立一個指令腳本，然後添加以下代碼：

vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep.simxFinish(-); % just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',,true,true,,);
           

這個代碼是怎麼來的呢？檢視user manual：

注意配置幾個檔案，就像添加matlab工具箱那樣，在Matlab添加接口相關檔案，這些檔案可以在Vrep的安裝檔案夾中找到，包括：

①remApi.m ②remoteApiProto.m

③選擇作業系統相同的檔案操作檔案: “remoteApi.dll” (Windows), “remoteApi.dylib” (Mac) or “remoteApi.so” (Linux) ④simpleTest.m (or any other example program)

Vrep安裝檔案夾->programming->remoteApiBindings->

(1)->lib->lib->(剩下的根據電腦的作業系統和matlab的安裝位數來選擇)

(2)->matlab->matlab->剩下的對比上述檔案選擇

将這些檔案複制到另一個自己定義的檔案夾中（建議放置在matlab安裝路徑的toolbox檔案夾下），最後再把路徑添加到matlab的path中。

剩下的如何啟動，如何擷取場景中的句柄，如何設定關節的力矩等将會在第3點中講到。

3. 進行力矩控制

• Vrep的配置。在simulation settings中設定仿真時間步為5ms。關于時間不user manual中多次提到了預設情況下實體引擎的執行頻率是仿真周期頻率的10倍，而預設情況下仿真周期是50ms，也就是說，實體引擎的調用周期是5ms。這樣一來，我們通過matlab腳本對Vrep進行同步控制，其控制周期就隻能夠是50ms，這樣的控制頻率是遠遠不夠的，是以我們需要人為地将仿真周期改為5ms，與實體引擎同步，進而實作力矩控制。可以在simulation->simulation settings中修改，又或者是在快捷菜單中修改。選擇costum模式，然後改為0.005。

  

  • 關節的配置方面。

    mode選擇為torque/force，dynamic properties中check “motor enable”，最大力矩改為50N，其實這裡多少都沒有太大的關系，也可以設定為零，因為後續在控制腳本中會有所改動。

    

  • matlab控制腳本的書寫

    下面我先将整個代碼貼上來，有興趣的可以仔細看一遍，沒興趣的自己複制粘貼一下然後調一下pd參數，看看運作的效果。（emmm，雖然注釋都是英文，那是為了粘貼複制的時候不亂嗎，代碼和注釋都是我自己寫的，就不要問出處了）

%% Configuration codes 
jointNum=;    baseName='Base';
torqueLimit=[,];
jointName='Joint';  
displayOn=false;
torque=zeros(jointNum,); qd=zeros(jointNum,); intE=zeros(jointNum,);
E=[]; Q=[];  DQ=[]; QD=[]; DQD=[]; TAU=[];

%% Connect to the Vrep
% 1. load api library
vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
% 2. close all the potential link
vrep.simxFinish(-);   
% 3. wait for connecting vrep, detect every 0.2s
while true
    clientID=vrep.simxStart('127.0.0.1',,true,true,,);
    if clientID>- 
        break;
    else
        pause();
        disp('please run the simulation on vrep...')
    end
end
disp('Connection success!')
% 4. set the simulation time step
tstep = ;  % 5ms per simulation pass
vrep.simxSetFloatingParameter(clientID,vrep.sim_floatparam_simulation_time_step,tstep,vrep.simx_opmode_oneshot);
% 5. open the synchronous mode to control the objects in vrep
vrep.simxSynchronous(clientID,true);

%% Simulation Initialization
vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot);
% get the handles
jointHandle=zeros(jointNum,); 
for i=:jointNum  % handles of the left and right arms
    [~,returnHandle]=vrep.simxGetObjectHandle(clientID,[jointName,int2str(i)],vrep.simx_opmode_blocking);
    jointHandle(i)=returnHandle;
end
[~,baseHandle]=vrep.simxGetObjectHandle(clientID,baseName,vrep.simx_opmode_blocking);  % reference coordinates of the arms'
vrep.simxSynchronousTrigger(clientID);
disp('Handles available!')
% first call to read the joints' configuration and end-effector pose, the mode is chosen to be simx_opmode_streaming
jointConfig=zeros(jointNum,); 
for i=:jointNum
    [~,jpos]=vrep.simxGetJointPosition(clientID,jointHandle(i),vrep.simx_opmode_streaming);
    jointConfig(i)=jpos;
end

for i=:jointNum
    vrep.simxSetJointTargetVelocity(clientID,jointHandle(i),,vrep.simx_opmode_oneshot);
    vrep.simxSetJointForce(clientID,jointHandle(i),,vrep.simx_opmode_oneshot);
end
% get simulation time
currCmdTime=vrep.simxGetLastCmdTime(clientID); 
tipPosLast=tipPos;        
tipOrtLast=tipOrt;
lastCmdTime=currCmdTime;
jointConfigLast=jointConfig;

vrep.simxSynchronousTrigger(clientID);         % every calls this function, verp is triggered, 50ms by default

%% Simulation Start
t=;  % start the simulation
while (vrep.simxGetConnectionId(clientID) ~= -)  % vrep connection is still active
    % 0. time update
    currCmdTime=vrep.simxGetLastCmdTime(clientID);
    dt=(currCmdTime-lastCmdTime)/;              % simulation step, unit: s 
    % 1. read the joints' and tips' configuration (position and velocity)
    for i=:jointNum
        [~,jpos]=vrep.simxGetJointPosition(clientID,jointHandle(i),vrep.simx_opmode_buffer);
        jointConfig(i)=jpos;
    end
    % using Finit Difference Method to get the velocity of joints' configuration (a simple version)
    q=jointConfig;       dq=(q-jointConfigLast)./dt;  % column vector
    % 2. display the acquisition data, or store them if plotting is needed.
    if displayOn==true
        disp('joints config:');       disp(q'.*/pi);
        disp('joints dconfig:');      disp(dq'.*/pi);
    end
    % 3. control alorithm can write down here
    % 1.pid controller
    %   3.1 feedback signal q,dq,c,dc, more importantly input the reference signal
    qd=[sin(t),cos(t)]';  dqd=[cos(t),-sin(t)]';
    qe=qd-q; dqe=dqd-dq;
    %   3.2 actuatation calculation
    Kp=[,;,];
    Kd=[,;,];
    Ki=[ ; ];
    intE=intE+qe.*dt;
    torque=ddqd+Kp*qe+Kd*dqe+Ki*intE;

    %   3.3 set the torque in vrep way
    for i=:jointNum
        if sign(torque(i))<
            setVel=-; % set a trememdous large velocity for the screwy operation of the vrep torque control implementaion
            if torque(i)<-torqueLimit(i)
                setTu=-torqueLimit(i);
            else
                setTu=-torque(i);
            end
        else
            setVel=;
            if torque(i)>torqueLimit(i)
                setTu=torqueLimit(i);
            else
                setTu=torque(i);
            end
        end
        vrep.simxSetJointTargetVelocity(clientID,jointHandle(i),setVel,vrep.simx_opmode_oneshot);
        vrep.simxSetJointForce(clientID,jointHandle(i),setTu,vrep.simx_opmode_oneshot);
    end

    % data recording for plotting
    E=[E qe];    Q=[Q q];        QD=[QD qd];
    DQ=[DQ dq];  DQD=[DQD dqd];  TAU=[TAU torque];

    % 4. update vrep(the server side)
    tipPosLast=tipPos;       
    tipOrtLast=tipOrt;
    lastCmdTime=currCmdTime;
    jointConfigLast=jointConfig;    
    vrep.simxSynchronousTrigger(clientID);
    t=t+dt; % updata simulation time
end
vrep.simxFinish(-);  % close the link
vrep.delete();        % destroy the 'vrep' class

%%
figure(); hold off;
subplot(); plot(E(,:),'r'); hold on; plot(E(,:),'b'); title('error')
subplot(); plot(Q(,:),'r'); hold on; plot(Q(,:),'b');  plot(QD(,:),'k'); hold on; plot(QD(,:),'k'); title('trajectory')
subplot(); plot(DQ(,:),'r'); hold on; plot(DQ(,:),'b');  plot(DQD(,:),'k'); hold on; plot(DQD(,:),'k'); title('velocity')
subplot(); plot(TAU(,:),'r'); hold on; plot(TAU(,:),'b'); title('torque')
           

代碼的控制架構就是

①首先連接配接vrep，這裡使用了死循環等待連接配接。

②擷取Vrep仿真周期作為dt，擷取vrep場景中的句柄，主要是擷取關節句柄，初始化資料流。

③構造一個大循環，檢測到連接配接斷開之後就跳出循環

④在大循環中，接受場景中關節的位置資訊（這裡隻能讀取到關節位置資訊，類似于機械臂中隻讀取到編碼器的數值，沒有速度資訊，速度資訊要自行計算獲得），利用有限差分的方法計算得到速度資訊。然後通過控制器代碼來計算應該輸出的控制力矩。最後加一個限幅，輸出力矩指令給Vrep場景中的關節，控制機械臂運動。

⑤跳出循環後對采集到的信号作圖

這裡隻是為了驗證設計的控制器做的一個很粗糙的架構，不用太過較真，真正要完成一個外部控制器控制的任務的時候，可以在這個架構下細化。

暫且不看其他東西，我們這裡首要亟需解決的問題是，如何通過腳本将力矩傳到vrep中，這也是這篇博文的精華所在。當然前面也是精華，但是沒有這個精。

在論壇上，這個問題被提出過幾次。實體引擎并沒有直接提供函數接口給力矩控制使用。 隻提供了兩個意味深長的函數接口；①設定最大力矩函數；②目标速度設定。這兩個函數是這樣的，我們可以在user manual中對這兩個函數的描述中，了解這兩個函數之間的關聯：當物體沒有達到目标速度的時候，輸出力矩将等于最大可輸出力矩。當達到了目标速度的時候，我不記得了，因為不太重要。這種條件設定跟bang-bang控制有點像，着實有點意味深長。然而，利用這種規則，我們可以構造出力矩控制所需要的方式：給定關節多少力矩，他就輸出多少力矩。注意：這裡輸出的力矩是沒有方向的，方向需要目标速度給定。是以，我們需要設定一個非常大的不可能達到的速度給Vrep的關節，并且根據力矩的正負相應地給不可能達到的速度添加正負号，然後，在安安心心地setforce給關節。

主要用到的兩個函數如下：

vrep.simxSetJointTargetVelocity(clientID,jointHandle,set_velocity,vrep.simx_opmode_oneshot);
vrep.simxSetJointForce(clientID,jointHandle,set_force,vrep.simx_opmode_oneshot);
           

4. 檢視控制效果

運作的方式是，先運作matlab腳本，然後再打開vrep。或者換個順序也行。然後就能夠看到機械臂在動了。想要結束運作的話，可以直接停止vrep裡面的仿真，然後matlab自己自動就停了還會自動作圖。當然顯示目前控制性能也可以在vrep中增加graph實時地顯示曲線，單不是本博文的主要目的，是以在這裡就不教大家了。最後控制的運動效果如下圖所示：

結束語

為了節省時間，沒有仔細排版，大家随意享用。

D. Huang

2018年6月9日

寫于華工