Skip to content

Commit 90e9c15

Browse files
author
Aart Stuurman
committed
WIP Working on checking sdf parameters
1 parent b1e9c00 commit 90e9c15

File tree

4 files changed

+116
-56
lines changed

4 files changed

+116
-56
lines changed

‎cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp‎

Lines changed: 50 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -3,22 +3,22 @@
33
//
44

55
#include "DifferentialCPGClean.h"
6+
#include "../util/SdfUtil.hpp"
67

78
using namespace revolve::gazebo;
89

910
DifferentialCPGClean::DifferentialCPGClean(const sdf::ElementPtr brain_sdf,
1011
const std::vector<MotorPtr> &_motors)
11-
: Brain()
12-
, revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors)
13-
{}
12+
: Brain(), revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors)
13+
{
14+
}
1415

1516
DifferentialCPGClean::DifferentialCPGClean(const sdf::ElementPtr brain_sdf,
16-
const std::vector<MotorPtr> &_motors,
17-
const NEAT::Genome &genome)
18-
: Brain()
19-
, revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors, genome)
20-
{}
21-
17+
const std::vector<MotorPtr> &_motors,
18+
const NEAT::Genome &genome)
19+
: Brain(), revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors, genome)
20+
{
21+
}
2222

2323
void DifferentialCPGClean::Update(const std::vector<MotorPtr> &_motors,
2424
const std::vector<SensorPtr> &_sensors,
@@ -28,36 +28,45 @@ void DifferentialCPGClean::Update(const std::vector<MotorPtr> &_motors,
2828
this->::revolve::DifferentialCPG::update(_motors, _sensors, _time, _step);
2929
}
3030

31-
revolve::DifferentialCPG::ControllerParams DifferentialCPGClean::load_params_from_sdf(sdf::ElementPtr brain_sdf) {
32-
// Get all params from the sdf
33-
// TODO: Add exception handling
34-
sdf::ElementPtr controller_sdf = brain_sdf->GetElement("rv:controller");
35-
revolve::DifferentialCPG::ControllerParams params;
36-
params.reset_neuron_random = (controller_sdf->GetAttribute("reset_neuron_random")->GetAsString() == "true");
37-
params.use_frame_of_reference = (controller_sdf->GetAttribute("use_frame_of_reference")->GetAsString() == "true");
38-
params.init_neuron_state = stod(controller_sdf->GetAttribute("init_neuron_state")->GetAsString());
39-
params.range_ub = stod(controller_sdf->GetAttribute("range_ub")->GetAsString());
40-
params.signal_factor_all = stod(controller_sdf->GetAttribute("signal_factor_all")->GetAsString());
41-
params.signal_factor_mid = stod(controller_sdf->GetAttribute("signal_factor_mid")->GetAsString());
42-
params.signal_factor_left_right = stod(controller_sdf->GetAttribute("signal_factor_left_right")->GetAsString());
43-
params.abs_output_bound = stod(controller_sdf->GetAttribute("abs_output_bound")->GetAsString());
44-
45-
// Get the weights from the sdf:
46-
// If loading with CPPN, the weights attribute does not exist
47-
if (controller_sdf->HasAttribute("weights")) {
48-
std::string sdf_weights = controller_sdf->GetAttribute("weights")->GetAsString();
49-
std::string delimiter = ";";
50-
51-
size_t pos = 0;
52-
std::string token;
53-
while ((pos = sdf_weights.find(delimiter)) != std::string::npos) {
54-
token = sdf_weights.substr(0, pos);
55-
params.weights.push_back(stod(token));
56-
sdf_weights.erase(0, pos + delimiter.length());
57-
}
58-
// push the last element that does not end with the delimiter
59-
params.weights.push_back(stod(sdf_weights));
60-
}
61-
62-
return params;
31+
revolve::DifferentialCPG::ControllerParams DifferentialCPGClean::load_params_from_sdf(sdf::ElementPtr brain_sdf)
32+
{
33+
try
34+
{
35+
// Get all params from the sdf
36+
sdf::ElementPtr controller_sdf = brain_sdf->GetElement("rv:controller");
37+
revolve::DifferentialCPG::ControllerParams params;
38+
params.reset_neuron_random = getSdfAttrSafe<bool>(controller_sdf, "reset_neuron_random");
39+
params.use_frame_of_reference = getSdfAttrSafe<bool>(controller_sdf, "use_frame_of_reference");
40+
params.init_neuron_state = getSdfAttrSafe<double>(controller_sdf, "init_neuron_state");
41+
params.range_ub = getSdfAttrSafe<double>(controller_sdf, "range_ub");
42+
params.signal_factor_all = getSdfAttrSafe<double>(controller_sdf, "signal_factor_all");
43+
params.signal_factor_mid = getSdfAttrSafe<double>(controller_sdf, "signal_factor_mid");
44+
params.signal_factor_left_right = getSdfAttrSafe<double>(controller_sdf, "signal_factor_left_right");
45+
params.abs_output_bound = getSdfAttrSafe<double>(controller_sdf, "abs_output_bound");
46+
47+
// Get the weights from the sdf:
48+
// If loading with CPPN, the weights attribute does not exist
49+
if (controller_sdf->HasAttribute("weights"))
50+
{
51+
std::string sdf_weights = getSdfAttrSafe<std::string>(controller_sdf, "weights");
52+
std::string delimiter = ";";
53+
54+
size_t pos = 0;
55+
std::string token;
56+
while ((pos = sdf_weights.find(delimiter)) != std::string::npos)
57+
{
58+
token = sdf_weights.substr(0, pos);
59+
params.weights.push_back(stod(token));
60+
sdf_weights.erase(0, pos + delimiter.length());
61+
}
62+
// push the last element that does not end with the delimiter
63+
params.weights.push_back(stod(sdf_weights));
64+
}
65+
66+
return params;
67+
}
68+
catch (std::runtime_error const &err)
69+
{
70+
throw std::runtime_error(std::string("Could not load sdf. ") + std::string(err.what()));
71+
}
6372
}

‎cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.cpp‎

Lines changed: 23 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,11 @@
88
#include "DifferentialCPPNCPG.h"
99
#include "DifferentialCPGClean.h"
1010

11+
#include "../util/SdfUtil.hpp"
12+
1113
using namespace revolve::gazebo;
1214

13-
bool string_replace(std::string& str, const std::string& from, const std::string& to)
15+
bool string_replace(std::string &str, const std::string &from, const std::string &to)
1416
{
1517
size_t start_pos = str.find(from);
1618
int substitutions = 0;
@@ -23,31 +25,39 @@ bool string_replace(std::string& str, const std::string& from, const std::string
2325
return substitutions > 0;
2426
}
2527

26-
2728
DifferentialCPPNCPG::DifferentialCPPNCPG(const sdf::ElementPtr brain_sdf,
28-
const std::vector<MotorPtr> &motors)
29-
: DifferentialCPGClean(
30-
brain_sdf,
31-
motors,
32-
DifferentialCPPNCPG::load_cppn_genome_from_sdf(brain_sdf))
33-
{}
29+
const std::vector<MotorPtr> &motors)
30+
: DifferentialCPGClean(
31+
brain_sdf,
32+
motors,
33+
DifferentialCPPNCPG::load_cppn_genome_from_sdf(brain_sdf))
34+
{
35+
}
3436

3537
/// \brief extracts CPPN genome from brain_sdf
3638
/// \param[in] brain_sdf ElementPtr containing the "brain" - tag of the model sdf
3739
/// \return the NEAT genome
3840
/// \details Loads genome from SDF as string and deserializes it into type type NEAT::Genome
3941
NEAT::Genome DifferentialCPPNCPG::load_cppn_genome_from_sdf(const sdf::ElementPtr brain_sdf)
4042
{
41-
const sdf::ElementPtr genome_sdf = brain_sdf->GetElement("rv:controller")->GetElement("rv:genome");
42-
const std::string genome_type = genome_sdf->GetAttribute("type")->GetAsString();
43+
try
44+
{
45+
std::cout << "AOSIJDOISADJASJODSA" << std::endl;
46+
const sdf::ElementPtr genome_sdf = brain_sdf->GetElement("rv:controller")->GetElement("rv:genome");
47+
const std::string genome_type = getSdfAttrSafe<std::string>(genome_sdf, "type");
4348
if (genome_type != "CPPN")
4449
{
4550
throw std::runtime_error("unexpected GENOME type");
4651
}
4752
std::string genome_string = genome_sdf->GetValue()->GetAsString();
4853
string_replace(genome_string, "inf", std::to_string(std::numeric_limits<double>::max()));
49-
NEAT::Genome genome = NEAT::Genome();
50-
genome.Deserialize(genome_string);
54+
NEAT::Genome genome = NEAT::Genome();
55+
genome.Deserialize(genome_string);
5156

52-
return genome;
57+
return genome;
58+
}
59+
catch (std::runtime_error const &err)
60+
{
61+
throw std::runtime_error(std::string("Could not load sdf. ") + std::string(err.what()));
62+
}
5363
}

‎cpprevolve/revolve/gazebo/util/SdfUtil.hpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
#include <sdf/Element.hh>
2+
3+
namespace revolve
4+
{
5+
namespace gazebo
6+
{
7+
8+
// Gets an attribute from an sdf element
9+
// Throws std::runtime_error when not present.
10+
// Can do types:
11+
// - 'std::string'
12+
// - 'bool'
13+
// - 'double'
14+
template <typename OfType>
15+
OfType getSdfAttrSafe(sdf::ElementPtr sdf, std::string const &attribute)
16+
{
17+
static_assert(
18+
std::is_same<OfType, std::string>::value ||
19+
std::is_same<OfType, double>::value ||
20+
std::is_same<OfType, bool>::value,
21+
"Type not supported");
22+
if (!sdf->HasAttribute(attribute) || !sdf->GetAttribute(attribute)->IsType<OfType>())
23+
{
24+
throw std::runtime_error(std::string("Could not get attribute: ") + attribute);
25+
}
26+
else
27+
{
28+
if constexpr (std::is_same<OfType, std::string>::value)
29+
{
30+
return sdf->GetAttribute(attribute)->GetAsString();
31+
}
32+
else
33+
{
34+
OfType buffer;
35+
return sdf->GetAttribute(attribute)->Get<OfType>(buffer);
36+
}
37+
}
38+
}
39+
40+
}
41+
}

‎experiments/examples/manager_pop.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@ async def run():
3030

3131
# experiment params #
3232
num_generations = 100
33-
population_size = 100
34-
offspring_size = 50
33+
population_size = 10
34+
offspring_size = 5
3535

3636
genotype_conf: DirectTreeGenotypeConfig = DirectTreeGenotypeConfig(
3737
max_parts=50,

0 commit comments

Comments
 (0)