track KB center

[germanpatt]

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]

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.

[germanpatt]

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>

[germanpatt]

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]

Great!

[germanpatt]

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]

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.