加速度计(Accelerometer) 

  VREP的模型浏览器components→sensors中可以找到加速度计的模型,用于测量物体沿着世界坐标系三个坐标轴的加速度值。

VREP中没有直接测量加速度的函数,可以间接地通过测量已知质量物体上的力来计算加速度。加速度计的结构如下图所示,其中动态物体Accelerometer_mass的质量为1g,通过函数读取力传感器测量的力的大小,可以计算出物体Accelerometer_mass的加速度。

加速度计Accelerometer的脚本代码如下:

-- Check the end of the script for some explanations!

if (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
end

 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 simTubeOpennumber 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中欧拉角α、β、γ描述了刚体相对于世界坐标系的姿态:

 下面测试一个简单的场景:创建一个旋转的立方体,将GyroSensor拖到其下方成为子对象。那么立方体旋转时会带着陀螺仪一起旋转,进而可以测出物体旋转的角速度。

 立方体的代码如下,代码会让立方体的γ角每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

end

可以将加速度计和陀螺仪组合起来使用,比如将获得的数据封装成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)

end

参考:

sensor_msgs/Imu Message

V-rep学习笔记:力传感器

ROS中发布IMU传感器消息

三轴加速度传感器原理及应用

加速度传感器检测物体倾角的原理

Means of communication in and around V-REP