Frame of Reference

Requests regarding how to set up experiments in ARGoS.
M_Rizk
Posts: 28
Joined: Thu Apr 30, 2015 2:46 am

Frame of Reference

Postby M_Rizk » Tue Jul 05, 2016 5:38 pm

Hi Carlo,

How can I make a foot-bot follow a heading vector expressed in the global frame of reference?

To clarify, I have a foot-bot that I want to move to a certain location. I know the global position of the foot-bot and the goal location so I create a vector between the two. However, this vector is expressed with respect to the global coordinate system. Since I know the orientation of the foot-bot, I use that to adjust it. I've written some code (modified from the flocking example), but when I run it on multiple robots, they each move in a different direction.

Code: Select all

SetWheelSpeedsFromVector(VectorToGoal()); CVector2 CInteraction::VectorToGoal() { /* Calculate a normalized vector that points to the goal */ Real xCartesian = goalLocation.GetX()-GetPosition().GetX(); Real yCartesian = goalLocation.GetY()-GetPosition().GetY(); Real distanceToGoal = pow( pow(xCartesian,2.0)+pow(yCartesian,2.0) , 0.5); Real angleToGoal = atan(yCartesian/xCartesian); CVector2 goalVector = CVector2(distanceToGoal,angleToGoal); Real orientationChange = -GetOrientation().GetValue(); CVector2 frameAdjustmentVector = CVector2(0,orientationChange); goalVector += frameAdjustmentVector; if(goalVector.Length() > 0.0f) { /* Make the vector long as 1/4 of the max speed */ goalVector.Normalize(); goalVector*= 0.25f * m_sWheelTurningParams.MaxSpeed; } return goalVector; }
Do you have any thoughts as to what I'm doing wrong?

Cheers,
Mostafa

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

Re: Frame of Reference

Postby pincy » Wed Jul 06, 2016 7:46 pm

Hi Mostafa,

Try this code instead:

Code: Select all

class CInteraction : public CCI_Controller { ... /* Positioning sensor */ CCI_PositioningSensor* m_pcPositioning; /* Goal location in the world */ CVector2 cGoalLocW; ... } CVector2 CInteraction::VectorToGoal() { /* Calculate vector between me and goal in the world */ CVector2 cMeToGoalW = cGoalLocW - m_pcPositioning.GetReading().Position; /* Get my heading in the world (heading = rotation along world Z axis) */ CRadians cMyHeadingW, cTmp1, cTmp2; m_pcPositioning.GetReading().Orientation.ToEulerAngles(cMyHeadingW, cTmp1, cTmp2); /* Calculate vector between me and goal with respect to my local frame of reference */ CVector2 cMeToGoalL = cMeToGoalW.Rotate(-cMyHeadingW); /* Return the final vector */ return cMeToGoalL; }
TIP: avoid performing vector calculations by hand, as in your code example. Use the CVector2 class instead - it's much more reliable. For instance, your line to calculate the angle of a vector uses atan(y/x), which won't work if x is close to zero. The CVector2::Angle() class uses a more reliable method.

Cheers,
Carlo
I made ARGoS.

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

Re: Frame of Reference

Postby M_Rizk » Thu Jul 07, 2016 7:49 am

Hi Carlo,

This does exactly what I want it to! I had to make some minor syntax tweaks, otherwise, it's perfect! Prompt and helpful as always :) Thanks!

For anyone trying this and wondering about the tweaks:

Code: Select all

/* Change this */ CVector2 cMeToGoalW = cGoalLocW - m_pcPositioning.GetReading().Position; m_pcPositioning.GetReading().Orientation.ToEulerAngles(cMyHeadingW, cTmp1, cTmp2); /* To this */ Real x = m_pcPositioning->GetReading().Position.GetX(); Real y = m_pcPositioning->GetReading().Position.GetY(); CVector2 cMeToGoalW = cGoalLocW - CVector2(x,y); m_pcPositioning->GetReading().Orientation.ToEulerAngles(cMyHeadingW, cTmp1, cTmp2);
Cheers


Return to “How to... ?”