Gyrosensor and Python

Typically: "How do I... ", "How can I... " questions
Post Reply
LeafTeaNeko
Posts: 3
Joined: 17 May 2020, 11:38

Gyrosensor and Python

Post by LeafTeaNeko »

Python version: 3.7

Hello, I am new at V-REP. I want to use the gyro sensor and accelerometer with python. In CoppeliaSim, I have the Gyro Sensor and accelerometer with its default script attached to my robot. I start and run the simulation from CoppeliaSim and the python script simply reads incoming data.
I've tired to follow along other examples listed in the forums and this is my script attached to gyrosensor

Code: Select all

function sysCall_init() 
    modelBase=sim.getObjectAssociatedWithScript(sim.handle_self)
    ref=sim.getObjectHandle('GyroSensor_reference')
    ui=simGetUIHandle('GyroSensor_UI')
    simSetUIButtonLabel(ui,0,sim.getObjectName(modelBase))
    gyroCommunicationTube=sim.tubeOpen(0,'gyroData'..sim.getNameSuffix(nil),1)
    oldTransformationMatrix=sim.getObjectMatrix(ref,-1)
    lastTime=sim.getSimulationTime()
end

function sysCall_cleanup() 
end 

function sysCall_sensing() 
    local transformationMatrix=sim.getObjectMatrix(ref,-1)
    local oldInverse=simGetInvertedMatrix(oldTransformationMatrix)
    local m=sim.multiplyMatrices(oldInverse,transformationMatrix)
    local euler=sim.getEulerAnglesFromMatrix(m)
    local currentTime=sim.getSimulationTime()
    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
    sim.tubeWrite(gyroCommunicationTube,sim.packFloatTable(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=sim.copyMatrix(transformationMatrix)
    lastTime=currentTime
end 

function getGyroData(inInts,inFloats,inStrings,inBuffer)
    return {},gyroData,{},''
end
and this is my python code:

Code: Select all

import sim as vrep

vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)

ERRORCODE, gyroRef = vrep.simxGetObjectHandle(clientID,'GyroSensor#0',vrep.simx_opmode_blocking)

if clientID != -1:
    ERRORCODE, gyro_data = vrep.simxGetFloatSignal(clientID, gyroRef, vrep.simx_opmode_streaming)
    while vrep.simxGetConnectionId(clientID) != -1:
        ERRORCODE, gyro_data = vrep.simxGetFloatSignal(clientID, gyroRef, vrep.simx_opmode_buffer)
	print(gyro_data)
else:
    vrep.simxFinish(clientID)
However I get the following error:
Traceback (most recent call last):

File "C:\Users\XXXX\Desktop\py-vrep\python-vrep.py", line 27, in <module>
ERRORCODE, gyro_data = vrep.simxGetFloatSignal(clientID, gyroRef, vrep.simx_opmode_streaming)

File "C:\Users\XXXX\Desktop\py-vrep\sim.py", line 948, in simxGetFloatSignal
return c_GetFloatSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value

ArgumentError: argument 2: <class 'TypeError'>: wrong type
A sample script that prints the data from gyro and accelerometer in python using the Remote API would be helpful to me. I tried to follow along the post https://forum.coppeliarobotics.com/viewtopic.php?t=8233 but the b0 API is not working for some reason.
I get the same errors as in the post, I edited the b0RemoteApi.py as suggested but that did not help either. I looked around the internet for possible answers but apparently this issue is persistent with python version lower than 3.8.

Answers similar to the post: https://forum.coppeliarobotics.com/vie ... yro#p33065 and would be incredibly helpful.
Thank you for your time and understanding, have a good day.

Edit: I've looked around and I've found a simxcallscriptfunction that I can use from python, Does that mean If I have a script in my simulation which prints all the values of accel and gryo sensor, I could essentially skip all the legwork and directly get the values from script?

coppelia
Site Admin
Posts: 7838
Joined: 14 Dec 2012, 00:25

Re: Gyrosensor and Python

Post by coppelia »

Hello,

sorry for the late reply. Yes, simxCallScriptFunction is basically what you would want to use, that simplifies things a lot. Just make sure that the called script function doesn't block. Typically you will call simple function inside of it, and return data. The simxCallScriptFunction can also be called in a streaming fashion, so that you can constantly retrieve data from that function, without having to call it constantly.

Cheers

LeafTeaNeko
Posts: 3
Joined: 17 May 2020, 11:38

Re: Gyrosensor and Python

Post by LeafTeaNeko »

Hello Coppeliateam, thank you for the response.
I have 5 gyros and 5 accelormeters and this script is attached to a random cuboid in my scene.
Will something like this work?

Code: Select all

function sysCall_init()
    -- do some initialization here
    simRemoteApi.start(19999)
  sim.clearStringSignal('stringname') 
   s=0
    lastReceivedFX=-1
    lastReceivedPos=-1
end

function sysCall_actuation()
packedFX=sim.getStringSignal('stringname')
            if packedFX then 
                sim.clearStringSignal('stringname')
                packedFX=sim.unpackFloatTable(packedFX)
           x1=packedFX[1]
           y1=packedFX[2]
           angle1=packedFX[3]
           x2=packedFX[4]
           y2=packedFX[5]
           angle2=packedFX[6]
           x3=packedFX[7]
           y3=packedFX[8]
           angle3=packedFX[9]
           x4=packedFX[10]
           y4=packedFX[11]
           angle4=packedFX[12]
           x5=packedFX[13]
           y5=packedFX[14]
           angle5=packedFX[15]
           packedFX1={x1,y1,angle1}
           packedFX2={x2,y2,angle2}
           packedFX3={x3,y3,angle3}
           packedFX4={x4,y4,angle4}
           packedFX5={x5,y5,angle5}
           sim.setStringSignal("copysignal",sim.packFloatTable(packedFX1))
           sim.setStringSignal("copysignal#0",sim.packFloatTable(packedFX2))
           sim.setStringSignal("copysignal#1",sim.packFloatTable(packedFX3))
           sim.setStringSignal("copysignal#2",sim.packFloatTable(packedFX4))
           sim.setStringSignal("copysignal#3",sim.packFloatTable(packedFX5))
            end
  --accuring gyro and acc data
     local data=sim.getStringSignal("myGyroData")
if data then
    gyroData=sim.unpackFloatTable(data)
    g1=gyroData[1]
     g2=gyroData[2]
     g3=gyroData[3]
end
     local data=sim.getStringSignal("myaccData")
if data then
    accelData=sim.unpackFloatTable(data)
    a1=accelData[1]
    a2=accelData[2]
    a3=accelData[3]
end  
      local data=sim.getStringSignal("myGyroData#0")
if data then
    gyroData=sim.unpackFloatTable(data)
    g4=gyroData[1]
     g5=gyroData[2]
     g6=gyroData[3]
end
     local data=sim.getStringSignal("myaccData#0")
if data then
    accelData=sim.unpackFloatTable(data)
    a4=accelData[1]
    a5=accelData[2]
    a6=accelData[3]
    
     local data=sim.getStringSignal("myGyroData#1")
if data then
    gyroData=sim.unpackFloatTable(data)
    g7=gyroData[1]
     g8=gyroData[2]
     g9=gyroData[3]
end
     local data=sim.getStringSignal("myaccData#1")
if data then
    accelData=sim.unpackFloatTable(data)
    a7=accelData[1]
    a8=accelData[2]
    a9=accelData[3]
end  
 local data=sim.getStringSignal("myGyroData#2")
if data then
    gyroData=sim.unpackFloatTable(data)
    g10=gyroData[1]
     g11=gyroData[2]
     g12=gyroData[3]
end
     local data=sim.getStringSignal("myaccData#2")
if data then
    accelData=sim.unpackFloatTable(data)
    a10=accelData[1]
    a11=accelData[2]
    a12=accelData[3]
end  
 local data=sim.getStringSignal("myGyroData#3")
if data then
    gyroData=sim.unpackFloatTable(data)
    g13=gyroData[1]
     g14=gyroData[2]
     g15=gyroData[3]
end
     local data=sim.getStringSignal("myaccData#3")
if data then
    accelData=sim.unpackFloatTable(data)
    a13=accelData[1]
    a14=accelData[2]
    a15=accelData[3]
end  
end  
     
     
   packetgyroandacc={g1,g2,g3,g4,g5,g6,g7,g8,g9,g10,g11,g12,g13,g14,g15,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13,a14,a15}
 -- px={g1,g2,g3,g4,g5,g6}
print(packetgyroandacc)
sim.setStringSignal("packetgyroandacc",sim.packFloatTable(packetgyroandacc))
return packetgyroandacc

end

function sysCall_sensing()
    -- put your sensing code here
end

function sysCall_cleanup()
    -- do some clean-up here
end
and python code would be

Code: Select all

import sim as vrep

vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)

