Commit 073c12a2 authored by LOPEZ GANDIA Axel's avatar LOPEZ GANDIA Axel

Added cost functions parameters

parent ee22f044
...@@ -32,15 +32,16 @@ ...@@ -32,15 +32,16 @@
/* /*
* Deviates the trajectory of the agent to prevent collisions. * Deviates the trajectory of the agent to prevent collisions.
* Uses FOE to detect and prevent collisions. * Uses FOE to detect and prevent collisions.
* Parameters: tau0
*/ */
class PowerLaw : public CostFunction class PowerLaw : public CostFunction
{ {
private: private:
float tau0 = 3;
public: public:
const static std::string GetName() { return "PowerLaw"; } const static std::string GetName() { return "PowerLaw"; }
float tau0 = 3;
const static std::string Name; const static std::string Name;
PowerLaw(); PowerLaw();
......
...@@ -29,7 +29,8 @@ ...@@ -29,7 +29,8 @@
#include <core/costFunction.h> #include <core/costFunction.h>
/* /*
* Implements social forces algorithm. * Implements social forces algorithm for collision avoidance only.
* Paramters: sigma
*/ */
class SocialForcesAvoidance : public CostFunction class SocialForcesAvoidance : public CostFunction
{ {
......
...@@ -40,20 +40,20 @@ ...@@ -40,20 +40,20 @@
* distance from each other, called dca. * distance from each other, called dca.
* *
* This cost function maximise those 2 metrics together. * This cost function maximise those 2 metrics together.
* Parameters: sigmaTtca, sigmaDca
*/ */
class TtcaDca : public CostFunction class TtcaDca : public CostFunction
{ {
/*
* Parameters of the cost function
*/
float sigTtca_ = 2;
float sigDca_ = 0.5;
private: private:
Vector2D GradDca(); Vector2D GradDca();
Vector2D GradTtca(); Vector2D GradTtca();
float Cost(float ttca, float dca); float Cost(float ttca, float dca);
/*
* Parameters of the cost function
*/
float sigTtca_ = 2;
float sigDca_ = 0.5;
public: public:
......
...@@ -29,19 +29,21 @@ ...@@ -29,19 +29,21 @@
#include <core/costFunction.h> #include <core/costFunction.h>
/* /*
* Implements social forces algorithm. * Implements social forces algorithm for collision avoidance and goal reaching at once.
* Parameters: AtractionForce, RepulsionForce, sigma
*/ */
class SocialForces : public CostFunction class SocialForces : public CostFunction
{ {
private: private:
float AtractorForce = 1;
float RepulsionForce = 3;
float sigma = 0.3;
public: public:
const static std::string GetName() { return "SocialForces"; } const static std::string GetName() { return "SocialForces"; }
const static std::string Name; const static std::string Name;
float AtractorForce = 1;
float RepulsionForce = 3;
SocialForces(); SocialForces();
......
...@@ -87,6 +87,10 @@ CostFunctionValues PowerLaw::GetCostFunctionGradient(Agent* agent, WorldBase * w ...@@ -87,6 +87,10 @@ CostFunctionValues PowerLaw::GetCostFunctionGradient(Agent* agent, WorldBase * w
void PowerLaw::parseParameters(tinyxml2::XMLElement * v) void PowerLaw::parseParameters(tinyxml2::XMLElement * v)
{ {
CostFunction::parseParameters(v); CostFunction::parseParameters(v);
float val;
if (v->QueryFloatAttribute("tau0", &val) == tinyxml2::XML_SUCCESS) {
tau0 = val;
}
} }
REGISTER_COST_FUNCTION(PowerLaw) REGISTER_COST_FUNCTION(PowerLaw)
\ No newline at end of file
...@@ -80,6 +80,10 @@ CostFunctionValues SocialForcesAvoidance::GetCostFunctionGradient(Agent* agent, ...@@ -80,6 +80,10 @@ CostFunctionValues SocialForcesAvoidance::GetCostFunctionGradient(Agent* agent,
void SocialForcesAvoidance::parseParameters(tinyxml2::XMLElement * v) void SocialForcesAvoidance::parseParameters(tinyxml2::XMLElement * v)
{ {
CostFunction::parseParameters(v); CostFunction::parseParameters(v);
float val;
if (v->QueryFloatAttribute("sigma", &val) == tinyxml2::XML_SUCCESS) {
sigma = val;
}
} }
REGISTER_COST_FUNCTION(SocialForcesAvoidance) REGISTER_COST_FUNCTION(SocialForcesAvoidance)
\ No newline at end of file
...@@ -157,6 +157,13 @@ CostFunctionValues TtcaDca::GetCostFunctionGradient(Agent* agent, WorldBase * wo ...@@ -157,6 +157,13 @@ CostFunctionValues TtcaDca::GetCostFunctionGradient(Agent* agent, WorldBase * wo
void TtcaDca::parseParameters(tinyxml2::XMLElement * v) void TtcaDca::parseParameters(tinyxml2::XMLElement * v)
{ {
CostFunction::parseParameters(v); CostFunction::parseParameters(v);
float val;
if (v->QueryFloatAttribute("sigmaTtca", &val) == tinyxml2::XML_SUCCESS) {
sigTtca_ = val;
}
if (v->QueryFloatAttribute("sigmaDca", &val) == tinyxml2::XML_SUCCESS) {
sigDca_ = val;
}
} }
REGISTER_COST_FUNCTION(TtcaDca) REGISTER_COST_FUNCTION(TtcaDca)
\ No newline at end of file
...@@ -71,7 +71,7 @@ CostFunctionValues SocialForces::GetCostFunctionGradient(Agent* agent, WorldBase ...@@ -71,7 +71,7 @@ CostFunctionValues SocialForces::GetCostFunctionGradient(Agent* agent, WorldBase
const Vector2D& DB = (1/(2*B))*(pow(R.magnitude() + (R - Vb * dt*eb).magnitude(), 2) / (R.magnitude() * (R - Vb * dt*eb).magnitude()))*R; const Vector2D& DB = (1/(2*B))*(pow(R.magnitude() + (R - Vb * dt*eb).magnitude(), 2) / (R.magnitude() * (R - Vb * dt*eb).magnitude()))*R;
Repulsion += RepulsionForce*exp(-B) * DB; Repulsion += RepulsionForce*exp(-B/sigma) * DB;
} }
Result.Gradient = Atraction + Repulsion; Result.Gradient = Atraction + Repulsion;
...@@ -83,22 +83,19 @@ CostFunctionValues SocialForces::GetCostFunctionGradient(Agent* agent, WorldBase ...@@ -83,22 +83,19 @@ CostFunctionValues SocialForces::GetCostFunctionGradient(Agent* agent, WorldBase
void SocialForces::parseParameters(tinyxml2::XMLElement * v) void SocialForces::parseParameters(tinyxml2::XMLElement * v)
{ {
CostFunction::parseParameters(v); CostFunction::parseParameters(v);
} float val;
if (v->QueryFloatAttribute("coeff", &val) == tinyxml2::XML_SUCCESS) {
REGISTER_COST_FUNCTION(SocialForces) coefficient_ = val;
/*
class AngryCostFunction : public CostFunction {
public:
const static std::string GetName() { return "Angry"; }
CostFunctionValues GetCostFunctionGradient(Agent* agent, WorldBase * world) override {
std::cerr << "Angry" << std::endl;
return CostFunctionValues();
} }
void parseParameters(tinyxml2::XMLElement* v) override { if (v->QueryFloatAttribute("AtractionForce", &val) == tinyxml2::XML_SUCCESS) {
CostFunction::parseParameters(v); AtractorForce = val;
} }
}; if (v->QueryFloatAttribute("RepulsionForce", &val) == tinyxml2::XML_SUCCESS) {
REGISTER_COST_FUNCTION(AngryCostFunction) RepulsionForce = val;
*/ }
if (v->QueryFloatAttribute("sigma", &val) == tinyxml2::XML_SUCCESS) {
sigma = val;
}
}
REGISTER_COST_FUNCTION(SocialForces)
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment