Get orientation and use it for a specific task

[DavideIng]

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]

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]

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]

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]

Now works!

Many thanks

[aburabee3]

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:

Code: Select all

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)

Code: Select all