if clientID != -1:
    while vrep.simxGetConnectionId(clientID) != -1:
	returnCode, outFloats=simxCallScriptFunction(clientID, cuboid, vrep.sim_scripttype_childscript, 'sysCall_actuation',vrep.simx_opmode_blocking)
	print(outFloats)
else:
    vrep.simxFinish(clientID)

coppelia
Site Admin
Posts: 7838
Joined: 14 Dec 2012, 00:25

Re: Gyrosensor and Python

Post by coppelia »

no this will not work: you should not call a system callback function (e.g. sysCall_actuation) from an external application. Create your own function and have that function collect and return the data you wish.

Typically, read your sensor data in the sensing callback script (sysCall_sensing), and make it available to your own script: your script function will simply return what was previously read via the sensing callback function.

Of course, you could also directly read the string signals from Python (simxGetStringSignal) and decode them with simxUnpackFloats.

Cheers

LeafTeaNeko
Posts: 3
Joined: 17 May 2020, 11:38

Re: Gyrosensor and Python

Post by LeafTeaNeko »

THANK YOU SO MUCH! simxGetStringSignal and simxUnpackFloats works flawlessly. That was way simpler than I thought, I was doing something entirely unnecesssary. Thank you again CoppeliaSim team, have yourself a great day!

The problem was that our project was a collabarative effort and I'm not on the team that works on Coppeliasim, I simply use the data they send over in python and run them through a couple filters. As a sidenote, Does CoppeliaSim have a wiki for sample codes for sensors in python? or Would you guys be intrested in user-made scripts submitted to said wiki?

coppelia
Site Admin
Posts: 7838
Joined: 14 Dec 2012, 00:25

Re: Gyrosensor and Python

Post by coppelia »

we don't yet have a wiki, but are always happy to get code snippets, models, scenes, etc. that we try to integrate into the user manual and/or next release.

Cheers

Post Reply