Skip to content

Commit 5525fcd

Browse files
author
fermendi
committed
add mate loop functions
1 parent 15b142a commit 5525fcd

14 files changed

+1200
-40
lines changed

‎loop-functions/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,4 +8,5 @@ link_directories(
88
#add_subdirectory(chocolate)
99
#add_subdirectory(example)
1010
add_subdirectory(gianduja)
11+
add_subdirectory(mate)
1112
#add_subdirectory(vanilla)

‎loop-functions/mate/APC/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
add_library(mate_apc_loopfunc SHARED MateAPCLoopFunc.h MateAPCLoopFunc.cpp)
2+
target_link_libraries(mate_apc_loopfunc argos3plugin_${ARGOS_BUILD_FOR}_epuck)

‎loop-functions/mate/APC/MateAPCLoopFunc.cpp

Lines changed: 165 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
/**
2+
* Any-Point Closseness loop function
3+
*
4+
* @file <loop-functions/mate/APC/MateAPCLoopFunc.cpp>
5+
*
6+
* @author Fernando Mendiburu - <[email protected]>
7+
*
8+
* @package experiments-loop-functions
9+
*
10+
* @license MIT License
11+
*/
12+
13+
#include "MateAPCLoopFunc.h"
14+
15+
/****************************************/
16+
/****************************************/
17+
18+
MateAPCLoopFunction::MateAPCLoopFunction() {
19+
20+
m_fSideSquare = 1.0;
21+
22+
m_cCoordSquareSpot = CVector2(0.6, 0);
23+
24+
m_unNumberPoints = 1000;
25+
26+
m_fObjectiveFunction = 0;
27+
28+
}
29+
30+
/****************************************/
31+
/****************************************/
32+
33+
MateAPCLoopFunction::MateAPCLoopFunction(const MateAPCLoopFunction& orig) {}
34+
35+
/****************************************/
36+
/****************************************/
37+
38+
MateAPCLoopFunction::~MateAPCLoopFunction() {}
39+
40+
/****************************************/
41+
/****************************************/
42+
43+
void MateAPCLoopFunction::Destroy() {}
44+
45+
/****************************************/
46+
/****************************************/
47+
48+
void MateAPCLoopFunction::Reset() {
49+
CoreLoopFunctions::Reset();
50+
}
51+
52+
/****************************************/
53+
/****************************************/
54+
55+
argos::CColor MateAPCLoopFunction::GetFloorColor(const argos::CVector2& c_position_on_plane) {
56+
CVector2 cCurrentPoint(c_position_on_plane.GetX(), c_position_on_plane.GetY());
57+
58+
if (IsOnSquareArea(cCurrentPoint)){
59+
return CColor::BLACK;
60+
} else{
61+
return CColor::GRAY50;
62+
}
63+
}
64+
65+
/****************************************/
66+
/****************************************/
67+
68+
void MateAPCLoopFunction::PostExperiment() {
69+
m_fObjectiveFunction = ComputeObjectiveFunction();
70+
LOG << "Score: " << GetObjectiveFunction() << std::endl;
71+
}
72+
73+
74+
75+
/****************************************/
76+
/****************************************/
77+
78+
Real MateAPCLoopFunction::GetObjectiveFunction() {
79+
return m_fObjectiveFunction;
80+
}
81+
82+
/****************************************/
83+
/****************************************/
84+
85+
Real MateAPCLoopFunction::ComputeObjectiveFunction() {
86+
CVector2 cRandomPoint;
87+
Real dA=0, dP=0;
88+
CSpace::TMapPerType mEpucks = GetSpace().GetEntitiesByType("epuck");
89+
CVector2 cEpuckPosition(0,0);
90+
Real fDistanceToRandomPoint = 0;
91+
92+
// White square area
93+
for(UInt32 i = 0; i < m_unNumberPoints; i++){
94+
95+
Real fMinDistanceOnSquare = 0.67; // Correspond to worst case, only one robot in the corner of the square
96+
97+
cRandomPoint = RandomPointOnSquareArea();
98+
99+
for (CSpace::TMapPerType::iterator it = mEpucks.begin(); it != mEpucks.end(); ++it) {
100+
CEPuckEntity* pcEpuck = any_cast<CEPuckEntity*> ((*it).second);
101+
cEpuckPosition.Set(pcEpuck->GetEmbodiedEntity().GetOriginAnchor().Position.GetX(),
102+
pcEpuck->GetEmbodiedEntity().GetOriginAnchor().Position.GetY());
103+
if(IsOnSquareArea(cEpuckPosition)){
104+
fDistanceToRandomPoint = (cRandomPoint - cEpuckPosition).Length();
105+
if(fDistanceToRandomPoint < fMinDistanceOnSquare){
106+
fMinDistanceOnSquare = fDistanceToRandomPoint;
107+
}
108+
}
109+
}
110+
111+
dA += fMinDistanceOnSquare;
112+
}
113+
dA /= m_unNumberPoints;
114+
115+
116+
117+
118+
Real performance = 100*100*dA*dA; // in cm2
119+
120+
return performance;
121+
}
122+
123+
/****************************************/
124+
/****************************************/
125+
126+
CVector2 MateAPCLoopFunction::RandomPointOnSquareArea(){
127+
return CVector2(m_pcRng->Uniform(CRange<Real>(m_cCoordSquareSpot.GetX() - m_fSideSquare/2.0f, m_cCoordSquareSpot.GetX() + m_fSideSquare/2.0f)),
128+
m_pcRng->Uniform(CRange<Real>(m_cCoordSquareSpot.GetY() - m_fSideSquare/2.0f, m_cCoordSquareSpot.GetY() + m_fSideSquare/2.0f)));
129+
}
130+
131+
/****************************************/
132+
/****************************************/
133+
134+
bool MateAPCLoopFunction::IsOnSquareArea(const CVector2& c_point){
135+
CRange<Real> cRangeSquareX(m_cCoordSquareSpot.GetX() - m_fSideSquare/2.0f, m_cCoordSquareSpot.GetX() + m_fSideSquare/2.0f);
136+
CRange<Real> cRangeSquareY(m_cCoordSquareSpot.GetY() - m_fSideSquare/2.0f, m_cCoordSquareSpot.GetY() + m_fSideSquare/2.0f);
137+
138+
if (cRangeSquareX.WithinMinBoundIncludedMaxBoundIncluded(c_point.GetX()) &&
139+
cRangeSquareY.WithinMinBoundIncludedMaxBoundIncluded(c_point.GetY())) {
140+
return true;
141+
}
142+
return false;
143+
}
144+
145+
146+
/****************************************/
147+
/****************************************/
148+
149+
CVector3 MateAPCLoopFunction::GetRandomPosition() {
150+
151+
CVector3 cPosition;
152+
153+
154+
do {
155+
cPosition = CVector3(m_pcRng->Uniform(CRange<Real>(-m_fDistributionRadius,-0.6)),
156+
m_pcRng->Uniform(CRange<Real>(-m_fDistributionRadius,m_fDistributionRadius)),
157+
0);
158+
} while(cPosition.Length()>=m_fDistributionRadius);
159+
160+
161+
return cPosition;
162+
}
163+
164+
165+
REGISTER_LOOP_FUNCTIONS(MateAPCLoopFunction, "mate_apc_loop_functions");

‎loop-functions/mate/APC/MateAPCLoopFunc.h

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
/**
2+
* Any-Point Closseness loop function
3+
*
4+
* @file <loop-functions/mate/APC/MateAPCLoopFunc.h>
5+
*
6+
* @author Fernando Mendiburu - <[email protected]>
7+
*
8+
* @package experiments-loop-functions
9+
*
10+
* @license MIT License
11+
*/
12+
13+
#ifndef MATE_APC_LOOP_FUNC_H
14+
#define MATE_APC_LOOP_FUNC_H
15+
16+
#include "../../../src/CoreLoopFunctions.h"
17+
#include <argos3/core/simulator/space/space.h>
18+
#include <argos3/plugins/robots/e-puck/simulator/epuck_entity.h>
19+
20+
using namespace argos;
21+
22+
class MateAPCLoopFunction : public CoreLoopFunctions {
23+
24+
public:
25+
MateAPCLoopFunction();
26+
MateAPCLoopFunction(const MateAPCLoopFunction& orig);
27+
virtual ~MateAPCLoopFunction();
28+
29+
virtual void Destroy();
30+
virtual void Reset();
31+
virtual void PostExperiment();
32+
33+
34+
Real GetObjectiveFunction();
35+
36+
virtual CColor GetFloorColor(const CVector2& c_position_on_plane);
37+
38+
virtual CVector3 GetRandomPosition();
39+
40+
private:
41+
Real ComputeObjectiveFunction();
42+
CVector2 RandomPointOnSquareArea();
43+
bool IsOnSquareArea(const CVector2& c_point);
44+
45+
Real m_fRadiusRobot;
46+
Real m_fSideSquare;
47+
CVector2 m_cCoordSquareSpot;
48+
UInt32 m_unNumberPoints;
49+
Real m_fObjectiveFunction;
50+
51+
};
52+
53+
#endif

‎loop-functions/mate/CC/CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
add_library(mate_cc_loopfunc SHARED MateCCLoopFunc.h MateCCLoopFunc.cpp)
2+
target_link_libraries(mate_cc_loopfunc argos3plugin_${ARGOS_BUILD_FOR}_epuck)

0 commit comments

Comments
 (0)