track KB center

Requests regarding how to set up experiments in ARGoS.
germanpatt
Posts: 6
Joined: Wed Jul 13, 2022 6:32 pm

track KB center

Postby germanpatt » Wed Jul 13, 2022 7:07 pm

hi there,

i would like to track the positions of the centres of kilbots, but i think that what is saving is an off-centre coordinate (maybe the led position) I also found that the communications rays are not from centre to centre but from led to led. is it correct? my tracking function is as follows:

Code: Select all

#include "track_loop_functions.h" #include <iostream> #include <fstream> using namespace std; ofstream myFile; void CTrajectoryLoopFunctions::Init(TConfigurationNode& t_tree) { myFile.open ("track.csv", std::ofstream::out | std::ofstream::app); } void CTrajectoryLoopFunctions::Reset() { } void CTrajectoryLoopFunctions::PostStep() { CSpace::TMapPerType& tKBMap = GetSpace().GetEntitiesByType("kilobot"); int tick = GetSpace().GetSimulationClock(); for (CSpace::TMapPerType::iterator it = tKBMap.begin(); it != tKBMap.end(); ++it) { CKilobotEntity* pcKB = any_cast<CKilobotEntity*>(it->second); double x = pcKB->GetEmbodiedEntity().GetOriginAnchor().Position.GetX(); double y = pcKB->GetEmbodiedEntity().GetOriginAnchor().Position.GetY(); LOG << pcKB->GetId() << ": " << "t=" << tick << " x=" << x << " y=" << y << std::endl; myFile << pcKB-> GetId() << "," << tick << "," << x << "," << y << std::endl; } } void CTrajectoryLoopFunctions::Destroy() { myFile.close(); } REGISTER_LOOP_FUNCTIONS(CTrajectoryLoopFunctions, "track_loop_functions")
thank you!

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

Re: track KB center

Postby pincy » Wed Jul 13, 2022 7:33 pm

The code that defines the position of the anchors is here.

From there, you can see that the center of the body is indeed the center of the body, and the communication anchor is just above it; however, the LED anchor is slightly offset. This reflects the real Kilobot. If you encounter a different result, would you mind showing me a screenshot?

For further reference, the code that updates the anchors is here.

There is one method for each anchor:
  • UpdateCommAnchor() is for the range-and-bearing system (communication)
  • UpdateLightAnchor() is for the LED
The origin anchor is updated using this code, which is generic for any single-body robot like the Kilobot.
I made ARGoS.

germanpatt
Posts: 6
Joined: Wed Jul 13, 2022 6:32 pm

Re: track KB center

Postby germanpatt » Thu Jul 14, 2022 1:28 am

hi pincy, thanks for your quick response

I understand what you say, and the code looks right but, for example, when the kilobots are placed in the arena, it seems that the relevant position is the led. you can see this in Figs.1 and 2 (grid distribution with the same orientations) and Figs. 3 and 4 (grid distribution with random orientations) what seems to be aligned are the led positions. Figure 5 shows that the rays are led to led and it might give problems with the estimation of distances

Code for figures 1, 2, and 5

Code: Select all

<?xml version="1.0" ?> <argos-configuration> <!-- ************************* --> <!-- * General configuration * --> <!-- ************************* --> <framework> <system threads="0" /> <experiment length="0" ticks_per_second="31" random_seed="124" /> </framework> <!-- *************** --> <!-- * Controllers * --> <!-- *************** --> <controllers> <kilobot_controller id="kbc"> <actuators> <differential_steering implementation="default" /> <kilobot_led implementation="default" /> <kilobot_communication implementation="default" /> </actuators> <sensors> <kilobot_communication implementation="default" medium="kilocomm" show_rays="true" /> </sensors> <params behavior="build/examples/behaviors/disperse" /> </kilobot_controller> </controllers> <!-- *********************** --> <!-- * Arena configuration * --> <!-- *********************** --> <arena size="2, 2, 1" center="0,0,0.5"> <distribute> <position method="grid" center="0,0,0" distances="0.1,0.1,0" layout="4,3,1" /> <orientation method="constant" values="0,0,0"/> <entity quantity="12" max_trials="100"> <kilobot id="kb"> <controller config="kbc" />> </kilobot> </entity> </distribute> </arena> <!-- ******************* --> <!-- * Physics engines * --> <!-- ******************* --> <physics_engines> <dynamics2d id="dyn2d" /> </physics_engines> <!-- ********* --> <!-- * Media * --> <!-- ********* --> <media> <kilobot_communication id="kilocomm" /> </media> <!-- ****************** --> <!-- * Visualization * --> <!-- ****************** --> <visualization> <qt-opengl> <camera> <placement idx="0" position="-0.660983,0,0.6875" look_at="0.0531593,0,-0.0125011" lens_focal_length="20" /> </camera> </qt-opengl> </visualization> </argos-configuration>
Code for figure 3

