Get orientation and use it for a specific task

Typically: "How do I... ", "How can I... " questions
DavideIng
Posts: 39
Joined: 21 Sep 2017, 17:27

Get orientation and use it for a specific task

Hello,
I am working with an ePuck, the task is:
When the conditions about the components of ImageTable_rgb (as you can see in the code below) are true, the ePuck has to be rotated of 90 degrees. To do that I am using the following code:

Code: Select all

ImageTable_rgb=simGetVisionSensorImage(handleVision,0,0,0)
Orientation = simGetObjectOrientation(ePuck,-1)
if (ImageTable_rgb [1]>0.5) and (ImageTable_rgb [2] < 0.2) and (ImageTable_rgb [3] < 0.2)then
while (Orientation[1] <= Orientation[1] + 1.57) or (Orientation[1] >= Orientation[1] - 1.57) do
simSetJointTargetVelocity(leftMotor,-math.pi*0.5)
simSetJointTargetVelocity(rightMotor,math.pi*0.5)
Orientation = simGetObjectOrientation(ePuck,-1)
simWait(0.5)
end
end

It doesn't work.
I am not sure about the following things:

1) I need to know which is the component of the vector Orientation (Euler Angle) refeared to absolute Z axes rotation.
2)I am not sure about the values retrieved by the function simGetObjectOrientation, are they in radians? Because I am considering the while condition in radians but seems to be far to my target.

Any suggestions?
Many thanks

Davide

coppelia
Posts: 7857
Joined: 14 Dec 2012, 00:25

Re: Get orientation and use it for a specific task

Hello Davide,

this is because the e-puck model base has a default orientation of {-pi/2,0,-pi/2} by default (i.e. x-component of the e-puck orientation frame is pointing up, the z-component of e-puck orientation frame is pointing to the forward movement direction). To overcome this you have two options:

1. you can reorient the base of the e-puck to have its reference frame z-component to point up, and y-component to point to the forward direction: select the base of the e-puck, the [Right-click --> edit --> reorient bounding box --> with reference frame of worl]. Once that is done, you can get the desired orientation of the e-puck with:

Code: Select all

    local e=simGetObjectOrientation(model,-1)
-- e[3] is the desired orientation of the e-puck, expressed in radians
2. or you can directly get the desired orientation with following:

Code: Select all

    model=simGetObjectAssociatedWithScript(sim_handle_self) -- e-puck model handle
local m=simGetObjectMatrix(model,-1)
local m2=simBuildMatrix({0,0,0},{0,90*math.pi/180,90*math.pi/180})
m=simMultiplyMatrices(m,m2)
local e=simGetEulerAnglesFromMatrix(m)
-- e[3] is the desired orientation of the e-puck, expressed in radians
Cheers

DavideIng
Posts: 39
Joined: 21 Sep 2017, 17:27

Re: Get orientation and use it for a specific task

Now I get the desired angle but my algorithm is still not working.
The basic idea is:

1) At the begining ''ImageTable_rgb'' is white, afther 10 seconds (simulation time) it became red and the if statement conditions became true
2)The orientation of the ePuck (i.e e[3]) now is around 0 radiant, therefore the while loop conditions is true.
3)Now the ePuck will start to turn until e[3] is equal to pi/2 (i.e 1.57 rad)
4)Ones out of while statement the ePuck has to go straight

I am using to do that the following code:

Code: Select all

ImageTable_rgb=simGetVisionSensorImage(Camera,0,0,0)
local m=simGetObjectMatrix(ePuck,-1)
local m2=simBuildMatrix({0,0,0},{0,90*math.pi/180,90*math.pi/180})
m=simMultiplyMatrices(m,m2)
local e=simGetEulerAnglesFromMatrix(m)
if (ImageTable_rgb [1] > 0.5) and (ImageTable_rgb [2] < 0.2) and (ImageTable_rgb [3] < 0.2) then
while(e[3] < math.pi/2) do
simSetJointTargetVelocity(leftMotor,-maxVel*0.5)
simSetJointTargetVelocity(rightMotor,maxVel*0.5)
local m=simGetObjectMatrix(ePuck,-1)
local m2=simBuildMatrix({0,0,0},{0,90*math.pi/180,90*math.pi/180})
m=simMultiplyMatrices(m,m2)
local e=simGetEulerAnglesFromMatrix(m)
end
end
simSetJointTargetVelocity(leftMotor,maxVel)
simSetJointTargetVelocity(rightMotor,maxVel)

The problem is that the algorithm remains in the while loop until infinity.
Any suggestions to fix it?

Many thanks

coppelia
Posts: 7857
Joined: 14 Dec 2012, 00:25

Re: Get orientation and use it for a specific task

In the while loop, you retrieve the orientation with:

Code: Select all

local e=simGetEulerAnglesFromMatrix(m)
but it should be:

Code: Select all

e=simGetEulerAnglesFromMatrix(m)
Otherwise you create another local variable e, that disappears when the while loop loops.

Cheers

DavideIng
Posts: 39
Joined: 21 Sep 2017, 17:27

Re: Get orientation and use it for a specific task

Now works!

Many thanks

aburabee3
Posts: 1
Joined: 01 Apr 2019, 21:50

Re: Get orientation and use it for a specific task

Hi,

I used the above mentioned code to do 90 degree rotation when sensing an object in front, it worked nicely. However on the next object sensing the epuck doesnt rotate again, as the e[3] remains 90, how can we please solve this issue like to reset the e[]3: here is the code:
local m=sim.getObjectMatrix(ePuck,-1)
local m2=sim.buildMatrix({0,0,0},{0,90*math.pi/180,90*math.pi/180})
m=sim.multiplyMatrices(m,m2)
local e=sim.getEulerAnglesFromMatrix(m)
print (e[3])
if ((proxSensDist[2] or proxSensDist[3] or proxSensDist[4] or proxSensDist[5]) < noDetectionDistance) then
while(e[3] < math.pi/2) do
sim.setJointTargetVelocity(leftMotor,-maxVel*0.5)
sim.setJointTargetVelocity(rightMotor,maxVel*0.5)
local m=sim.getObjectMatrix(ePuck,-1)
local m2=sim.buildMatrix({0,0,0},{0,90*math.pi/180,90*math.pi/180})
m=sim.multiplyMatrices(m,m2)
e=sim.getEulerAnglesFromMatrix(m)
print (e[3])
end
end
sim.setJointTargetVelocity(leftMotor,maxVel)
sim.setJointTargetVelocity(rightMotor,maxVel)