Get orientation and use it for a specific task

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

Get orientation and use it for a specific task

Post by DavideIng » 16 Nov 2017, 17:53

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
Site Admin
Posts: 6218
Joined: 14 Dec 2012, 00:25

Re: Get orientation and use it for a specific task

Post by coppelia » 17 Nov 2017, 10:09

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: 34
Joined: 21 Sep 2017, 17:27

Re: Get orientation and use it for a specific task

Post by DavideIng » 17 Nov 2017, 15:06

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
Site Admin
Posts: 6218
Joined: 14 Dec 2012, 00:25

Re: Get orientation and use it for a specific task

Post by coppelia » 17 Nov 2017, 15:40

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: 34
Joined: 21 Sep 2017, 17:27

Re: Get orientation and use it for a specific task

Post by DavideIng » 17 Nov 2017, 16:14

Now works!

Many thanks

Post Reply