Code: Select all

<?xml version="1.0" ?> <argos-configuration> <!-- ************************* --> <!-- * General configuration * --> <!-- ************************* --> <framework> <system threads="0" /> <experiment length="0" ticks_per_second="31" random_seed="124" /> </framework> <!-- *************** --> <!-- * Controllers * --> <!-- *************** --> <controllers> <kilobot_controller id="kbc"> <actuators> <differential_steering implementation="default" /> <kilobot_led implementation="default" /> <kilobot_communication implementation="default" /> </actuators> <sensors> <kilobot_communication implementation="default" medium="kilocomm" show_rays="true" /> </sensors> <params behavior="build/examples/behaviors/disperse" /> </kilobot_controller> </controllers> <!-- *********************** --> <!-- * Arena configuration * --> <!-- *********************** --> <arena size="2, 2, 1" center="0,0,0.5"> <distribute> <position method="grid" center="0,0,0" distances="0.1,0.1,0" layout="4,3,1" /> <orientation method="gaussian" mean="0,0,0" std_dev="360,0,0"/> <entity quantity="12" max_trials="100"> <kilobot id="kb"> <controller config="kbc" />> </kilobot> </entity> </distribute> </arena> <!-- ******************* --> <!-- * Physics engines * --> <!-- ******************* --> <physics_engines> <dynamics2d id="dyn2d" /> </physics_engines> <!-- ********* --> <!-- * Media * --> <!-- ********* --> <media> <kilobot_communication id="kilocomm" /> </media> <!-- ****************** --> <!-- * Visualization * --> <!-- ****************** --> <visualization> <qt-opengl> <camera> <placement idx="0" position="-0.660983,0,0.6875" look_at="0.0531593,0,-0.0125011" lens_focal_length="20" /> </camera> </qt-opengl> </visualization> </argos-configuration>
Attachments
fig1.jpeg
fig1.jpeg (32 KiB) Viewed 29282 times
fig2.jpeg
fig2.jpeg (34.4 KiB) Viewed 29282 times
fig3.jpeg
fig3.jpeg (49.26 KiB) Viewed 29282 times
fig4.jpeg
fig4.jpeg (62.66 KiB) Viewed 29282 times
fig5.jpeg
fig5.jpeg (44.31 KiB) Viewed 29282 times

germanpatt
Posts: 6
Joined: Wed Jul 13, 2022 6:32 pm

Re: track KB center

Postby germanpatt » Thu Jul 14, 2022 12:44 pm

hi again, i found this in KilobotEntity. maybe i can save orientations too a make the calculations to get the center position
thank you!

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

Re: track KB center

Postby pincy » Thu Jul 14, 2022 3:18 pm

Great!
I made ARGoS.

germanpatt
Posts: 6
Joined: Wed Jul 13, 2022 6:32 pm

Re: track KB center

Postby germanpatt » Thu Jul 14, 2022 7:36 pm

using KILOBOT_ECCENTRICITY and cZAngle.GetValue() solves the centre problem:
x = x + KILOBOT_ECCENTRICITY*cos(cZAngle.GetValue())
y = y + KILOBOT_ECCENTRICITY*sin(cZAngle.GetValue())

i am not sure how to fix the communication issue. if commAnchor is not located at the centre of the bot, the communication and distance estimation are not isotropic

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

Re: track KB center

Postby pincy » Fri Jul 15, 2022 3:34 pm

You can change the code locally to make it work as you need. In the end, it's just a matter of changing those parameters to suit your needs.
I made ARGoS.


Return to “How to... ?”