Newer Battery

cpprevolve/revolve/gazebo/CMakeLists.txt

@@ -169,6 +169,7 @@ set_source_files_properties(
 # Plugin C++ files
 file(GLOB_RECURSE
         REVOLVE_GZ_SRC
+        battery/*.cpp
         brains/*.cpp
         motors/*.cpp
         sensors/*.cpp

cpprevolve/revolve/gazebo/battery/Battery.cpp

@@ -0,0 +1,31 @@
+//
+// Created by Roy Basmacier on 2019-07-09.
+//
+
+#include "Battery.h"
+
+using namespace revolve::gazebo;
+
+Battery::Battery(double initial_charge)
+    : initial_charge(initial_charge), current_charge(initial_charge), time_init(std::to_string(time(0))), robot_name("")
+    {}
+
+void Battery::Update(double global_time, double delta_time)
+{
+    double sum = 0.0;
+    //    std::cout << "battery: " << this->Voltage() << "V" << std::endl;
+    for (const auto &consumer: this->PowerLoads()) {
+//            std::cout << "comsumer: " << consumer.first << " -> " << consumer.second << std::endl;
+        sum += consumer.second; // TODO add constant so its linear
+    }
+    this->current_charge += sum * delta_time; // charge is measured in joules
+
+    //TODO properly save battery data somewhere
+    /*
+    std::ofstream b_info_file;
+    b_info_file.open("output/cpg_bo/" + this->robot_name + "/" + this->time_init + "/battery.txt", std::ios_base::app);
+    if (b_info_file.fail())
+        std::cout << "Failed to open: " << b_info_file.fail() <<  " " << "output/cpg_bo/" + this->robot_name + "/" + this->time_init + "/battery.txt" << std::endl;
+    b_info_file << global_time << " " << sum << " " << current_charge << std::endl;
+    */
+}

cpprevolve/revolve/gazebo/battery/Battery.h

@@ -0,0 +1,46 @@
+//
+// Created by Roy Basmacier on 2019-07-09.
+//
+
+#ifndef REVOLVE_BATTERY_H
+#define REVOLVE_BATTERY_H
+
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/msgs/msgs.hh>
+
+#include <revolve/gazebo/Types.h>
+
+namespace revolve{
+namespace gazebo{
+
+class Battery : public ::gazebo::common::Battery
+{
+public:
+    explicit Battery(double initial_charge);
+
+    void Update(double global_time, double delta_time);
+
+protected:
+    /// \brief initial charge of the battery in joules
+    double initial_charge; // it is set in RobotController.cpp
+
+    /// \brief current charge of the battery in joules
+    double current_charge;
+
+    /// \brief the time of initiation (for creating data files of battery delete later)
+    std::string time_init;
+
+    std::string robot_name;
+
+    friend class RobotController;
+    friend class Evaluator;
+    friend class DifferentialCPG;
+};
+
+}
+}
+
+#endif //REVOLVE_BATTERY_H

cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp

@@ -74,10 +74,13 @@ DifferentialCPG::DifferentialCPG(
     const ::gazebo::physics::ModelPtr &_model,
     const sdf::ElementPtr robot_config,
     const std::vector< revolve::gazebo::MotorPtr > &_motors,
-    const std::vector< revolve::gazebo::SensorPtr > &_sensors)
+    const std::vector< revolve::gazebo::SensorPtr > &_sensors,
+    std::shared_ptr<::revolve::gazebo::Battery> battery
+    )
     : next_state(nullptr)
     , input(new double[_sensors.size()])
     , output(new double[_motors.size()])
+    , battery_(battery)
 {
 
   this->learner = robot_config->GetElement("rv:brain")->GetElement("rv:learner");
@@ -280,12 +283,15 @@ DifferentialCPG::DifferentialCPG(
   }
 
   // Create directory for output.
-  this->directory_name = controller->GetAttribute("output_directory")->GetAsString();
+  // this->directory_name = controller->GetAttribute("output_directory")->GetAsString();
   if(this->directory_name.empty())
   {
-    this->directory_name = "output/cpg_bo/";
-    this->directory_name += std::to_string(time(0)) + "/";
+      std::cout << "§yes§";
+    this->directory_name = "output/cpg_bo/" + this->robot->GetName() + "/"; // CHANGETHIS
+    this->directory_name += this->battery_->time_init + "/";
   }
+  else
+      std::cout << "§no§\n";
 
   std::system(("mkdir -p " + this->directory_name).c_str());
 
@@ -353,7 +359,7 @@ DifferentialCPG::DifferentialCPG(
   }
 
   // Initiate the cpp Evaluator
-  this->evaluator.reset(new Evaluator(this->evaluation_rate));
+  this->evaluator.reset(new Evaluator(battery, this->evaluation_rate));
   this->evaluator->directory_name = this->directory_name;
 }
 
@@ -814,7 +820,7 @@ void DifferentialCPG::Update(
       {
         std::cout << std::endl << "I am finished " << std::endl;
       }
-      std::exit(0);
+      // std::exit(0);
     }
 
     // Evaluation policy here

cpprevolve/revolve/gazebo/brains/DifferentialCPG.h

@@ -68,7 +68,8 @@ namespace revolve
           const ::gazebo::physics::ModelPtr &_model,
           const sdf::ElementPtr robot_config,
           const std::vector< MotorPtr > &_motors,
-          const std::vector< SensorPtr > &_sensors);
+          const std::vector< SensorPtr > &_sensors,
+          std::shared_ptr<::revolve::gazebo::Battery> battery);
 
       public: void set_ode_matrix();
 
@@ -259,6 +260,9 @@ namespace revolve
       /// \brief Use frame of reference {-1,0,1} version or not
       private: bool use_frame_of_reference;
 
+      /// \brief shared pointer to battery
+      protected: std::shared_ptr<::revolve::gazebo::Battery> battery_;
+
         // BO Learner parameters
     private: double kernel_noise_;
     private: bool kernel_optimize_noise_;