Merge pull request #35 from ci-group/revolve_bot

RevolveBot

Dockerfile

@@ -15,7 +15,8 @@ RUN apt-get install -y build-essential      \
                        python               \
                        python3-pip          \
                        libyaml-cpp-dev      \
-                       xsltproc
+                       xsltproc             \
+                       libcairo2-dev
 RUN apt-get install -y libgazebo9-dev gazebo9
 RUN apt-get clean && rm -rf /var/lib/apt/lists/*
 

cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp

@@ -86,18 +86,22 @@ NeuralNetwork::NeuralNetwork(
   // motors, creating the adequate neurons in place as we do so.
 
   // Map of ID to neuron element
+  // neuron.id ---> sdf_element
   std::map< std::string, sdf::ElementPtr > neuronMap;
+  std::map< std::string, std::vector<sdf::ElementPtr> > neuronPartIdMap;
 
-  // List of all hidden neurons for convenience
+  // List of all hidden neurons (ids) for convenience
   std::vector< std::string > hiddenNeurons;
 
-  // Set for tracking all collected input/output neurons
+  // Set for tracking all collected input/output neurons (ids)
   std::set< std::string > toProcess;
 
+  auto controller_settings = _settings->GetElement("rv:controller");
+
   // Fetch the first neuron; note the HasElement call is necessary to
   // prevent SDF from complaining if no neurons are present.
-  auto neuron = _settings->HasElement("rv:neuron")
-                ? _settings->GetElement("rv:neuron")
+  auto neuron = controller_settings->HasElement("rv:neuron")
+                ? controller_settings->GetElement("rv:neuron")
                 : sdf::ElementPtr();
   while (neuron)
   {
@@ -109,8 +113,9 @@ NeuralNetwork::NeuralNetwork(
     }
     auto layer = neuron->GetAttribute("layer")->GetAsString();
     auto neuronId = neuron->GetAttribute("id")->GetAsString();
+    auto neuronPartId = neuron->GetAttribute("part_id")->GetAsString();
 
-    if (this->layerMap_.count(neuronId))
+    if (this->layerMap_.count(neuronId) == 1)
     {
       std::cerr << "Duplicate neuron ID '" << neuronId << "'" << std::endl;
       throw std::runtime_error("Robot brain error");
@@ -119,6 +124,12 @@ NeuralNetwork::NeuralNetwork(
     this->layerMap_[neuronId] = layer;
     neuronMap[neuronId] = neuron;
 
+    if (neuronPartIdMap.find(neuronPartId) == neuronPartIdMap.end()) {
+        neuronPartIdMap[neuronPartId] = std::vector<sdf::ElementPtr>();
+    }
+    neuronPartIdMap[neuronPartId].push_back(neuron);
+
+    // INPUT LAYER
     if ("input" == layer)
     {
       if (this->nInputs_ >= MAX_INPUT_NEURONS)
@@ -132,6 +143,7 @@ NeuralNetwork::NeuralNetwork(
       toProcess.insert(neuronId);
       ++(this->nInputs_);
     }
+    // OUTPUT LAYER
     else if ("output" == layer)
     {
       if (this->nOutputs_ >= MAX_OUTPUT_NEURONS)
@@ -145,6 +157,7 @@ NeuralNetwork::NeuralNetwork(
       toProcess.insert(neuronId);
       ++(this->nOutputs_);
     }
+    // HIDDEN LAYER
     else if ("hidden" == layer)
     {
       if (hiddenNeurons.size() >= MAX_HIDDEN_NEURONS)
@@ -157,7 +170,7 @@ NeuralNetwork::NeuralNetwork(
       }
 
       hiddenNeurons.push_back(neuronId);
-     ++(this->nHidden_);
+      ++(this->nHidden_);
     }
     else
     {
@@ -171,70 +184,80 @@ NeuralNetwork::NeuralNetwork(
   // Create motor output neurons at the correct position
   // We iterate a part's motors and just assign every
   // neuron we find in order.
-  std::map< std::string, unsigned int > outputCountMap;
   unsigned int outputsIndex = 0;
   for (const auto &motor : _motors)
   {
-    auto partId = motor->PartId();
-    if (not outputCountMap.count(partId))
+    std::string partId = motor->PartId();
+    auto details = neuronPartIdMap.find(partId);
+    if (details == neuronPartIdMap.end())
     {
-      outputCountMap[partId] = 0;
+      std::cerr << "Required output neuron " << partId
+                << " for motor could not be located" << std::endl;
+      throw std::runtime_error("Robot brain error");
     }
 
+    const auto &neuron_list = details->second;
+    auto neuron_iter = neuron_list.cbegin();
+
     for (unsigned int i = 0, l = motor->Outputs(); i < l; ++i)
     {
-      std::stringstream neuronId;
-      neuronId << partId << "-out-" << outputCountMap[partId];
-      ++outputCountMap[partId];
-
-      auto details = neuronMap.find(neuronId.str());
-      if (details == neuronMap.end())
-      {
-        std::cerr << "Required output neuron " << neuronId.str()
-                  << " for motor could not be located" << std::endl;
-        throw std::runtime_error("Robot brain error");
-      }
+        while (not ((*neuron_iter)->GetAttribute("layer")->GetAsString() == "output"))
+        {
+            ++neuron_iter;
+            if (neuron_iter == neuron_list.cend())
+            {
+                std::cerr << "Required input neuron " << partId
+                          << " for sensor could not be located" << std::endl;
+                throw std::runtime_error("Robot brain error");
+            }
+        }
+      std::string neuronId = (*neuron_iter)->GetAttribute("id")->GetAsString();
 
       neuronHelper(&this->params_[outputsIndex * MAX_NEURON_PARAMS],
                    &this->types_[outputsIndex],
-                   details->second);
-      this->positionMap_[neuronId.str()] = outputsIndex;
-      toProcess.erase(neuronId.str());
+                   *neuron_iter);
+      this->positionMap_[neuronId] = outputsIndex;
+      toProcess.erase(neuronId);
       ++outputsIndex;
+      ++neuron_iter;
     }
   }
 
   // Create sensor input neurons
-  std::map< std::string, unsigned int > inputCountMap;
   unsigned int inputsIndex = 0;
   for (const auto &sensor : _sensors)
   {
     auto partId = sensor->PartId();
-
-    if (not inputCountMap.count(partId))
+    auto details = neuronPartIdMap.find(partId);
+    if (details == neuronPartIdMap.end())
     {
-      inputCountMap[partId] = 0;
+      std::cerr << "Required input neuron list " << partId
+                << " for sensor could not be located" << std::endl;
+      throw std::runtime_error("Robot brain error");
     }
+    const auto &neuron_list = details->second;
+    auto neuron_iter = neuron_list.cbegin();
 
     for (unsigned int i = 0, l = sensor->Inputs(); i < l; ++i)
     {
-      std::stringstream neuronId;
-      neuronId << partId << "-in-" << inputCountMap[partId];
-      ++inputCountMap[partId];
-
-      auto details = neuronMap.find(neuronId.str());
-      if (details == neuronMap.end())
+      while (not ((*neuron_iter)->GetAttribute("layer")->GetAsString() == "input"))
       {
-        std::cerr << "Required input neuron " << neuronId.str()
-                  << " for sensor could not be located" << std::endl;
-        throw std::runtime_error("Robot brain error");
+          ++neuron_iter;
+          if (neuron_iter == neuron_list.cend())
+          {
+              std::cerr << "Required input neuron " << partId
+                        << " for sensor could not be located" << std::endl;
+              throw std::runtime_error("Robot brain error");
+          }
       }
+      std::string neuronId = (*neuron_iter)->GetAttribute("id")->GetAsString();
 
       // Input neurons can currently not have a type, so
       // there is no need to process it.
-      this->positionMap_[neuronId.str()] = inputsIndex;
-      toProcess.erase(neuronId.str());
+      this->positionMap_[neuronId] = inputsIndex;
+      toProcess.erase(neuronId);
       ++inputsIndex;
+      ++neuron_iter;
     }
   }
 

cpprevolve/revolve/gazebo/brains/RLPower.cpp

@@ -24,6 +24,7 @@
 #include <random>
 #include <string>
 #include <vector>
+#include <algorithm>
 
 #include <gsl/gsl_spline.h>
 
@@ -64,7 +65,10 @@ RLPower::RLPower(
   this->tau_ = 0.2;
   this->sourceYSize_ = 3;
 
-  this->stepRate_ = this->numInterpolationPoints_ / this->sourceYSize_;
+  this->stepRate_ = std::max(
+          static_cast<size_t>(1),
+          this->numInterpolationPoints_ / this->sourceYSize_);
+
 
   // Generate first random policy
   auto numMotors = _motors.size();
@@ -404,7 +408,9 @@ void RLPower::IncreaseSplinePoints(const size_t _numSplines)
   this->sourceYSize_++;
 
   // LOG code
-  this->stepRate_ = this->numInterpolationPoints_ / this->sourceYSize_;
+  this->stepRate_ = std::max(
+          static_cast<size_t>(1),
+          this->numInterpolationPoints_ / this->sourceYSize_);
 
   // Copy current policy for resizing
   Policy policy_copy(this->currentPolicy_->size());

cpprevolve/revolve/gazebo/motors/MotorFactory.cpp

@@ -27,8 +27,8 @@ namespace gz = gazebo;
 using namespace revolve::gazebo;
 
 /////////////////////////////////////////////////
-MotorFactory::MotorFactory(::gazebo::physics::ModelPtr _model)
-    : model_(_model)
+MotorFactory::MotorFactory(::gazebo::physics::ModelPtr model)
+    : model_(std::move(model))
 {
 }
 
@@ -39,17 +39,17 @@ MotorFactory::~MotorFactory() = default;
 MotorPtr MotorFactory::Motor(
     sdf::ElementPtr _motorSdf,
     const std::string &_type,
-    const std::string &_motorId,
-    const std::string &_partId)
+    const std::string &_partId,
+    const std::string &_motorId)
 {
   MotorPtr motor;
   if ("position" == _type)
   {
-    motor.reset(new PositionMotor(model_, _motorId, _partId, _motorSdf));
+    motor.reset(new PositionMotor(this->model_, _partId, _motorId, _motorSdf));
   }
   else if ("velocity" == _type)
   {
-    motor.reset(new VelocityMotor(model_, _motorId, _partId, _motorSdf));
+    motor.reset(new VelocityMotor(this->model_, _partId, _motorId, _motorSdf));
   }
 
   return motor;
@@ -60,6 +60,7 @@ MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf)
 {
   auto typeParam = _motorSdf->GetAttribute("type");
   auto partIdParam = _motorSdf->GetAttribute("part_id");
+//  auto partNameParam = _motorSdf->GetAttribute("part_name");
   auto idParam = _motorSdf->GetAttribute("id");
 
   if (not typeParam or not partIdParam or not idParam)
@@ -69,6 +70,7 @@ MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf)
     throw std::runtime_error("Motor error");
   }
 
+//  auto partName = partNameParam->GetAsString();
   auto partId = partIdParam->GetAsString();
   auto type = typeParam->GetAsString();
   auto id = idParam->GetAsString();

cpprevolve/revolve/gazebo/motors/MotorFactory.h

@@ -38,7 +38,7 @@ namespace revolve
       /// \brief[in] _partId Module identifier
       /// \brief[in] _motorId Motor identifier
       /// \brief[in] _outputs Number of motor outputs
-      public: explicit MotorFactory(::gazebo::physics::ModelPtr _model);
+      public: explicit MotorFactory(::gazebo::physics::ModelPtr model);
 
       /// \brief Destructor
       public: virtual ~MotorFactory();
@@ -48,10 +48,10 @@ namespace revolve
       /// required attributes already checked, usually you should override
       /// this when adding new motor types.
       public: virtual MotorPtr Motor(
-          sdf::ElementPtr _motorSdf,
-          const std::string &_type,
-          const std::string &_motorId,
-          const std::string &_partId);
+        sdf::ElementPtr _motorSdf,
+        const std::string &_type,
+        const std::string &_partId,
+        const std::string &_motorId);
 
       /// \brief Creates a motor for the given model for the given SDF element.
       public: virtual MotorPtr Create(sdf::ElementPtr _motorSdf);

cpprevolve/revolve/gazebo/motors/PositionMotor.cpp

@@ -30,8 +30,8 @@ PositionMotor::PositionMotor(
     gz::physics::ModelPtr _model,
     const std::string &_partId,
     const std::string &_motorId,
-    sdf::ElementPtr _motor)
-    : JointMotor(_model, _partId, _motorId, _motor, 1)
+    const sdf::ElementPtr _motor)
+    : JointMotor(std::move(_model), _partId, _motorId, _motor, 1)
     , positionTarget_(0)
     , noise_(0)
 {

cpprevolve/revolve/gazebo/motors/PositionMotor.h

@@ -44,15 +44,15 @@ namespace revolve
           ::gazebo::physics::ModelPtr _model,
           const std::string &_partId,
           const std::string &_motorId,
-          sdf::ElementPtr _motor);
+          const sdf::ElementPtr _motor);
 
       /// \brief Destructor
-      public: virtual ~PositionMotor();
+      public: virtual ~PositionMotor() override;
 
       /// \brief
       public: virtual void Update(
           double *_outputs,
-          double _step);
+          double _step) override;
 
       /// \brief World update event function
 //      protected: void OnUpdate(const ::gazebo::common::UpdateInfo info);