V-rep中的加速度計與陀螺儀
阿新 • • 發佈：2017-12-27
gets 函數 nal result cte 物體 publisher gettime mage 
加速度計(Accelerometer)
VREP的模型瀏覽器components→sensors中可以找到加速度計的模型，用於測量物體沿著世界坐標系三個坐標軸的加速度值。
VREP中沒有直接測量加速度的函數，可以間接地通過測量已知質量物體上的力來計算加速度。加速度計的結構如下圖所示，其中動態物體Accelerometer_mass的質量為1g，通過函數讀取力傳感器測量的力的大小，可以計算出物體Accelerometer_mass的加速度。
加速度計Accelerometer的腳本代碼如下：
-- Check the end of the script for some explanations!View Codeif (sim_call_type==sim_childscriptcall_initialization) then modelBase=simGetObjectAssociatedWithScript(sim_handle_self) massObject=simGetObjectHandle(‘Accelerometer_mass‘) sensor=simGetObjectHandle(‘Accelerometer_forceSensor‘) result,mass=simGetObjectFloatParameter(massObject,sim_shapefloatparam_mass) ui=simGetUIHandle(‘Accelerometer_UI‘) simSetUIButtonLabel(ui,0,simGetObjectName(modelBase)) accelCommunicationTube=simTubeOpen(0,‘accelerometerData‘..simGetNameSuffix(nil),1) end if (sim_call_type==sim_childscriptcall_cleanup) then end if (sim_call_type==sim_childscriptcall_sensing) then result,force=simReadForceSensor(sensor) if (result>0) then accel={force[1]/mass,force[2]/mass,force[3]/mass} simTubeWrite(accelCommunicationTube,simPackFloatTable(accel)) simSetUIButtonLabel(ui,3,string.format("X-Accel: %.4f",accel[1])) simSetUIButtonLabel(ui,4,string.format("Y-Accel: %.4f",accel[2])) simSetUIButtonLabel(ui,5,string.format("Z-Accel: %.4f",accel[3])) else simSetUIButtonLabel(ui,3,"X-Accel: -") simSetUIButtonLabel(ui,4,"Y-Accel: -") simSetUIButtonLabel(ui,5,"Z-Accel: -") end -- To read data from this accelerometer in another script, use following code: -- -- accelCommunicationTube=simTubeOpen(0,‘accelerometerData‘..simGetNameSuffix(nil),1) -- put this in the initialization phase -- data=simTubeRead(accelCommunicationTube) -- if (data) then -- acceleration=simUnpackFloatTable(data) -- end -- -- If the script in which you read the acceleration has a different suffix than the accelerometer suffix, -- then you will have to slightly adjust the code, e.g.: -- accelCommunicationTube=simTubeOpen(0,‘accelerometerData#‘) -- if the accelerometer script has no suffix -- or -- accelCommunicationTube=simTubeOpen(0,‘accelerometerData#0‘) -- if the accelerometer script has a suffix 0 -- or -- accelCommunicationTube=simTubeOpen(0,‘accelerometerData#1‘) -- if the accelerometer script has a suffix 1 -- etc. -- -- -- You can of course also use global variables (not elegant and not scalable), e.g.: -- In the accelerometer script: -- simSetFloatSignal(‘accelerometerX‘,accel[1]) -- simSetFloatSignal(‘accelerometerY‘,accel[2]) -- simSetFloatSignal(‘accelerometerZ‘,accel[3]) -- -- And in the script that needs the data: -- xAccel=simGetFloatSignal(‘accelerometerX‘) -- yAccel=simGetFloatSignal(‘accelerometerY‘) -- zAccel=simGetFloatSignal(‘accelerometerZ‘) -- -- In addition to that, there are many other ways to have 2 scripts exchange data. Check the documentation for more details end
拖入一個加速度計到空場景中，開始仿真，可以看到Z軸方向加速度為-9.81m/s2（重力加速度沿著Z軸負方向）：
在rviz中可以添加Imu類型的數據顯示加速度（rviz_plugin_tutorials中的Imu類型只能顯示加速度的大小和方向）
將加速度計安裝在一個緩慢移動的小車上，rviz中顯示的加速度如下圖所示：
陀螺儀(GyroSensor)
VREP中陀螺儀可以測量運動物體絕對的角速度
陀螺儀模型的代碼如下：
-- Check the end of the script for some explanations! if (sim_call_type==sim_childscriptcall_initialization) then modelBase=simGetObjectAssociatedWithScript(sim_handle_self) ref=simGetObjectHandle(‘GyroSensor_reference‘) ui=simGetUIHandle(‘GyroSensor_UI‘) simSetUIButtonLabel(ui,0,simGetObjectName(modelBase)) gyroCommunicationTube=simTubeOpen(0,‘gyroData‘..simGetNameSuffix(nil),1) oldTransformationMatrix=simGetObjectMatrix(ref,-1) lastTime=simGetSimulationTime() end if (sim_call_type==sim_childscriptcall_cleanup) then end if (sim_call_type==sim_childscriptcall_sensing) then local transformationMatrix=simGetObjectMatrix(ref,-1) local oldInverse=simGetInvertedMatrix(oldTransformationMatrix) local m=simMultiplyMatrices(oldInverse,transformationMatrix) local euler=simGetEulerAnglesFromMatrix(m) -- Retrieves the Euler angles from a transformation matrix local currentTime=simGetSimulationTime() local gyroData={0,0,0} local dt=currentTime-lastTime if (dt~=0) then gyroData[1]=euler[1]/dt gyroData[2]=euler[2]/dt gyroData[3]=euler[3]/dt end simTubeWrite(gyroCommunicationTube,simPackFloatTable(gyroData)) simSetUIButtonLabel(ui,3,string.format("X-Gyro: %.4f",gyroData[1])) simSetUIButtonLabel(ui,4,string.format("Y-Gyro: %.4f",gyroData[2])) simSetUIButtonLabel(ui,5,string.format("Z-Gyro: %.4f",gyroData[3])) oldTransformationMatrix=simCopyMatrix(transformationMatrix) lastTime=currentTime -- To read data from this gyro sensor in another script, use following code: -- -- gyroCommunicationTube=simTubeOpen(0,‘gyroData‘..simGetNameSuffix(nil),1) -- put this in the initialization phase -- data=simTubeRead(gyroCommunicationTube) -- if (data) then -- angularVariations=simUnpackFloatTable(data) -- end -- -- If the script in which you read the gyro sensor has a different suffix than the gyro suffix, -- then you will have to slightly adjust the code, e.g.: -- gyroCommunicationTube=simTubeOpen(0,‘gyroData#‘) -- if the gyro script has no suffix -- or -- gyroCommunicationTube=simTubeOpen(0,‘gyroData#0‘) -- if the gyro script has a suffix 0 -- or -- gyroCommunicationTube=simTubeOpen(0,‘gyroData#1‘) -- if the gyro script has a suffix 1 -- etc. -- -- -- You can of course also use global variables (not elegant and not scalable), e.g.: -- In the gyro script: -- simSetFloatSignal(‘gyroX‘,angularVariation[1]) -- simSetFloatSignal(‘gyroY‘,angularVariation[2]) -- simSetFloatSignal(‘gyroZ‘,angularVariation[3]) -- -- And in the script that needs the data: -- angularVariationX=simGetFloatSignal(‘gyroX‘) -- angularVariationY=simGetFloatSignal(‘gyroY‘) -- angularVariationZ=simGetFloatSignal(‘gyroZ‘) -- -- In addition to that, there are many other ways to have 2 scripts exchange data. Check the documentation for more details endView Code
VREP中兩個entity之間進行通信（交換數據）有多種方式，具體可以參考：Means of communication in and around V-REP. 在VREP的陀螺儀模型中用到了tube通信方式。Tubes are bidirectional communication lines similar to pipes. The tube denomination was selected in order to avoid confusion with pipes, since tubes cannot be used to communicate with the outside world. Tubes are a very convenient and easy means to connecting two entities and exchanging information in a sequential way. They are often used between a child script and a server-like threaded communication child script (e.g. where the latter could handle socket or serial port communication).
Tube通信相關的函數如下：
-- Opens a tube for communication within V-REP. Messages written on one side can be read on the other side in the same order as they were written.
-- Tubes opened via a script will automatically close upon simulation end.
number tubeHandle = simTubeOpen(number dataHeader, string dataName, number readBufferSize)
-- Sends a data packet into a communication tube previously opened with simTubeOpen
number result = simTubeWrite(number tubeHandle, string data)
-- Receives a data packet from a communication tube previously opened with simTubeOpen.
string data = simTubeRead(number tubeHandle, boolean blockingOperation=false)
VREP中歐拉角$\alpha$、$\beta$、$\gamma$描述了剛體相對於世界坐標系的姿態：$$Q=R_x(\alpha)\cdot R_y(\beta)\cdot R_z(\gamma)$$其中$R_x$、$R_y$、$R_z$分別代表繞X、Y、Z軸的旋轉。在V-rep中物體姿態以X-Y-Z歐拉角的方式確定（以指定的參考系為初始姿態，然後按X-Y-Z的順序依次繞自身的坐標軸旋轉Alpha,Beta,Gamma角度後得到）。
腳本中計算角速度的方法如下：假設$t-1$時刻物體姿態為$_{t-1}^{0}R$，在$dt$時間段內以恒定的角速度旋轉到姿態$_{t}^{0}R$，則有$_{t}^{0}R=_{t-1}^{0}R\cdot R(\alpha,\beta,\gamma)$，於是可以求出相對變換矩陣為：$_{t-1}^{0}R^{-1} \cdot _{t}^{0}R$。註意：若繞靜坐標系（世界坐標系）旋轉，則左乘；若是繞動坐標系旋轉（自身坐標系），則右乘 。
下面測試一個簡單的場景：創建一個旋轉的立方體，將GyroSensor拖到其下方成為子對象。那麽立方體旋轉時會帶著陀螺儀一起旋轉，進而可以測出物體旋轉的角速度。
立方體的代碼如下，代碼會讓立方體的$\gamma$角每50ms增加0.02弧度，即繞Z軸旋轉角速度為0.4rad/s
if (sim_call_type==sim_childscriptcall_initialization) then -- Put some initialization code here handle=simGetObjectHandle(‘Cuboid‘) gyroCommunicationTube=simTubeOpen(0,‘gyroData‘..simGetNameSuffix(nil),1) end if (sim_call_type==sim_childscriptcall_actuation) then -- Put your main ACTUATION code here local pos=simGetObjectOrientation(handle,-1) pos[3]=pos[3]+0.02 simSetObjectOrientation(handle,-1,pos) end if (sim_call_type==sim_childscriptcall_sensing) then -- Put your main SENSING code here data=simTubeRead(gyroCommunicationTube) if (data) then angularVariations=simUnpackFloatTable(data) simAddStatusbarMessage(string.format("alpha:%.1f beta:%.1f yaw:%.1f", angularVariations[1],angularVariations[2],angularVariations[3])) end endView Code
可以將加速度計和陀螺儀組合起來使用，比如將獲得的數據封裝成ROS中的sensor_msgs/Imu消息發送出去。在rviz中添加rviz_imu_plugin下的IMU類型信進行顯示（可以顯示代表物體姿態的Box、坐標軸，以及顯示加速度）：
將加速度計、陀螺儀安裝到下面的移動機器人上，輸出IMU信息（下圖中白色軸代表加速度信息）：
VREP中發布ROS IMU消息的代碼如下：
if (sim_call_type==sim_childscriptcall_initialization) then -- Put some initialization code here pub = simExtRosInterface_advertise(‘/imu‘, ‘sensor_msgs/Imu‘) simExtRosInterface_publisherTreatUInt8ArrayAsString(pub) Imu_data={} gyroCommunicationTube=simTubeOpen(0,‘gyroData‘..simGetNameSuffix(nil),1) accelCommunicationTube=simTubeOpen(0,‘accelerometerData‘..simGetNameSuffix(nil),1) handle = simGetObjectHandle(‘lumibot_body‘) end if (sim_call_type==sim_childscriptcall_actuation) then end if (sim_call_type==sim_childscriptcall_sensing) then -- Put your main SENSING code here quaternion = simGetObjectQuaternion(handle, -1) accele_data=simTubeRead(accelCommunicationTube) gyro_data=simTubeRead(gyroCommunicationTube) if (accele_data and gyro_data) then acceleration=simUnpackFloatTable(accele_data) angularVariations=simUnpackFloatTable(gyro_data) Imu_data[‘orientation‘] = {x=quaternion[1],y=quaternion[2],z=quaternion[3],w=quaternion[4]} Imu_data[‘header‘]={seq=0,stamp=simExtRosInterface_getTime(), frame_id="sensor_frame"} Imu_data[‘linear_acceleration‘]= {x=acceleration[1],y=acceleration[2],z=acceleration[3]} Imu_data[‘angular_velocity‘] = {x=angularVariations[1],y=angularVariations[2],z=angularVariations[3]} simExtRosInterface_publish(pub, Imu_data) end end if (sim_call_type==sim_childscriptcall_cleanup) then -- Put some restoration code here simExtRosInterface_shutdownPublisher(pub) endView Code
參考：
sensor_msgs/Imu Message
V-rep學習筆記：力傳感器
ROS中發布IMU傳感器消息
三軸加速度傳感器原理及應用
加速度傳感器檢測物體傾角的原理
Means of communication in and around V-REP
V-rep中的加速度計與陀螺儀