Footbot - Position sensor

Requests regarding how to set up experiments in ARGoS.
Wleo
Posts: 7
Joined: Fri Feb 05, 2016 8:15 pm

Footbot - Position sensor

Postby Wleo » Sun Jul 03, 2016 2:55 am

hello everyone!

I am using the position sensor with the footbot. I am rotating the footbot and when it gets to the desired rotation angle it will trigger and go foward. To get the angle of the rotation using:

m_cAlpha = m_pcPosSens->GetReading().Orientation;

Using this line gives me a Quaternion as an output. This gives me (angle,0,0), from here I just want to get the angle so I can use the angle which the footbot is rotating at a particular moment, the angle I get from the quaternion is nice, because half the rotation is positive and the other half is negative going from 0 to 180 and -180 to 0, but I don't know how to get just the angle from the Quaternion (first integer of the group of numbers) so I use:

m_cAlpha.GetW()

This gives me a number between 0 to 1. The problem is with the rotation sign, first whole rotation is positive from 0 to 1 and 1 to 0 and then, second whole rotation changes to negative same 0 to -1 and -1 to 0 making it really difficult to select a fix value, since it always will have a second identical value in the opposite site of the rotation that the footbot is orientated.
I have tried everything, using:

m_cAlpha.SetX()
m_cAlpha.SetY()

to change the sign on the rotation but nothing seem to give me the desired result. One of my issues is my poor understanding of quaternions, but I have read a lot about them and still can not figure it out. It will be really helpful if someone can give me some advise. Let me know if something is not clear I will try and explain myself better.

I have attach the files so it is visible in the Log.
footbot_diffusion.cpp
(3.42 KiB) Downloaded 718 times
footbot_diffusion.h
(3.76 KiB) Downloaded 640 times
rot = to the Quaterion so it give (angle,0,0)
rot1 = to the m_cAlpha.GetW()

After a few rotation is possible to see that rot1 changes the sign after a whole rotation. I hope this makes thing more clear.

For some reason it does not let me attach the XML file. I used the diffusion_1.argos just adding in the sensors section the position sensor of the footbot:

<positioning implementation="default" />

Thanks for your time,
Leonardo.

pincy
Site Admin
Posts: 632
Joined: Thu Mar 08, 2012 8:04 pm
Location: Boston, MA
Contact:

Re: Footbot - Position sensor

Postby pincy » Sun Jul 03, 2016 6:16 pm

Hi Leonardo,

Quaternions are a vectorial representation for angles in 3D dimensions. The components of a quaternion are 4 and do not directly map to the X-Y-Z angles you're looking for - which are called Euler angles. You need to perform a transformation from quaternion to Euler angles.

The transformation can be performed using the method CQuaternion::ToEulerAngles() as follows:

Code: Select all

CQuaternion cQuat = ... CRadians cZAngle, cYAngle, cXAngle; cQuat.ToEulerAngles(cZAngle, cYAngle, cXAngle);
The angle you're looking for is cZAngle. This angle is the absolute orientation of the foot-bot in the experimental arena along the Z axis. The reason why the order is Z-Y-X rather than X-Y-Z is to remind people that, when transforming from Euler angles to quaternion, a robot is first rotated around Z, then around Y, and finally around X.

Hope this helps!

Cheers,
Carlo
I made ARGoS.

Wleo
Posts: 7
Joined: Fri Feb 05, 2016 8:15 pm

Re: Footbot - Position sensor

Postby Wleo » Sun Jul 03, 2016 8:32 pm

Dear Carlo,

Helpful as always, I am using the cZAngle to get the angle of orientation of the footbot. I am trying to use the cZAngle as a threshold to make the footbot change color and go foward, but for some reason I cant use it properly

if (cZAngle >= 1.7) {

m_pcWheels->SetLinearVelocity(m_fWheelVelocity, m_fWheelVelocity);
m_pcLEDs->SetAllColors(CColor::YELLOW);

}

This if loop does not work, but this is the main idea I am trying to execute. I have tried double, int but still does not give me radians or degrees.

Thanks for your time Carlo,
Leonardo.

pincy
Site Admin
Posts: 632
Joined: Thu Mar 08, 2012 8:04 pm
Location: Boston, MA
Contact:

Re: Footbot - Position sensor

Postby pincy » Mon Jul 04, 2016 12:09 am

Hi Leonardo,

I'm not sure I understood what is the problem with your code. The code that you posted looks more or less correct - let's see if with a larger code example we manage to make it work:

Code: Select all

void CMyController::ControlStep() { // ... // Get Euler angle representation of foot-bot position // Every angle is for sure in the range [-PI,PI] CRadians cZAngle, cYAngle, cXAngle; m_pcPosSens->GetReading().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle); // If the robot is in the X<0, Y>0 quadrant, go straight, otherwise rotate on the spot if(cZAngle >= 1.7) { m_pcWheels->SetLinearVelocity(m_fWheelVelocity, m_fWheelVelocity); m_pcLEDs->SetAllColors(CColor::YELLOW); } else { m_pcWheels->SetLinearVelocity(-m_fWheelVelocity, m_fWheelVelocity); } }
Is this what you were trying to achieve?

Cheers,
Carlo
I made ARGoS.

Wleo
Posts: 7
Joined: Fri Feb 05, 2016 8:15 pm

Re: Footbot - Position sensor

Postby Wleo » Mon Jul 04, 2016 12:57 am

Dear Carlo,

That's what I want to achieve.The robot its rotating in the spot, when it reaches +90 degrees (1.6 radians approx.). Change color to yellow and go forward. Which it seems clear in the code you just send me. But its not that easy for some reason.
The problem is at the beginning of the if loop

if(cZAngle >= 1.7) {}

I think the problem is that I'm trying to compare an instance of the CRadians class with the 'double (1.7).
When I run the code this is the error that I am getting:

[ 4%] Building CXX object controllers/footbot_diffusion/CMakeFiles/footbot_diffusion.dir/footbot_diffusion.cpp.o
/home/wleo/argos40/argos3-examples/controllers/footbot_diffusion/footbot_diffusion.cpp: In member function ‘virtual void CFootBotDiffusion::ControlStep()’:
/home/wleo/argos40/argos3-examples/controllers/footbot_diffusion/footbot_diffusion.cpp:77:15: error: no match for ‘operator>=’ (operand types are ‘argos::CRadians’ and ‘double’)
if(cZAngle >= 1.7) {
^
/home/wleo/argos40/argos3-examples/controllers/footbot_diffusion/footbot_diffusion.cpp:77:15: note: candidate is:
In file included from /usr/include/argos3/plugins/robots/foot-bot/control_interface/ci_footbot_proximity_sensor.h:40:0,
from /home/wleo/argos40/argos3-examples/controllers/footbot_diffusion/footbot_diffusion.h:30,
from /home/wleo/argos40/argos3-examples/controllers/footbot_diffusion/footbot_diffusion.cpp:2:
/usr/include/argos3/core/utility/math/angles.h:233:19: note: bool argos::CRadians::operator>=(const argos::CRadians&) const
inline bool operator>=(const CRadians& c_radians) const {
^
/usr/include/argos3/core/utility/math/angles.h:233:19: note: no known conversion for argument 1 from ‘double’ to ‘const argos::CRadians&’
make[2]: *** [controllers/footbot_diffusion/CMakeFiles/footbot_diffusion.dir/footbot_diffusion.cpp.o] Error 1
make[1]: *** [controllers/footbot_diffusion/CMakeFiles/footbot_diffusion.dir/all] Error 2
make: *** [all] Error 2

Thanks for your time Carlo,
Kind regards,
Leonardo.

M_Rizk
Posts: 28
Joined: Thu Apr 30, 2015 2:46 am

Re: Footbot - Position sensor

Postby M_Rizk » Mon Jul 04, 2016 12:49 pm

Hi Leonardo,

Your assessment is correct. Try passing 1.7 to the CRadians constructor to create a CRadians object.

CRadians valueToCompare = CRadians( Real(1.7) );

if(cZAngle >= valueToCompare) {}

I think that should fix your problem. You can find more information about the CRadians class in angles.h:
https://github.com/ilpincy/argos3/blob/ ... h/angles.h

Mostafa


Return to “How to... ?”